iCub-main
Loading...
Searching...
No Matches
SmithPredictor.cpp
Go to the documentation of this file.
1/*
2 * Copyright (C) 2012 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia
3 * Author: Ugo Pattacini
4 * email: ugo.pattacini@iit.it
5 * website: www.robotcub.org
6 * Permission is granted to copy, distribute, and/or modify this program
7 * under the terms of the GNU General Public License, version 2 or any
8 * later version published by the Free Software Foundation.
9 *
10 * A copy of the license can be found at
11 * http://www.robotcub.org/icub/license/gpl.txt
12 *
13 * This program is distributed in the hope that it will be useful, but
14 * WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16 * Public License for more details
17*/
18
19#include <string>
20#include <sstream>
21#include <cmath>
22
23#include <yarp/os/Bottle.h>
24#include <yarp/math/Math.h>
25
26#include "SmithPredictor.h"
27
28using namespace std;
29using namespace yarp::os;
30using namespace yarp::sig;
31using namespace yarp::math;
32using namespace iCub::ctrl;
33using namespace iCub::iKin;
34
35
36/************************************************************************/
38{
39 I=NULL;
40 enabled=false;
41}
42
43
44/************************************************************************/
49
50
51/************************************************************************/
53{
54 if (I!=NULL)
55 delete I;
56
57 for (size_t i=0; i<F.size(); i++)
58 delete F[i];
59
60 for (size_t i=0; i<tappedDelays.size(); i++)
61 delete tappedDelays[i];
62
63 F.clear();
64 tappedDelays.clear();
65}
66
67
68/************************************************************************/
69void SmithPredictor::configure(const Property &options, iKinChain &chain)
70{
71 enabled=options.check("smith_predictor",Value("off")).asString()=="on";
72 if (!enabled)
73 return;
74
75 dealloc();
76
77 // default values
78 Vector Kp(chain.getDOF(),1.0);
79 Vector Tz(chain.getDOF(),0.0);
80 Vector Tw(chain.getDOF(),0.0);
81 Vector Zeta(chain.getDOF(),0.0);
82 for (unsigned int i=0; i<chain.getDOF(); i++)
83 tappedDelays.push_back(new deque<double>);
84
85 double Ts=options.check("Ts",Value(0.01)).asFloat64();
86 Vector y0(chain.getDOF());
87 Matrix lim(chain.getDOF(),2);
88
89 // we're forced to cycle by joints and
90 // not by dofs since the configuration
91 // parameters are given with joints ordering
92 int i=0;
93 for (unsigned int j=0; j<chain.getN(); j++)
94 {
95 if (!chain[j].isBlocked())
96 {
97 y0[i]=chain[j].getAng();
98 lim(i,0)=chain[j].getMin();
99 lim(i,1)=chain[j].getMax();
100
101 ostringstream entry;
102 entry<<"joint_"<<j;
103 string entry_str(entry.str());
104 if (options.check(entry_str))
105 {
106 if (Bottle *params=options.find(entry_str).asList())
107 {
108 if (params->check("Kp"))
109 Kp[i]=params->find("Kp").asFloat64();
110
111 if (params->check("Tz"))
112 Tz[i]=params->find("Tz").asFloat64();
113
114 if (params->check("Tw"))
115 Tw[i]=params->find("Tw").asFloat64();
116
117 if (params->check("Zeta"))
118 Zeta[i]=params->find("Zeta").asFloat64();
119
120 if (params->check("Td"))
121 {
122 int depth=(int)ceil(params->find("Td").asFloat64()/Ts);
123 tappedDelays[i]->assign(depth,y0[i]);
124 }
125 }
126 }
127
128 i++;
129 }
130 }
131
132 // create integrator
133 I=new Integrator(Ts,y0,lim);
134
135 // account for possible internal saturation
136 Vector _y0=I->get();
137
138 // create filters
139 Vector num(3);
140 Vector den(3);
141 Vector y01(1);
142 double Ts2=Ts*Ts;
143 double twoTs2=2.0*Ts2;
144 for (unsigned int i=0; i<chain.getDOF(); i++)
145 {
146 // implementing F(s)=Kp*(1+Tz*s)/(1+2*Zeta*Tw*s+(Tw*s)^2)
147 double _num_0=2.0*Tz[i]*Ts;
148 num[0]=Kp[i] * (Ts2 + _num_0);
149 num[1]=Kp[i] * twoTs2;
150 num[2]=Kp[i] * (Ts2 - _num_0);
151
152 double _den_0=4.0*Tw[i]*Tw[i];
153 double _den_1=2.0*_den_0;
154 double _den_2=4.0*Zeta[i]*Ts*Tw[i];
155 den[0]=Ts2 + _den_2 + _den_0;
156 den[1]=twoTs2 - _den_1;
157 den[2]=Ts2 - _den_2 + _den_0;
158
159 y01[0]=_y0[i];
160 F.push_back(new Filter(num,den,y01));
161 }
162}
163
164
165/************************************************************************/
166void SmithPredictor::restart(const Vector &y0)
167{
168 if (enabled && (tappedDelays.size()==y0.length()))
169 {
170 // init the content of tapped delay lines
171 for (size_t i=0; i<tappedDelays.size(); i++)
172 for (size_t j=0; j<tappedDelays[i]->size(); j++)
173 tappedDelays[i]->at(j)=y0[i];
174
175 // init the integral part
176 I->reset(y0);
177
178 // account for possible internal saturation
179 Vector _y0=I->get();
180
181 // init the filters
182 Vector y01(1);
183 for (size_t i=0; i<F.size(); i++)
184 {
185 y01[0]=_y0[i];
186 F[i]->init(y01);
187 }
188 }
189}
190
191
192/************************************************************************/
193Vector SmithPredictor::computeCmd(const Vector &u)
194{
195 if (enabled && (tappedDelays.size()==u.length()))
196 {
197 Vector _u(F.size());
198 for (size_t i=0; i<F.size(); i++)
199 {
200 Vector u1(1); u1[0]=u[i];
201 Vector _y=F[i]->filt(u1);
202 _u[i]=_y[i];
203 }
204
205 Vector y=I->integrate(_u);
206 Vector out(y.length());
207 for (size_t i=0; i<out.length(); i++)
208 {
209 tappedDelays[i]->push_back(y[i]);
210 out[i]=y[i]-tappedDelays[i]->front();
211 tappedDelays[i]->pop_front();
212 }
213
214 return out;
215 }
216 else
217 return zeros((int)u.length());
218}
219
220
std::deque< std::deque< double > * > tappedDelays
void configure(const yarp::os::Property &options, iCub::iKin::iKinChain &chain)
yarp::sig::Vector computeCmd(const yarp::sig::Vector &u)
std::deque< iCub::ctrl::Filter * > F
iCub::ctrl::Integrator * I
void restart(const yarp::sig::Vector &y0)
IIR and FIR.
Definition filters.h:77
A class for defining a saturated integrator based on Tustin formula: .
Definition pids.h:48
const yarp::sig::Vector & get() const
Returns the current output vector.
Definition pids.h:154
const yarp::sig::Vector & integrate(const yarp::sig::Vector &x)
Executes one-step integration of input vector.
Definition pids.cpp:115
void reset(const yarp::sig::Vector &y0)
Resets the internal state and sets the output vector to the given value.
Definition pids.cpp:128
A Base class for defining a Serial Link Chain.
Definition iKinFwd.h:354
yarp::sig::Vector getAng()
Returns the current free joint angles values.
Definition iKinFwd.cpp:611
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition iKinFwd.h:549
unsigned int getDOF() const
Returns the current number of Chain's DOF.
Definition iKinFwd.h:556
zeros(2, 2) eye(2
out
Definition sine.m:8