iCub-main
Loading...
Searching...
No Matches
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
20using namespace std;
21using namespace yarp::os;
22using namespace yarp::sig;
23using namespace yarp::math;
24using namespace iCub::ctrl;
25
26
27/************************************************************************/
28Integrator::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/************************************************************************/
42Integrator::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/************************************************************************/
57{
58 y =I.y;
59 x_old =I.x_old;
60 Ts =I.Ts;
61 dim =I.dim;
63}
64
65
66/************************************************************************/
67Vector 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/************************************************************************/
87void Integrator::setSaturation(bool _applySat)
88{
89 applySat=_applySat;
90 if (applySat)
91 y=saturate(y);
92}
93
94
95/************************************************************************/
96void Integrator::setTs(const double _Ts)
97{
98 if (_Ts>0.0)
99 Ts=_Ts;
100}
101
102
103/************************************************************************/
104void 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/************************************************************************/
115const 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/************************************************************************/
128void Integrator::reset(const Vector &y0)
129{
130 yAssert(y0.length()==dim);
131 y=saturate(y0);
132 x_old=0.0;
133}
134
135
136/************************************************************************/
137void 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.addFloat64(val[i]);
144}
145
146
147/************************************************************************/
148bool 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).asFloat64();
161
162 return true;
163 }
164 }
165
166 return false;
167}
168
169
170/************************************************************************/
171parallelPID::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/************************************************************************/
213const 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/************************************************************************/
247void 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/************************************************************************/
264void 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.addFloat64(Ts);
285}
286
287
288/************************************************************************/
289void 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").asFloat64();
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/************************************************************************/
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/************************************************************************/
385seriesPID::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/************************************************************************/
428const 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/************************************************************************/
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/************************************************************************/
480void 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.addFloat64(Ts);
497}
498
499
500/************************************************************************/
501void 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").asFloat64();
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/************************************************************************/
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: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
yarp::sig::Vector y
Definition pids.h:55
yarp::sig::Vector x_old
Definition pids.h:56
const yarp::sig::Vector & integrate(const yarp::sig::Vector &x)
Executes one-step integration of input vector.
Definition pids.cpp:115
void setSaturation(bool _applySat)
Sets the saturation status.
Definition pids.cpp:87
void reset(const yarp::sig::Vector &y0)
Resets the internal state and sets the output vector to the given value.
Definition pids.cpp:128
void allocate(const Integrator &I)
Definition pids.cpp:56
yarp::sig::Vector saturate(const yarp::sig::Vector &v)
Definition pids.cpp:67
void setTs(const double _Ts)
Sets the sample time.
Definition pids.cpp:96
yarp::sig::Matrix lim
Definition pids.h:57
unsigned int dim
Definition pids.h:54
void setLim(const yarp::sig::Matrix &_lim)
Sets the output vector constraints matrix.
Definition pids.cpp:104
static void addVectorToOption(yarp::os::Bottle &option, const char *key, const yarp::sig::Vector &val)
Add the data contained in the specified vector to the specified bottle, using property-like form (i....
Definition pids.cpp:137
static bool getVectorFromOption(const yarp::os::Bottle &options, const char *key, yarp::sig::Vector &val, int &size)
Fill the specified vector with the data associated with the specified key in the specified property-l...
Definition pids.cpp:148
Integrator * Int
Definition pids.h:238
virtual void getOptions(yarp::os::Bottle &options)
Returns the current options used by the pid.
Definition pids.cpp:264
yarp::sig::Vector N
Definition pids.h:225
yarp::sig::Vector I
Definition pids.h:230
yarp::sig::Vector Ki
Definition pids.h:218
yarp::sig::Vector uSat
Definition pids.h:233
~parallelPID()
Destructor.
Definition pids.cpp:373
unsigned int dim
Definition pids.h:235
virtual void setOptions(const yarp::os::Bottle &options)
Update the options used by the pid.
Definition pids.cpp:289
yarp::sig::Vector Wp
Definition pids.h:221
yarp::sig::Vector P
Definition pids.h:229
yarp::sig::Vector Wi
Definition pids.h:222
std::deque< Filter * > Der
Definition pids.h:239
yarp::sig::Vector Kd
Definition pids.h:219
virtual void reset(const yarp::sig::Vector &u0)
Resets the internal state of integral and derivative part.
Definition pids.cpp:247
yarp::sig::Vector Tt
Definition pids.h:226
yarp::sig::Matrix satLim
Definition pids.h:227
yarp::sig::Vector Kp
Definition pids.h:217
yarp::sig::Vector u
Definition pids.h:232
yarp::sig::Vector D
Definition pids.h:231
yarp::sig::Vector Wd
Definition pids.h:223
virtual const yarp::sig::Vector & compute(const yarp::sig::Vector &ref, const yarp::sig::Vector &fb)
Computes the PID output.
Definition pids.cpp:213
yarp::sig::Vector u
Definition pids.h:341
virtual void setOptions(const yarp::os::Bottle &options)
Update the options used by the pid.
Definition pids.cpp:501
yarp::sig::Vector e
Definition pids.h:337
~seriesPID()
Destructor.
Definition pids.cpp:578
yarp::sig::Vector I
Definition pids.h:339
std::deque< Filter * > Int
Definition pids.h:347
yarp::sig::Matrix satLim
Definition pids.h:335
virtual void reset()
Resets the internal state of integral and derivative part.
Definition pids.cpp:468
yarp::sig::Vector P
Definition pids.h:338
yarp::sig::Vector Kd
Definition pids.h:332
yarp::sig::Vector D
Definition pids.h:340
virtual const yarp::sig::Vector & compute(const yarp::sig::Vector &ref, const yarp::sig::Vector &fb)
Computes the PID output.
Definition pids.cpp:428
std::deque< Filter * > Der
Definition pids.h:348
yarp::sig::Vector Kp
Definition pids.h:330
unsigned int dim
Definition pids.h:344
yarp::sig::Vector uSat
Definition pids.h:342
yarp::sig::Vector N
Definition pids.h:334
yarp::sig::Vector Ti
Definition pids.h:331
virtual void getOptions(yarp::os::Bottle &options)
Returns the current options used by the pid.
Definition pids.cpp:480
#define PID_SAT(x, L, H)
Definition pids.cpp:18