iCub-main
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 
28 using namespace std;
29 using namespace yarp::os;
30 using namespace yarp::sig;
31 using namespace yarp::math;
32 using namespace iCub::ctrl;
33 using namespace iCub::iKin;
34 
35 
36 /************************************************************************/
38 {
39  I=NULL;
40  enabled=false;
41 }
42 
43 
44 /************************************************************************/
46 {
47  dealloc();
48 }
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 /************************************************************************/
69 void 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 /************************************************************************/
166 void 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 /************************************************************************/
193 Vector 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 
iCub::ctrl::Integrator
Definition: pids.h:47
SmithPredictor::dealloc
void dealloc()
Definition: SmithPredictor.cpp:52
iCub::iKin::iKinChain
Definition: iKinFwd.h:354
out
out
Definition: sine.m:8
SmithPredictor::SmithPredictor
SmithPredictor()
Definition: SmithPredictor.cpp:37
iCub::iKin::iKinChain::getDOF
unsigned int getDOF() const
Returns the current number of Chain's DOF.
Definition: iKinFwd.h:557
SmithPredictor::~SmithPredictor
~SmithPredictor()
Definition: SmithPredictor.cpp:45
F
F
Definition: compute_ekf_fast.m:22
iCub::iKin::iKinChain::getAng
yarp::sig::Vector getAng()
Returns the current free joint angles values.
Definition: iKinFwd.cpp:619
iCub::iKin
Definition: iKinFwd.h:71
iCub::ctrl::Filter
Definition: filters.h:76
iCub::iKin::iKinChain::getN
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition: iKinFwd.h:550
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
y
y
Definition: show_eyes_axes.m:21
SmithPredictor::restart
void restart(const yarp::sig::Vector &y0)
Definition: SmithPredictor.cpp:166
SmithPredictor::configure
void configure(const yarp::os::Property &options, iCub::iKin::iKinChain &chain)
Definition: SmithPredictor.cpp:69
SmithPredictor::computeCmd
yarp::sig::Vector computeCmd(const yarp::sig::Vector &u)
Definition: SmithPredictor.cpp:193
SmithPredictor.h
zeros
zeros(2, 2) eye(2