iCub-main
pids.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
4  * All rights reserved.
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 <cmath>
12 
13 #include <yarp/os/Log.h>
14 #include <yarp/math/Math.h>
15 #include <iCub/ctrl/math.h>
16 #include <iCub/ctrl/pids.h>
17 
18 #define PID_SAT(x,L,H) ((x)>(H)?(H):((x)<(L)?(L):(x)))
19 
20 using namespace std;
21 using namespace yarp::os;
22 using namespace yarp::sig;
23 using namespace yarp::math;
24 using namespace iCub::ctrl;
25 
26 
27 /************************************************************************/
28 Integrator::Integrator(const double _Ts, const Vector &y0, const Matrix &_lim)
29 {
30  yAssert(_lim.rows()==y0.length());
31  dim=(unsigned int)y0.length();
32  x_old.resize(dim,0.0);
33  applySat=true;
34 
35  Ts =_Ts;
36  lim=_lim;
37  y =saturate(y0);
38 }
39 
40 
41 /************************************************************************/
42 Integrator::Integrator(const double _Ts, const Vector &y0)
43 {
44  dim=(unsigned int)y0.length();
45  x_old.resize(dim,0.0);
46  applySat=false;
47 
48  lim.resize(1,2);
49 
50  Ts=_Ts;
51  y =y0;
52 }
53 
54 
55 /************************************************************************/
56 void Integrator::allocate(const Integrator &I)
57 {
58  y =I.y;
59  x_old =I.x_old;
60  Ts =I.Ts;
61  dim =I.dim;
62  applySat=I.applySat;
63 }
64 
65 
66 /************************************************************************/
67 Vector Integrator::saturate(const Vector &v)
68 {
69  if (applySat)
70  {
71  Vector res=v;
72 
73  for (unsigned int i=0; i<dim; i++)
74  if (res[i]<lim(i,0))
75  res[i]=lim(i,0);
76  else if (res[i]>lim(i,1))
77  res[i]=lim(i,1);
78 
79  return res;
80  }
81  else
82  return v;
83 }
84 
85 
86 /************************************************************************/
87 void Integrator::setSaturation(bool _applySat)
88 {
89  applySat=_applySat;
90  if (applySat)
91  y=saturate(y);
92 }
93 
94 
95 /************************************************************************/
96 void Integrator::setTs(const double _Ts)
97 {
98  if (_Ts>0.0)
99  Ts=_Ts;
100 }
101 
102 
103 /************************************************************************/
104 void Integrator::setLim(const Matrix &_lim)
105 {
106  yAssert(_lim.rows()==dim);
107  lim=_lim;
108 
109  if (applySat)
110  y=saturate(y);
111 }
112 
113 
114 /************************************************************************/
115 const Vector& Integrator::integrate(const Vector &x)
116 {
117  yAssert(x.length()==dim);
118 
119  // implements the Tustin formula
120  y=saturate(y+(x+x_old)*(Ts/2));
121  x_old=x;
122 
123  return y;
124 }
125 
126 
127 /************************************************************************/
128 void Integrator::reset(const Vector &y0)
129 {
130  yAssert(y0.length()==dim);
131  y=saturate(y0);
132  x_old=0.0;
133 }
134 
135 
136 /************************************************************************/
137 void helperPID::addVectorToOption(Bottle &option, const char *key, const Vector &val)
138 {
139  Bottle &bKey=option.addList();
140  bKey.addString(key);
141  Bottle &bKeyContent=bKey.addList();
142  for (size_t i=0; i<val.length(); i++)
143  bKeyContent.addDouble(val[i]);
144 }
145 
146 
147 /************************************************************************/
148 bool helperPID::getVectorFromOption(const Bottle &options, const char *key,
149  Vector &val, int &size)
150 {
151  if (options.check(key))
152  {
153  if (Bottle *b=options.find(key).asList())
154  {
155  int len=(int)val.length();
156  int bSize=(int)b->size();
157 
158  size=bSize<len?bSize:len;
159  for (int i=0; i<size; i++)
160  val[i]=b->get(i).asDouble();
161 
162  return true;
163  }
164  }
165 
166  return false;
167 }
168 
169 
170 /************************************************************************/
171 parallelPID::parallelPID(const double _Ts,
172  const Vector &_Kp, const Vector &_Ki, const Vector &_Kd,
173  const Vector &_Wp, const Vector &_Wi, const Vector &_Wd,
174  const Vector &_N, const Vector &_Tt, const Matrix &_satLim) :
175  Ts(_Ts), Kp(_Kp), Ki(_Ki), Kd(_Kd), Wp(_Wp), Wi(_Wi), Wd(_Wd),
176  N(_N), Tt(_Tt), satLim(_satLim)
177 {
178  dim=(unsigned int)N.length();
179  u.resize(dim,0.0);
180 
181  uSat.resize(dim,0.0);
182  for (unsigned int i=0; i<dim; i++)
183  uSat[i]=PID_SAT(u[i],satLim(i,0),satLim(i,1));
184 
185  Int=new Integrator(Ts,uSat);
186 
187  Vector u0(1,0.0);
188  Vector num(2),den(2);
189  for (unsigned int i=0; i<dim; i++)
190  {
191  if ((Kp[i]!=0.0) && (N[i]!=0.0))
192  {
193  double tau=Kd[i]/(Kp[i]*N[i]);
194  num[0]=2.0; num[1]=-2.0;
195  den[0]=Ts+2.0*tau; den[1]=Ts-2.0*tau;
196  }
197  else
198  {
199  num[0]=num[1]=den[1]=0.0;
200  den[0]=1.0;
201  }
202 
203  Der.push_back(new Filter(num,den,u0));
204  }
205 
206  P.resize(dim,0.0);
207  I.resize(dim,0.0);
208  D.resize(dim,0.0);
209 }
210 
211 
212 /************************************************************************/
213 const Vector& parallelPID::compute(const Vector &ref, const Vector &fb)
214 {
215  // proportional part
216  P=Kp*(Wp*ref-fb);
217 
218  // integral part
219  I=Int->integrate(Ki*(Wi*ref-fb)+(uSat-u)/Tt);
220 
221  // enforce zero ouput of a disabled integrator
222  for (unsigned int i=0; i<dim; i++)
223  if (Ki[i]==0.0)
224  I[i]=0.0;
225 
226  // derivative part
227  Vector inputD=Kd*(Wd*ref-fb);
228  for (unsigned int i=0; i<dim; i++)
229  {
230  Vector inputDi(1,inputD[i]);
231  Vector outputDi=Der[i]->filt(inputDi);
232  D[i]=outputDi[0];
233  }
234 
235  // cumul output
236  u=P+I+D;
237 
238  // saturation stage
239  for (unsigned int i=0; i<dim; i++)
240  uSat[i]=PID_SAT(u[i],satLim(i,0),satLim(i,1));
241 
242  return uSat;
243 }
244 
245 
246 /************************************************************************/
247 void parallelPID::reset(const Vector &u0)
248 {
249  size_t len=u0.length()>(size_t)dim?(size_t)dim:u0.length();
250 
251  Vector y=Int->get();
252  Vector z1(1,0.0);
253  for (size_t i=0; i<len; i++)
254  {
255  y[i]=u0[i];
256  Der[i]->init(z1);
257  }
258 
259  Int->reset(y);
260 }
261 
262 
263 /************************************************************************/
264 void parallelPID::getOptions(Bottle &options)
265 {
266  Vector satLimVect(satLim.rows()*satLim.cols());
267  for (int r=0; r<satLim.rows(); r++)
268  for (int c=0; c<satLim.cols(); c++)
269  satLimVect[r*satLim.cols()+c]=satLim(r,c);
270 
271  options.clear();
272  addVectorToOption(options,"Kp",Kp);
273  addVectorToOption(options,"Ki",Ki);
274  addVectorToOption(options,"Kd",Kd);
275  addVectorToOption(options,"Wp",Wp);
276  addVectorToOption(options,"Wi",Wi);
277  addVectorToOption(options,"Wd",Wd);
278  addVectorToOption(options,"N",N);
279  addVectorToOption(options,"Tt",Tt);
280  addVectorToOption(options,"satLim",satLimVect);
281 
282  Bottle &bTs=options.addList();
283  bTs.addString("Ts");
284  bTs.addDouble(Ts);
285 }
286 
287 
288 /************************************************************************/
289 void parallelPID::setOptions(const Bottle &options)
290 {
291  Vector satLimVect(satLim.rows()*satLim.cols());
292  for (int r=0; r<satLim.rows(); r++)
293  for (int c=0; c<satLim.cols(); c++)
294  satLimVect[r*satLim.cols()+c]=satLim(r,c);
295 
296  bool recomputeQuantities=false;
297  int size;
298  getVectorFromOption(options,"Ki",Ki,size);
299  getVectorFromOption(options,"Wp",Wp,size);
300  getVectorFromOption(options,"Wi",Wi,size);
301  getVectorFromOption(options,"Wd",Wd,size);
302  getVectorFromOption(options,"Tt",Tt,size);
303 
304  if (getVectorFromOption(options,"Kp",Kp,size))
305  recomputeQuantities=true;
306 
307  if (getVectorFromOption(options,"Kd",Kd,size))
308  recomputeQuantities=true;
309 
310  if (getVectorFromOption(options,"N",N,size))
311  recomputeQuantities=true;
312 
313  if (getVectorFromOption(options,"satLim",satLimVect,size))
314  {
315  for (int r=0; r<satLim.rows(); r++)
316  for (int c=0; c<satLim.cols(); c++)
317  satLim(r,c)=satLimVect[r*satLim.cols()+c];
318 
319  recomputeQuantities=true;
320  }
321 
322  if (options.check("Ts"))
323  {
324  double _Ts=options.find("Ts").asDouble();
325  if (_Ts>0.0)
326  {
327  Ts=_Ts;
328  recomputeQuantities=true;
329  }
330  }
331 
332  if (recomputeQuantities)
333  {
334  for (unsigned int i=0; i<dim; i++)
335  uSat[i]=PID_SAT(uSat[i],satLim(i,0),satLim(i,1));
336 
337  Vector u0(1,0.0);
338  Vector num(2),den(2);
339  for (unsigned int i=0; i<dim; i++)
340  {
341  if ((Kp[i]!=0.0) && (N[i]!=0.0))
342  {
343  double tau=Kd[i]/(Kp[i]*N[i]);
344  num[0]=2.0; num[1]=-2.0;
345  den[0]=Ts+2.0*tau; den[1]=Ts-2.0*tau;
346  }
347  else
348  {
349  num[0]=num[1]=den[1]=0.0;
350  den[0]=1.0;
351  }
352 
353  Der[i]->adjustCoeffs(num,den);
354  Der[i]->init(u0);
355  }
356 
357  Int->setTs(Ts);
358  }
359 
360  Vector v(dim);
361  if (getVectorFromOption(options,"reset",v,size))
362  {
363  Vector u0(size);
364  for (int i=0; i<size; i++)
365  u0[i]=v[i];
366 
367  reset(u0);
368  }
369 }
370 
371 
372 /************************************************************************/
373 parallelPID::~parallelPID()
374 {
375  delete Int;
376 
377  for (unsigned int i=0; i<dim; i++)
378  delete Der[i];
379 
380  Der.clear();
381 }
382 
383 
384 /************************************************************************/
385 seriesPID::seriesPID(const double _Ts,
386  const Vector &_Kp, const Vector &_Ti, const Vector &_Kd,
387  const Vector &_N, const Matrix &_satLim) :
388  Ts(_Ts), Kp(_Kp), Ti(_Ti), Kd(_Kd), N(_N), satLim(_satLim)
389 {
390  dim=(unsigned int)N.length();
391  u.resize(dim,0.0);
392 
393  uSat.resize(dim,0.0);
394  for (unsigned int i=0; i<dim; i++)
395  uSat[i]=PID_SAT(u[i],satLim(i,0),satLim(i,1));
396 
397  Vector u0(1,0.0);
398  Vector num(2),den(2);
399  for (unsigned int i=0; i<dim; i++)
400  {
401  num[0]=Ts; num[1]=Ts;
402  den[0]=Ts+2.0*Ti[i]; den[1]=Ts-2.0*Ti[i];
403  Int.push_back(new Filter(num,den,u0));
404 
405  if ((Kp[i]!=0.0) && (N[i]!=0.0))
406  {
407  double tau=Kd[i]/(Kp[i]*N[i]);
408  num[0]=2.0; num[1]=-2.0;
409  den[0]=Ts+2.0*tau; den[1]=Ts-2.0*tau;
410  }
411  else
412  {
413  num[0]=num[1]=den[1]=0.0;
414  den[0]=1.0;
415  }
416 
417  Der.push_back(new Filter(num,den,u0));
418  }
419 
420  e.resize(dim,0.0);
421  P.resize(dim,0.0);
422  I.resize(dim,0.0);
423  D.resize(dim,0.0);
424 }
425 
426 
427 /************************************************************************/
428 const Vector& seriesPID::compute(const Vector &ref, const Vector &fb)
429 {
430  // compute error
431  e=ref-fb;
432 
433  // derivative part
434  for (unsigned int i=0; i<dim; i++)
435  {
436  Vector inputDi(1,Kd[i]*e[i]);
437  Vector outputDi=Der[i]->filt(inputDi);
438  D[i]=outputDi[0];
439  }
440 
441  // proportional part
442  P=Kp*(e+D);
443 
444  // integral part
445  for (unsigned int i=0; i<dim; i++)
446  {
447  Vector inputIi(1);
448  Vector outputIi(1);
449 
450  inputIi[0]=uSat[i];
451  outputIi=Int[i]->filt(inputIi);
452 
453  I[i]=outputIi[0];
454  }
455 
456  // cumul output
457  u=P+I;
458 
459  // saturation stage
460  for (unsigned int i=0; i<dim; i++)
461  uSat[i]=PID_SAT(u[i],satLim(i,0),satLim(i,1));
462 
463  return uSat;
464 }
465 
466 
467 /************************************************************************/
468 void seriesPID::reset()
469 {
470  Vector u0(1,0.0);
471  for (unsigned int i=0; i<dim; i++)
472  {
473  Int[i]->init(u0);
474  Der[i]->init(u0);
475  }
476 }
477 
478 
479 /************************************************************************/
480 void seriesPID::getOptions(Bottle &options)
481 {
482  Vector satLimVect(satLim.rows()*satLim.cols());
483  for (int r=0; r<satLim.rows(); r++)
484  for (int c=0; c<satLim.cols(); c++)
485  satLimVect[r*satLim.cols()+c]=satLim(r,c);
486 
487  options.clear();
488  addVectorToOption(options,"Kp",Kp);
489  addVectorToOption(options,"Ti",Ti);
490  addVectorToOption(options,"Kd",Kd);
491  addVectorToOption(options,"N",N);
492  addVectorToOption(options,"satLim",satLimVect);
493 
494  Bottle &bTs=options.addList();
495  bTs.addString("Ts");
496  bTs.addDouble(Ts);
497 }
498 
499 
500 /************************************************************************/
501 void seriesPID::setOptions(const Bottle &options)
502 {
503  Vector satLimVect(satLim.rows()*satLim.cols());
504  for (int r=0; r<satLim.rows(); r++)
505  for (int c=0; c<satLim.cols(); c++)
506  satLimVect[r*satLim.cols()+c]=satLim(r,c);
507 
508  bool recomputeQuantities=false;
509  int size;
510  if (getVectorFromOption(options,"Kp",Kp,size))
511  recomputeQuantities=true;
512 
513  if (getVectorFromOption(options,"Ti",Ti,size))
514  recomputeQuantities=true;
515 
516  if (getVectorFromOption(options,"Kd",Kd,size))
517  recomputeQuantities=true;
518 
519  if (getVectorFromOption(options,"N",N,size))
520  recomputeQuantities=true;
521 
522  if (getVectorFromOption(options,"satLim",satLimVect,size))
523  {
524  for (int r=0; r<satLim.rows(); r++)
525  for (int c=0; c<satLim.cols(); c++)
526  satLimVect[r*satLim.cols()+c]=satLim(r,c);
527 
528  recomputeQuantities=true;
529  }
530 
531  if (options.check("Ts"))
532  {
533  double _Ts=options.find("Ts").asDouble();
534  if (_Ts>0.0)
535  {
536  Ts=_Ts;
537  recomputeQuantities=true;
538  }
539  }
540 
541  if (recomputeQuantities)
542  {
543  for (unsigned int i=0; i<dim; i++)
544  uSat[i]=PID_SAT(uSat[i],satLim(i,0),satLim(i,1));
545 
546  Vector u0(1,0.0);
547  Vector num(2),den(2);
548  for (unsigned int i=0; i<dim; i++)
549  {
550  num[0]=Ts; num[1]=Ts;
551  den[0]=Ts+2.0*Ti[i]; den[1]=Ts-2.0*Ti[i];
552  Int[i]->adjustCoeffs(num,den);
553  Int[i]->init(u0);
554 
555  if ((Kp[i]!=0.0) && (N[i]!=0.0))
556  {
557  double tau=Kd[i]/(Kp[i]*N[i]);
558  num[0]=2.0; num[1]=-2.0;
559  den[0]=Ts+2.0*tau; den[1]=Ts-2.0*tau;
560  }
561  else
562  {
563  num[0]=num[1]=den[1]=0.0;
564  den[0]=1.0;
565  }
566 
567  Der[i]->adjustCoeffs(num,den);
568  Der[i]->init(u0);
569  }
570  }
571 
572  if (options.check("reset"))
573  reset();
574 }
575 
576 
577 /************************************************************************/
578 seriesPID::~seriesPID()
579 {
580  for (unsigned int i=0; i<dim; i++)
581  {
582  delete Int[i];
583  delete Der[i];
584  }
585 
586  Int.clear();
587  Der.clear();
588 }
589 
590 
IIR and FIR.
Definition: filters.h:76
Q15 saturate(const Q15result x, bool &saturated)
Definition: strain.cpp:1181
STL namespace.
yarp::sig::Vector x_old
Definition: pids.h:56
yarp::sig::Vector y
Definition: pids.h:55
static int v
Definition: iCub_Sim.cpp:47
A class for defining a saturated integrator based on Tustin formula: .
Definition: pids.h:47
unsigned int dim
Definition: pids.h:54
#define PID_SAT(x, L, H)
Definition: pids.cpp:18