iCub-main
minJerkCtrl.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
3  * Copyright (C) 2006-2010 RobotCub Consortium
5  *
6  * This software may be modified and distributed under the terms
7  * of the BSD-3-Clause license. See the accompanying LICENSE file for
8  * details.
9 */
10
11 #include <sstream>
12 #include <cmath>
13
14 #include <yarp/os/Time.h>
15 #include <yarp/math/Math.h>
16 #include <iCub/ctrl/minJerkCtrl.h>
17
18 using namespace std;
19 using namespace yarp::os;
20 using namespace yarp::sig;
21 using namespace yarp::math;
22 using namespace iCub::ctrl;
23
24
25 /*******************************************************************************************/
26 minJerkVelCtrlForIdealPlant::minJerkVelCtrlForIdealPlant(const double _Ts, const int _dim) :
27  Ts(_Ts), dim(_dim), T(1.0), F(NULL)
28 {
29  computeCoeffs();
30 }
31
32
33 /*******************************************************************************************/
35 {
36  double T2=T*T;
37  double T3=T2*T;
38  double twoOnTs=2.0/Ts;
39
40  // 90% of steady-state value in t=T
41  // transient extinguished for t>=1.5*T
42  double a=-150.765868956161/T3;
43  double b=-84.9812819469538/T2;
44  double c=-15.9669610709384/T;
45
46  // implementing F(s)=-a/(s^2-c*s-b)
47  Vector num(3);
48  Vector den(3);
49
50  double c1=twoOnTs*(twoOnTs-c)-b;
51  double c2=-a/c1;
52
53  num[0]=c2;
54  num[1]=2.0*c2;
55  num[2]=c2;
56
57  den[0]=1.0;
58  den[1]=-2.0*(twoOnTs*twoOnTs+b)/c1;
59  den[2]=(twoOnTs*(twoOnTs+c)-b)/c1;
60
61  if (F==NULL)
62  {
63  Vector e0(dim); e0=0.0;
64  F=new Filter(num,den,e0);
65  }
66  else
68 }
69
70
71 /*******************************************************************************************/
72 Vector minJerkVelCtrlForIdealPlant::computeCmd(const double _T, const Vector &e)
73 {
74  if (T!=_T)
75  {
76  T=_T;
77  computeCoeffs();
78  }
79
80  return F->filt(e);
81 }
82
83
84 /*******************************************************************************************/
85 void minJerkVelCtrlForIdealPlant::reset(const Vector &u0)
86 {
87  F->init(u0);
88 }
89
90
91 /*******************************************************************************************/
93 {
94  delete F;
95 }
96
97
98 /*******************************************************************************************/
99 minJerkVelCtrlForNonIdealPlant::minJerkVelCtrlForNonIdealPlant(const double _Ts, const int _dim) :
100  Ts(_Ts), dim(_dim), T(1.0)
101 {
102  Kp.resize(dim,1.0);
103  Tz.resize(dim,0.0);
104  Tw.resize(dim,0.0);
105  Zeta.resize(dim,0.0);
106
107  for (int i=0; i<dim; i++)
108  F.push_back(NULL);
109
110  computeCoeffs();
111 }
112
113
114 /*******************************************************************************************/
116 {
117  Vector num(4);
118  Vector den(4);
119
120  double T2=T*T;
121  double T3=T2*T;
122
123  // 90% of steady-state value in t=T
124  // transient extinguished for t>=1.5*T
125  double a=-150.765868956161/T3;
126  double b=-84.9812819469538/T2;
127  double c=-15.9669610709384/T;
128
129  double Ts2=Ts*Ts;
130  double Ts3=Ts2*Ts;
131  double _num_0=3.0*Ts3;
132  double _den_0=4.0*Ts;
133  double _den_1=2.0*Ts2;
134  double _den_2=_den_1*c;
135  double _den_3=Ts3*b;
136  double _den_4=3.0*_den_3;
137
138  for (int i=0; i<dim; i++)
139  {
140  // implementing F(s)=-a/(s^2-c*s-b) * ((1/Kp)*(1+2*Zeta*Tw*s+(Tw*s)^2)/(1+Tz*s))
141  double _num_1=4.0*Zeta[i]*Ts2*Tw[i];
142  double _num_2=4.0*Ts*Tw[i]*Tw[i];
143  num[0]=a * (Ts3 + _num_1 + _num_2);
144  num[1]=a * (_num_0 + _num_1 - _num_2);
145  num[2]=a * (_num_0 - _num_1 - _num_2);
146  num[3]=a * (Ts3 - _num_1 + _num_2);
147
148  double _den_5=_den_1*Tz[i]*b;
149  double _den_6=_den_0*Tz[i]*c;
150  double _den_7=8.0*Tz[i];
151  double _den_8=3.0*_den_7;
152  den[0]=Kp[i] * (_den_3 - _den_7 - _den_0 + _den_2 + _den_5 + _den_6);
153  den[1]=Kp[i] * (_den_0 + _den_8 + _den_4 + _den_2 + _den_5 - _den_6);
154  den[2]=Kp[i] * (_den_0 - _den_8 + _den_4 - _den_2 - _den_5 - _den_6);
155  den[3]=Kp[i] * (_den_7 - _den_0 + _den_3 - _den_2 - _den_5 + _den_6);
156
157  if (F[i]==NULL)
158  {
159  Vector e0(1); e0=0.0;
160  F[i]=new Filter(num,den,e0);
161  }
162  else
164  }
165 }
166
167
168 /*******************************************************************************************/
169 Vector minJerkVelCtrlForNonIdealPlant::computeCmd(const double _T, const Vector &e)
170 {
171  if (T!=_T)
172  {
173  T=_T;
174  computeCoeffs();
175  }
176
177  Vector y(dim), _e(1);
178  for (int i=0; i<dim; i++)
179  {
180  _e[0]=e[i];
181  Vector _y=F[i]->filt(_e);
182  y[i]=_y[0];
183  }
184
185  return y;
186 }
187
188
189 /*******************************************************************************************/
191 {
192  Vector _u0(1);
193  for (int i=0; i<dim; i++)
194  {
195  _u0[0]=u0[i];
196  F[i]->init(_u0);
197  }
198 }
199
200
201 /*******************************************************************************************/
203  const string &entryTag,
204  const Bottle &ordering)
205 {
206  // default values
207  Kp.resize(dim,1.0);
208  Tz.resize(dim,0.0);
209  Tw.resize(dim,0.0);
210  Zeta.resize(dim,0.0);
211
212  int len=ordering.size()==0?dim:(int)ordering.size();
213  for (int i=0; i<len; i++)
214  {
215  ostringstream entry;
216  entry<<entryTag<<"_"<<(ordering.size()==0?i:ordering.get(i).asInt32());
217  if (parameters.check(entry.str()))
218  {
219  if (Bottle *options=parameters.find(entry.str()).asList())
220  {
221  if (options->check("Kp"))
222  Kp[i]=options->find("Kp").asFloat64();
223
224  if (options->check("Tz"))
225  Tz[i]=options->find("Tz").asFloat64();
226
227  if (options->check("Tw"))
228  Tw[i]=options->find("Tw").asFloat64();
229
230  if (options->check("Zeta"))
231  Zeta[i]=options->find("Zeta").asFloat64();
232  }
233  }
234  }
235
236  computeCoeffs();
237 }
238
239
240 /*******************************************************************************************/
242  const string &entryTag)
243 {
244  ostringstream entry;
245  for (int i=0; i<dim; i++)
246  {
247  entry<<"("<<entryTag<<"_"<<i<<" (";
248
249  Property prop;
250  prop.put("Kp",Kp[i]);
251  prop.put("Tz",Tz[i]);
252  prop.put("Tw",Tw[i]);
253  prop.put("Zeta",Zeta[i]);
254
255  entry<<prop.toString()<<")) ";
256  }
257
258  parameters.fromString(entry.str());
259 }
260
261
262 /*******************************************************************************************/
264 {
265  for (size_t i=0; i<F.size(); i++)
266  delete F[i];
267
268  F.clear();
269 }
270
271
272 /*******************************************************************************************/
273 minJerkBaseGen::minJerkBaseGen(const unsigned int _dim, const double _Ts, const double _T)
274  :dim(_dim), Ts(_Ts), T(_T)
275 {
276  posFilter = velFilter = accFilter = NULL;
277  pos = vel = acc = lastRef = zeros(dim);
278 }
279
280
281 /*******************************************************************************************/
282 minJerkBaseGen::minJerkBaseGen(const Vector &y0, const double _Ts, const double _T)
283  :dim((unsigned int)y0.size()), Ts(_Ts), T(_T)
284 {
285  posFilter = velFilter = accFilter = NULL;
286  lastRef = pos = y0;
287  vel = acc = zeros(dim);
288 }
289
290
291 /*******************************************************************************************/
293 {
294  posFilter = velFilter = accFilter = NULL;
295  pos = z.pos;
296  vel = z.vel;
297  acc = z.acc;
298  lastRef = z.lastRef;
299  T = z.T;
300  Ts = z.Ts;
301  dim = z.dim;
302 }
303
304
305 /*******************************************************************************************/
307 {
308  delete posFilter;
309  delete velFilter;
310  delete accFilter;
311 }
312
313
314 /*******************************************************************************************/
316 {
317  delete posFilter;
318  delete velFilter;
319  delete accFilter;
320  posFilter = velFilter = accFilter = NULL;
321
322  pos = z.pos;
323  vel = z.vel;
324  acc = z.acc;
325  lastRef = z.lastRef;
326  T = z.T;
327  Ts = z.Ts;
328  dim = z.dim;
329
330  return *this;
331 }
332
333
334 /*******************************************************************************************/
335 void minJerkBaseGen::init(const Vector &y0)
336 {
337  // save initial state y0, so that if setT() or setTs() are called afterwards
338  // the vel and acc filters are initialized with the right value (i.e. y0)
339  lastRef = pos = y0;
340  if (posFilter!=NULL)
341  posFilter->init(y0);
342
343  if (velFilter!=NULL)
344  velFilter->init(zeros(dim), y0);
345
346  if (accFilter!=NULL)
347  accFilter->init(zeros(dim), y0);
348 }
349
350
351 /*******************************************************************************************/
352 bool minJerkBaseGen::setT(const double _T)
353 {
354  if(_T<=0.0)
355  return false;
356  T = _T;
357  computeCoeffs();
358  return true;
359 }
360
361
362 /*******************************************************************************************/
363 bool minJerkBaseGen::setTs(const double _Ts)
364 {
365  if(_Ts<=0.0)
366  return false;
367  Ts = _Ts;
368  computeCoeffs();
369  return true;
370 }
371
372
373 /*******************************************************************************************/
374 minJerkTrajGen::minJerkTrajGen(const unsigned int _dim, const double _Ts, const double _T)
375  :minJerkBaseGen(_dim,_Ts,_T)
376 {
377  computeCoeffs();
378 }
379
380
381 /*******************************************************************************************/
382 minJerkTrajGen::minJerkTrajGen(const Vector &y0, const double _Ts, const double _T)
383  :minJerkBaseGen(y0,_Ts,_T)
384 {
385  computeCoeffs();
386 }
387
388
389 /*******************************************************************************************/
391  :minJerkBaseGen(z)
392 {
393  computeCoeffs();
394 }
395
396
397 /*******************************************************************************************/
399 {
401  computeCoeffs();
402  return *this;
403 }
404
405
406 /*******************************************************************************************/
408 {
409  // 90% of steady-state value in t=T
410  // transient extinguished for t>=1.5*T
411  double a = -150.765868956161/(T*T*T);
412  double b = -84.9812819469538/(T*T);
413  double c = -15.9669610709384/T;
414
415  // implementing F(s)=-a/(s^3-c*s^2-b*s-a)
416  double m = 4.0*c*Ts;
417  double n = 2.0*b*Ts*Ts;
418  double p = a*Ts*Ts*Ts;
419  Vector num = cat(p, 3.0*p, 3.0*p, p);
420  Vector den = cat(m+n+p-8.0, -m+n+3.0*p+24.0, -m-n+3.0*p-24.0, m-n+p+8.0);
421  if (posFilter==NULL)
422  posFilter=new Filter(num,den,pos);
423  else
425
426  // implementing F(s)=-a*s/(s^3-c*s^2-b*s-a)
427  p = 2.0*a*Ts*Ts;
428  num = cat(p, p, -p, -p);
429  if (velFilter==NULL)
430  velFilter=new Filter(num,den,vel);
431  else
433  velFilter->init(zeros(dim), pos); //init filter to avoid spikes at the start
434
435  // implementing F(s)=-a*s^2/(s^3-c*s^2-b*s-a)
436  p = 4.0*a*Ts;
437  num = cat(p, -p, -p, p);
438  if (accFilter==NULL)
439  accFilter=new Filter(num,den,acc);
440  else
442  accFilter->init(zeros(dim), pos); //init filter to avoid spikes at the start
443 }
444
445
446 /*******************************************************************************************/
447 void minJerkTrajGen::computeNextValues(const Vector &yd)
448 {
449  lastRef = yd;
450
451  if (posFilter!=NULL)
452  pos = posFilter->filt(yd);
453
454  if (velFilter!=NULL)
455  vel = velFilter->filt(yd);
456
457  if (accFilter!=NULL)
458  acc = accFilter->filt(yd);
459 }
460
461
462 /*******************************************************************************************/
463 minJerkRefGen::minJerkRefGen(const unsigned int _dim, const double _Ts, const double _T)
464  :minJerkBaseGen(_dim,_Ts,_T)
465 {
466  computeCoeffs();
467 }
468
469
470 /*******************************************************************************************/
471 minJerkRefGen::minJerkRefGen(const Vector &y0, const double _Ts, const double _T)
472  :minJerkBaseGen(y0,_Ts,_T)
473 {
474  computeCoeffs();
475 }
476
477
478 /*******************************************************************************************/
480  :minJerkBaseGen(z)
481 {
482  computeCoeffs();
483 }
484
485
486 /*******************************************************************************************/
488 {
490  computeCoeffs();
491  return *this;
492 }
493
494
495 /*******************************************************************************************/
497 {
498  // 90% of steady-state value in t=T
499  // transient extinguished for t>=1.5*T
500  double a = -150.765868956161/(T*T*T);
501  double b = -84.9812819469538/(T*T);
502  double c = -15.9669610709384/T;
503
504  // implementing F(s)=-a/(s^3-c*s^2-b*s-a)
505  double m = 4.0*c*Ts;
506  double n = 2.0*b*Ts*Ts;
507  double p = a*Ts*Ts*Ts;
508  Vector num = cat(p, 3.0*p, 3.0*p, p);
509  Vector den = cat(m+n+p-8.0, -m+n+3.0*p+24.0, -m-n+3.0*p-24.0, m-n+p+8.0);
510  if (posFilter==NULL)
511  posFilter=new Filter(num,den,pos);
512  else
514
515  // implementing F(s)=-a/(s^2-c*s-b)
516  double twoOnTs=2.0/Ts;
517  double c1=twoOnTs*(twoOnTs-c)-b;
518  double c2=-a/c1;
519  num = cat(c2, 2.0*c2, c2);
520  den = cat(1.0, -2.0*(twoOnTs*twoOnTs+b)/c1, (twoOnTs*(twoOnTs+c)-b)/c1);
521  if (velFilter==NULL)
522  velFilter = new Filter(num,den,zeros(dim));
523  else
525  velFilter->init(zeros(dim), pos); //init filter to avoid spikes at the start
526
527  // implementing F(s)=-a*s/(s^2-c*s-b)
528  m = 2.0*c*Ts;
529  n = b*Ts*Ts;
530  p = 2.0*a*Ts;
531  num = cat(-p, 0.0, p);
532  den = cat(4.0-m-n, -8.0+m-2.0*n, 4.0-n);
533  if (accFilter==NULL)
534  accFilter=new Filter(num,den,acc);
535  else
537  accFilter->init(zeros(dim), pos); //init filter to avoid spikes at the start
538 }
539
540
541 /*******************************************************************************************/
542 void minJerkRefGen::computeNextValues(const yarp::sig::Vector &y)
543 {
544  if (posFilter!=NULL)
546
547  // rotate pos around lastRef so that it lies along the distance y-lastRef
548  /*double n = yarp::math::norm(y-lastRef);
549  if(n!=0.0)
550  pos = lastRef + yarp::math::norm(pos-lastRef)*(y-lastRef)/n;*/
551
552  if (velFilter!=NULL)
553  vel = velFilter->filt(lastRef-y);
554
555  if (accFilter!=NULL)
556  acc = accFilter->filt(lastRef-y);
557 }
558
559
560 /*******************************************************************************************/
561 void minJerkRefGen::computeNextValues(const yarp::sig::Vector &y, const yarp::sig::Vector &yd)
562 {
563  lastRef = yd;
565 }
566
567
IIR and FIR.
Definition: filters.h:77
virtual const yarp::sig::Vector & filt(const yarp::sig::Vector &u)
Performs filtering on the actual input.
Definition: filters.cpp:140
bool adjustCoeffs(const yarp::sig::Vector &num, const yarp::sig::Vector &den)
Modifies the values of existing filter coefficients without varying their lengths.
Definition: filters.cpp:118
virtual void init(const yarp::sig::Vector &y0)
Internal state reset.
Base class for minimum jerk generators.
Definition: minJerkCtrl.h:228
virtual void computeCoeffs()=0
minJerkBaseGen(const unsigned int _dim, const double _Ts, const double _T)
Constructor.
virtual ~minJerkBaseGen()
Destructor.
minJerkBaseGen & operator=(const minJerkBaseGen &z)
Assignment operator.
bool setT(const double _T)
Set the trajectory reference time (90% of steady-state value in t=_T, transient extinguished for t>=1...
yarp::sig::Vector pos
Definition: minJerkCtrl.h:234
yarp::sig::Vector acc
Definition: minJerkCtrl.h:236
yarp::sig::Vector lastRef
Definition: minJerkCtrl.h:237
bool setTs(const double _Ts)
Set the sample time.
yarp::sig::Vector vel
Definition: minJerkCtrl.h:235
virtual void init(const yarp::sig::Vector &y0)
Initialize the trajectory.
Generator of position, velocity and acceleration references that are approximately minimum jerk.
Definition: minJerkCtrl.h:407
void computeNextValues(const yarp::sig::Vector &y)
Computes the position, velocity and acceleration references.
minJerkRefGen(const unsigned int _dim, const double _Ts, const double _T)
Constructor.
minJerkRefGen & operator=(const minJerkRefGen &z)
Assignment operator.
Generator of approximately minimum jerk trajectories.
Definition: minJerkCtrl.h:352
minJerkTrajGen & operator=(const minJerkTrajGen &z)
Assignment operator.
virtual void computeCoeffs()
void computeNextValues(const yarp::sig::Vector &yd)
Compute the next position, velocity and acceleration.
minJerkTrajGen(const unsigned int _dim, const double _Ts, const double _T)
Constructor.
virtual void reset(const yarp::sig::Vector &u0)
Resets the controller to a given value.
Definition: minJerkCtrl.cpp:85
virtual ~minJerkVelCtrlForIdealPlant()
Destructor.
Definition: minJerkCtrl.cpp:92
virtual yarp::sig::Vector computeCmd(const double _T, const yarp::sig::Vector &e)
Computes the velocity command.
Definition: minJerkCtrl.cpp:72
virtual yarp::sig::Vector computeCmd(const double _T, const yarp::sig::Vector &e)
Computes the velocity command.
virtual ~minJerkVelCtrlForNonIdealPlant()
Destructor.
virtual void reset(const yarp::sig::Vector &u0)
Resets the controller to a given value.
std::deque< ctrl::Filter * > F
Definition: minJerkCtrl.h:146
virtual void setPlantParameters(const yarp::os::Property &parameters, const std::string &entryTag="dimension", const yarp::os::Bottle &ordering=yarp::os::Bottle())
Allows user to assign values to plant parameters.
virtual void getPlantParameters(yarp::os::Property &parameters, const std::string &entryTag="dimension")
Allows user to retrieve plant parameters.
zeros(2, 2) eye(2
int n