iCub-main
tuning.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 <limits>
12 #include <cmath>
13 #include <algorithm>
14 #include <string>
15 #include <sstream>
16 
17 #include <yarp/math/Math.h>
18 #include <yarp/math/SVD.h>
19 #include <iCub/ctrl/tuning.h>
20 
21 using namespace std;
22 using namespace yarp::os;
23 using namespace yarp::dev;
24 using namespace yarp::sig;
25 using namespace yarp::math;
26 using namespace iCub::ctrl;
27 
28 
29 /**********************************************************************/
30 OnlineDCMotorEstimator::OnlineDCMotorEstimator()
31 {
32  Vector x0(4,0.0);
33  x0[2]=x0[3]=1.0;
34  init(0.01,1.0,1.0,1e5,x0);
35 }
36 
37 
38 /**********************************************************************/
39 bool OnlineDCMotorEstimator::init(const double Ts, const double Q,
40  const double R, const double P0,
41  const Vector &x0)
42 {
43  if (x0.length()<4)
44  return false;
45 
46  A=F=eye(4,4);
47  B.resize(4,0.0);
48  C.resize(1,4); C(0,0)=1.0;
49  Ct=C.transposed();
50 
51  P=P0*eye(4,4);
52  this->Q=Q*eye(4,4);
53  this->R=R;
54 
55  this->Ts=Ts;
56  x=_x=x0.subVector(0,3);
57  x[2]=1.0/_x[2];
58  x[3]=_x[3]/_x[2];
59 
60  uOld=0.0;
61 
62  return true;
63 }
64 
65 
66 /**********************************************************************/
67 bool OnlineDCMotorEstimator::init(const double P0, const Vector &x0)
68 {
69  if (x0.length()<4)
70  return false;
71 
72  P=P0*eye(4,4);
73  x=_x=x0.subVector(0,3);
74  x[2]=1.0/_x[2];
75  x[3]=_x[3]/_x[2];
76 
77  uOld=0.0;
78 
79  return true;
80 }
81 
82 
83 /**********************************************************************/
84 Vector OnlineDCMotorEstimator::estimate(const double u, const double y)
85 {
86  double &x1=x[0];
87  double &x2=x[1];
88  double &x3=x[2];
89  double &x4=x[3];
90 
91  double _exp=exp(-Ts*x3);
92  double _exp_1=1.0-_exp;
93  double _x3_2=x3*x3;
94  double _tmp_1=(Ts*x3-_exp_1)/_x3_2;
95 
96  A(0,1)=_exp_1/x3;
97  A(1,1)=_exp;
98 
99  B[0]=x4*_tmp_1;
100  B[1]=x4*A(0,1);
101 
102  F(0,1)=A(0,1);
103  F(1,1)=A(1,1);
104 
105  F(0,2)=-(x2*_exp_1)/_x3_2 + (uOld*x4*Ts*_exp_1)/_x3_2 - (2.0*uOld*B[0])/x3 + (Ts*x2*_exp)/x3;
106  F(1,2)=-(uOld*x4*_exp_1)/_x3_2 - Ts*x2*_exp + (uOld*x4*Ts*_exp)/x3;
107 
108  F(0,3)=uOld*_tmp_1;
109  F(1,3)=uOld*A(0,1);
110 
111  // prediction
112  x=A*x+B*uOld;
113  P=F*P*F.transposed()+Q;
114 
115  // Kalman gain
116  Matrix tmpMat=C*P*Ct;
117  Matrix K=P*Ct/(tmpMat(0,0)+R);
118 
119  // correction
120  x+=K*(y-C*x);
121  P=(eye(4,4)-K*C)*P;
122 
123  _x[0]=x[0];
124  _x[1]=x[1];
125  _x[2]=1.0/x[2];
126  _x[3]=x[3]/x[2];
127 
128  uOld=u;
129 
130  return _x;
131 }
132 
133 
134 /**********************************************************************/
135 OnlineStictionEstimator::OnlineStictionEstimator() :
136  PeriodicThread(1.0), velEst(32,4.0), accEst(32,4.0),
137  trajGen(1,1.0,1.0), intErr(1.0,Vector(2,0.0)), done(2)
138 {
139  imod=NULL;
140  ilim=NULL;
141  ienc=NULL;
142  ipid=NULL;
143  ipwm=NULL;
144  icur=NULL;
145  configured=false;
146 }
147 
148 
149 /**********************************************************************/
150 bool OnlineStictionEstimator::configure(PolyDriver &driver, const Property &options)
151 {
152  if (driver.isValid() && options.check("joint"))
153  {
154  bool ok=true;
155  ok&=driver.view(imod);
156  ok&=driver.view(ilim);
157  ok&=driver.view(ienc);
158  ok&=driver.view(ipid);
159  ok&=driver.view(ipwm);
160  ok&=driver.view(icur);
161 
162  if (!ok)
163  return false;
164 
165  joint=options.find("joint").asInt32();
166  setPeriod(options.check("Ts",Value(0.01)).asFloat64());
167 
168  T=options.check("T",Value(2.0)).asFloat64();
169  Kp=options.check("Kp",Value(10.0)).asFloat64();
170  Ki=options.check("Ki",Value(250.0)).asFloat64();
171  Kd=options.check("Kd",Value(15.0)).asFloat64();
172  vel_thres=fabs(options.check("vel_thres",Value(5.0)).asFloat64());
173  e_thres=fabs(options.check("e_thres",Value(1.0)).asFloat64());
174 
175  gamma.resize(2,1.0);
176  if (Bottle *pB=options.find("gamma").asList())
177  {
178  size_t len=std::min(gamma.length(),(size_t)pB->size());
179  for (size_t i=0; i<len; i++)
180  gamma[i]=pB->get(i).asFloat64();
181  }
182 
183  stiction.resize(2,0.0);
184  if (Bottle *pB=options.find("stiction").asList())
185  {
186  size_t len=std::min(stiction.length(),(size_t)pB->size());
187  for (size_t i=0; i<len; i++)
188  stiction[i]=pB->get(i).asFloat64();
189  }
190 
191  return configured=true;
192  }
193  else
194  return false;
195 }
196 
197 
198 /**********************************************************************/
199 bool OnlineStictionEstimator::reconfigure(const Property &options)
200 {
201  if (configured)
202  {
203  if (options.check("joint"))
204  joint=options.find("joint").asInt32();
205 
206  if (options.check("Ts"))
207  setPeriod(options.find("Ts").asFloat64());
208 
209  if (options.check("T"))
210  T=options.find("T").asFloat64();
211 
212  if (options.check("Kp"))
213  Kp=options.find("Kp").asFloat64();
214 
215  if (options.check("Ki"))
216  Ki=options.find("Ki").asFloat64();
217 
218  if (options.check("Kd"))
219  Kd=options.find("Kd").asFloat64();
220 
221  if (options.check("vel_thres"))
222  vel_thres=options.find("vel_thres").asFloat64();
223 
224  if (options.check("e_thres"))
225  e_thres=options.find("e_thres").asFloat64();
226 
227  if (options.check("gamma"))
228  {
229  if (Bottle *pB=options.find("gamma").asList())
230  {
231  size_t len=std::min(gamma.length(),(size_t)pB->size());
232  for (size_t i=0; i<len; i++)
233  gamma[i]=pB->get(i).asFloat64();
234  }
235  }
236 
237  if (options.check("stiction"))
238  {
239  if (Bottle *pB=options.find("stiction").asList())
240  {
241  size_t len=std::min(stiction.length(),(size_t)pB->size());
242  for (size_t i=0; i<len; i++)
243  stiction[i]=pB->get(i).asFloat64();
244  }
245  }
246 
247  return true;
248  }
249  else
250  return false;
251 }
252 
253 
254 /**********************************************************************/
256 {
259 }
260 
261 
262 /**********************************************************************/
264 {
265  if (!configured)
266  return false;
267 
268  ilim->getLimits(joint,&x_min,&x_max);
269  double x_range=x_max-x_min;
270  x_min+=0.1*x_range;
271  x_max-=0.1*x_range;
272  imod->setControlMode(joint,VOCAB_CM_PWM);
273 
274  ienc->getEncoder(joint,&x_pos);
275  x_vel=0.0;
276  x_acc=0.0;
277 
278  tg=x_min;
279  xd_pos=x_pos;
280  state=(tg-x_pos>0.0)?rising:falling;
281  adapt=adaptOld=false;
282 
283  trajGen.setTs(getPeriod());
284  trajGen.setT(T);
285  trajGen.init(Vector(1,x_pos));
286 
287  Vector _Kp(1,Kp), _Ki(1,Ki), _Kd(1,Kd);
288  Vector _Wp(1,1.0), _Wi(1,1.0), _Wd(1,1.0);
289  Vector _N(1,10.0), _Tt(1,1.0);
290 
291  Pid pidInfo;
292  ipid->getPid(VOCAB_PIDTYPE_POSITION,joint,&pidInfo);
293  dpos_dV=(pidInfo.kp>=0.0?-1.0:1.0);
294  stiction*=dpos_dV;
295  Matrix _satLim(1,2);
296  _satLim(0,0)=-pidInfo.max_int;
297  _satLim(0,1)=pidInfo.max_int;
298  stiction_limit=pidInfo.max_output;
300 
301  pid=new parallelPID(getPeriod(),_Kp,_Ki,_Kd,_Wp,_Wi,_Wd,_N,_Tt,_satLim);
302  pid->reset(Vector(1,0.0));
303 
304  intErr.setTs(getPeriod());
306 
307  done=0.0;
308  t0=Time::now();
309 
310  return true;
311 }
312 
313 
314 /**********************************************************************/
316 {
317  mtx.lock();
318 
319  ienc->getEncoder(joint,&x_pos);
320 
321  AWPolyElement el(Vector(1,x_pos),Time::now());
322  Vector vel=velEst.estimate(el); x_vel=vel[0];
323  Vector acc=accEst.estimate(el); x_acc=acc[0];
324 
325  double t=Time::now()-t0;
326  if (t>2.0*trajGen.getT())
327  {
328  tg=(tg==x_min)?x_max:x_min;
329  state=(tg-x_pos>0.0)?rising:falling;
330  adapt=(fabs(x_vel)<vel_thres);
331  t0=Time::now();
332  }
333 
334  trajGen.computeNextValues(Vector(1,tg));
335  xd_pos=trajGen.getPos()[0];
336 
337  Vector pid_out=pid->compute(Vector(1,xd_pos),Vector(1,x_pos));
338  double e_pos=xd_pos-x_pos;
339  double fw=(state==rising)?stiction[0]:stiction[1];
340  double u=fw+pid_out[0];
341 
342  Vector adaptGate(stiction.length(),0.0);
343  if ((fabs(x_vel)<vel_thres) && adapt)
344  adaptGate[(state==rising)?0:1]=1.0;
345  else
346  adapt=false;
347 
348  Vector cumErr=intErr.integrate(e_pos*adaptGate);
349 
350  // trigger on falling edge
351  if (!adapt && adaptOld)
352  {
353  Vector e_mean=cumErr/t;
354  if (yarp::math::norm(e_mean)>e_thres)
355  {
356  stiction+=gamma*e_mean;
358  done[state]=0.0;
359  }
360  else
361  done[state]=1.0;
362 
363  intErr.reset(Vector(stiction.length(),0.0));
364  }
365 
366  ipwm->setRefDutyCycle(joint,dpos_dV*u);
367  adaptOld=adapt;
368 
369  // fill in info
370  info.unput("voltage"); info.put("voltage",u);
371  info.unput("position"); info.put("position",x_pos);
372  info.unput("reference"); info.put("reference",xd_pos);
373 
374  mtx.unlock();
375 
376  if (done[0]*done[1]!=0.0)
377  cv_doneEvent.notify_all();
378 }
379 
380 
381 /**********************************************************************/
383 {
384  ipwm->setRefDutyCycle(joint,0.0);
385  imod->setControlMode(joint,VOCAB_CM_POSITION);
386  delete pid;
387 
388  cv_doneEvent.notify_all();
389 }
390 
391 
392 /**********************************************************************/
394 {
395  if (!configured)
396  return false;
397 
398  lock_guard<mutex> lck(mtx);
399  bool ret=(done[0]*done[1]!=0.0);
400  return ret;
401 }
402 
403 
404 /**********************************************************************/
406 {
407  if (!configured)
408  return false;
409 
410  unique_lock<mutex> lck(mtx_doneEvent);
411  cv_doneEvent.wait(lck);
412 
413  return isDone();
414 }
415 
416 
417 /**********************************************************************/
419 {
420  if (!configured)
421  return false;
422 
423  lock_guard<mutex> lck(mtx);
424  results=dpos_dV*stiction;
425  return true;
426 }
427 
428 
429 /**********************************************************************/
431 {
432  if (!configured)
433  return false;
434 
435  lock_guard<mutex> lck(mtx);
436  info=this->info;
437  return true;
438 }
439 
440 
441 /**********************************************************************/
443  predictor(Matrix(2,2),Matrix(2,1),Matrix(1,2),
444  Matrix(2,2),Matrix(1,1))
445 {
446  imod=NULL;
447  ilim=NULL;
448  ienc=NULL;
449  ipos=NULL;
450  idir=NULL;
451  ipid=NULL;
452  ipwm=NULL;
453  icur=NULL;
454  configured=false;
455 }
456 
457 
458 /**********************************************************************/
459 bool OnlineCompensatorDesign::configure(PolyDriver &driver, const Property &options)
460 {
461  if (!driver.isValid())
462  return false;
463 
464  bool ok=true;
465  ok&=driver.view(imod);
466  ok&=driver.view(ilim);
467  ok&=driver.view(ienc);
468  ok&=driver.view(ipos);
469  ok&=driver.view(idir);
470  ok&=driver.view(ipid);
471  ok&=driver.view(ipwm);
472  ok&=driver.view(icur);
473 
474  if (!ok)
475  return false;
476 
477  // general options
478  Bottle &optGeneral=options.findGroup("general");
479  if (optGeneral.isNull())
480  return false;
481 
482  if (!optGeneral.check("joint"))
483  return false;
484 
485  joint=optGeneral.find("joint").asInt32();
486 
487  if (optGeneral.check("port"))
488  {
489  string name=optGeneral.find("port").asString();
490  if (name[0]!='/')
491  name="/"+name;
492 
493  if (!port.open(name))
494  return false;
495  }
496 
497  // configure plant estimator
498  Bottle &optPlant=options.findGroup("plant_estimation");
499  if (optPlant.isNull())
500  return false;
501 
502  Pid pidInfo;
503  ipid->getPid(VOCAB_PIDTYPE_POSITION,joint,&pidInfo);
504  dpos_dV=(pidInfo.kp>=0.0?-1.0:1.0);
505 
506  ilim->getLimits(joint,&x_min,&x_max);
507  double x_range=x_max-x_min;
508  x_min+=0.1*x_range;
509  x_max-=0.1*x_range;
510 
511  x0.resize(4,0.0);
512  x0[2]=optPlant.check("tau",Value(1.0)).asFloat64();
513  x0[3]=optPlant.check("K",Value(1.0)).asFloat64();
514 
515  double Ts=optPlant.check("Ts",Value(0.01)).asFloat64();
516  double Q=optPlant.check("Q",Value(1.0)).asFloat64();
517  double R=optPlant.check("R",Value(1.0)).asFloat64();
518  P0=optPlant.check("P0",Value(1e5)).asFloat64();
519  max_pwm=optPlant.check("max_pwm",Value(800)).asFloat64();
520 
521  setPeriod(Ts);
522 
523  if (!plant.init(Ts,Q,R,P0,x0))
524  return false;
525 
526  // configure stiction estimator
527  Bottle &optStiction=options.findGroup("stiction_estimation");
528  if (!optStiction.isNull())
529  {
530  Property propStiction(optStiction.toString().c_str());
531 
532  // enforce the equality between the common properties
533  propStiction.unput("joint");
534  propStiction.put("joint",joint);
535 
536  if (!stiction.configure(driver,propStiction))
537  return false;
538  }
539 
540  meanParams.resize(2,0.0);
541 
542  return configured=true;
543 }
544 
545 
546 /**********************************************************************/
548 {
549  bool ret=true;
550  switch (mode)
551  {
552  // -----
553  case plant_estimation:
554  {
555  imod->setControlMode(joint,VOCAB_CM_PWM);
556  ienc->getEncoder(joint,&x0[0]);
557  plant.init(P0,x0);
558  meanParams=0.0;
559  meanCnt=0;
560  x_tg=x_max;
561  pwm_pos=true;
562  t1=Time::now();
563  break;
564  }
565 
566  // -----
567  case plant_validation:
568  {
569  Vector _x0(2,0.0);
570  imod->setControlMode(joint,VOCAB_CM_PWM);
571  ienc->getEncoder(joint,&_x0[0]);
572  predictor.init(_x0,predictor.get_P());
574  x_tg=x_max;
575  pwm_pos=true;
576  t1=Time::now();
577  break;
578  }
579 
580  // -----
581  case stiction_estimation:
582  {
584  break;
585  }
586 
587  // -----
589  {
590  pidCur=&pidOld;
591  x_tg=x_max;
593  {
594  imod->setControlMode(joint,VOCAB_CM_POSITION_DIRECT);
595  idir->setPosition(joint,x_tg);
596  }
597  else
598  {
599  imod->setControlMode(joint,VOCAB_CM_POSITION);
600  ipos->setRefAcceleration(joint,std::numeric_limits<double>::max());
602  ipos->positionMove(joint,x_tg);
603  }
605  t1=Time::now();
606  break;
607  }
608  }
609 
610  t0=Time::now();
611 
612  return ret;
613 }
614 
615 
616 /**********************************************************************/
617 void OnlineCompensatorDesign::commandJoint(double &enc, double &u)
618 {
619  double t=Time::now();
620  bool timeoutExpired=(switch_timeout>0.0?t-t1>switch_timeout:false);
621  ienc->getEncoder(joint,&enc);
622 
623  // switch logic
624  if (x_tg==x_max)
625  {
626  if ((enc>x_max) || timeoutExpired)
627  {
628  x_tg=x_min;
629  pwm_pos=false;
630  t1=t;
631  }
632  }
633  else if ((enc<x_min) || timeoutExpired)
634  {
635  x_tg=x_max;
636  pwm_pos=true;
637  t1=t;
638  }
639 
640  u=(pwm_pos?max_pwm:-max_pwm);
641  ipwm->setRefDutyCycle(joint,dpos_dV*u);
642 }
643 
644 
645 /**********************************************************************/
647 {
648  double t=Time::now();
649  if (max_time>0.0)
650  if (t-t0>max_time)
651  askToStop();
652 
653  lock_guard<mutex> lck(mtx);
654  switch (mode)
655  {
656  // -----
657  case plant_estimation:
658  {
659  double enc,u;
660  commandJoint(enc,u);
661  plant.estimate(u,enc);
662 
663  // average parameters tau and K
666  meanParams/=++meanCnt;
667 
668  if (port.getOutputCount()>0)
669  {
670  Vector &info=port.prepare();
671  info.resize(3);
672 
673  info[0]=0.0;
674  info[1]=u;
675  info[2]=enc;
676  info=cat(info,plant.get_x());
677  info=cat(info,meanParams);
678 
679  port.write();
680  }
681 
682  break;
683  }
684 
685  // -----
686  case plant_validation:
687  {
688  double enc,u;
689  commandJoint(enc,u);
690  predictor.predict(Vector(1,u));
691 
692  // correction only when requested
693  if (measure_update_ticks>0)
694  {
696  {
697  predictor.correct(Vector(1,enc));
699  }
700  }
701 
702  if (port.getOutputCount()>0)
703  {
704  Vector &info=port.prepare();
705  info.resize(3);
706 
707  info[0]=1.0;
708  info[1]=u;
709  info[2]=enc;
710  info=cat(info,predictor.get_x());
711  info=cat(info,Vector(4,0.0)); // zero-padding
712 
713  port.write();
714  }
715 
716  break;
717  }
718 
719  // -----
720  case stiction_estimation:
721  {
722  if (stiction.isDone())
723  askToStop();
724 
725  if (port.getOutputCount()>0)
726  {
727  Property stiction_info;
728  stiction.getInfo(stiction_info);
729 
730  Vector &info=port.prepare();
731  info.resize(4);
732 
733  info[0]=2.0;
734  info[1]=stiction_info.find("voltage").asFloat64();
735  info[2]=stiction_info.find("position").asFloat64();
736  info[3]=stiction_info.find("reference").asFloat64();
737 
738  Vector results;
739  stiction.getResults(results);
740  info=cat(info,results);
741  info=cat(info,Vector(3,0.0)); // zero-padding
742 
743  port.write();
744  }
745 
746  break;
747  }
748 
749  // -----
751  {
754  {
756  t1=t;
757 
758  if (x_tg==x_max)
759  {
761  {
764  ipwm->setRefDutyCycle(joint,0.0);
765 
766  ipid->setPid(VOCAB_PIDTYPE_POSITION,joint,*pidCur);
768  }
769  }
770 
774 
776  idir->setPosition(joint,x_tg);
777  else
778  ipos->positionMove(joint,x_tg);
779  }
780 
781  if (port.getOutputCount()>0)
782  {
783  Vector &info=port.prepare();
784  info.resize(5);
785 
786  info[0]=3.0;
787  ipwm->getDutyCycle(joint,&info[1]);
788  ienc->getEncoder(joint,&info[2]);
789  ipid->getPidReference(VOCAB_PIDTYPE_POSITION,joint,&info[3]);
790  info[4]=(pidCur==&pidOld?0.0:1.0);
791  info=cat(info,Vector(4,0.0)); // zero-padding
792 
793  port.write();
794  }
795 
796  break;
797  }
798  }
799 }
800 
801 
802 /**********************************************************************/
804 {
805  switch (mode)
806  {
807  // -----
808  case plant_estimation:
809  // -----
810  case plant_validation:
811  {
812  ipwm->setRefDutyCycle(joint,0.0);
813  imod->setControlMode(joint,VOCAB_CM_POSITION);
814  break;
815  }
816 
817  // -----
818  case stiction_estimation:
819  {
821  break;
822  }
823 
824  // -----
826  {
827  ipos->stop(joint);
828  ipid->setPid(VOCAB_PIDTYPE_POSITION,joint,pidOld);
829  break;
830  }
831  }
832 
833  cv_doneEvent.notify_all();
834 }
835 
836 
837 /**********************************************************************/
838 bool OnlineCompensatorDesign::tuneController(const Property &options,
839  Property &results)
840 {
841  if (!options.check("tau") || !options.check("K") ||
842  !options.check("type") || !options.check("f_c"))
843  return false;
844 
845  double tau=options.find("tau").asFloat64();
846  double K=options.find("K").asFloat64();
847  string type=options.check("type",Value("PI")).asString();
848  double omega_c=2.0*M_PI*options.find("f_c").asFloat64();
849  double Kp,Ki;
850 
851  // P design
852  if (type=="P")
853  {
854  Kp=(omega_c/K)*sqrt(1.0+omega_c*omega_c*tau*tau);
855  Ki=0.0;
856  }
857  // PI design
858  else if (type=="PI")
859  {
860  double T_dr=1.0/omega_c;
861  if (options.check("T_dr"))
862  T_dr=options.find("T_dr").asFloat64();
863 
864  double tau_dr=T_dr/3.0;
865  double omega_dr=1.0/tau_dr;
866 
867  // reinforce omega_dr as the slowest pole among the three
868  omega_dr=std::min(omega_dr,1.0/(3.0*tau));
869 
870  Kp=(omega_c/K)*sqrt(1.0+omega_c*omega_c*tau*tau);
871  Ki=omega_dr*(Kp-(omega_dr*(1.0-omega_dr*tau))/K);
872 
873  // reinforce no mutual dependency between Kp and Ki design
874  Ki=std::min(Ki,sqrt(10.0)*omega_c*Kp);
875 
876  // reinforce stable closed loop system
877  Ki=std::min(Ki,Kp/tau);
878  }
879  else
880  return false;
881 
882  results.clear();
883  results.put("Kp",Kp);
884  results.put("Ki",Ki);
885 
886  return true;
887 }
888 
889 
890 /**********************************************************************/
892 {
893  if (!configured)
894  return false;
895 
896  max_time=options.check("max_time",Value(0.0)).asFloat64();
897  switch_timeout=options.check("switch_timeout",Value(0.0)).asFloat64();
898 
900  return PeriodicThread::start();
901 }
902 
903 
904 /**********************************************************************/
906 {
907  if (!configured || !options.check("tau") || !options.check("K"))
908  return false;
909 
910  max_time=options.check("max_time",Value(0.0)).asFloat64();
911  switch_timeout=options.check("switch_timeout",Value(0.0)).asFloat64();
912  measure_update_ticks=options.check("measure_update_ticks",Value(100)).asInt32();
913 
914  double tau=options.find("tau").asFloat64();
915  double K=options.find("K").asFloat64();
916  double Ts=getPeriod();
917  double a=1.0/tau;
918  double b=K/tau;
919 
920  double Q=options.check("Q",Value(1.0)).asFloat64();
921  double R=options.check("R",Value(1.0)).asFloat64();
922  double P0=options.check("P0",Value(this->P0)).asFloat64();
923 
924  // set up the Kalman filter
925  double _exp=exp(-Ts*a);
926  double _exp_1=1.0-_exp;
927 
928  Matrix A=eye(2,2);
929  A(0,1)=_exp_1/a;
930  A(1,1)=_exp;
931 
932  Matrix B(2,1);
933  B(0,0)=b*(a*Ts-_exp_1)/(a*a);
934  B(1,0)=b*A(0,1);
935 
936  Matrix H(1,2);
937  H(0,0)=1.0;
938  H(0,1)=0.0;
939 
940  predictor.set_A(A);
941  predictor.set_B(B);
942  predictor.set_H(H);
943  predictor.set_Q(Q*eye(2,2));
944  predictor.set_R(R*eye(1,1));
945  predictor.init(Vector(2,0.0),P0*eye(2,2));
946 
948  return PeriodicThread::start();
949 }
950 
951 
952 /**********************************************************************/
954 {
955  if (!configured)
956  return false;
957 
958  Property opt=options;
959  max_time=opt.check("max_time",Value(0.0)).asFloat64();
960 
961  opt.unput("joint");
962  if (!stiction.reconfigure(opt))
963  return false;
964 
966  return PeriodicThread::start();
967 }
968 
969 
970 /**********************************************************************/
972 {
973  if (!configured || !options.check("Kp"))
974  return false;
975 
976  max_time=options.check("max_time",Value(0.0)).asFloat64();
977 
978  ipid->getPid(VOCAB_PIDTYPE_POSITION,joint,&pidOld);
979  pidNew=pidOld;
980 
981  // enforce the correct sign of gains
982  double Kp=options.find("Kp").asFloat64();
983  double Ki=options.check("Ki",Value(0.0)).asFloat64();
984  double Kd=options.check("Kd",Value(0.0)).asFloat64();
985  Kp=(Kp*pidOld.kp>0.0)?Kp:-Kp;
986  Ki=(Ki*pidOld.ki>0.0)?Ki:-Ki;
987  Kd=(Kd*pidOld.kd>0.0)?Kd:-Kd;
988  pidNew.setKp(Kp);
989  pidNew.setKi(Ki);
990  pidNew.setKd(Kd);
991  pidNew.setStictionValues(0.0,0.0);
992 
993  if (options.check("scale"))
994  pidNew.setScale(options.find("scale").asInt32());
995 
996  controller_validation_ref_square=(options.check("ref_type",Value("square")).asString()=="square");
997  controller_validation_ref_period=options.check("ref_period",Value(2.0)).asFloat64();
998  controller_validation_ref_sustain_time=options.check("ref_sustain_time",Value(0.0)).asFloat64();
999  controller_validation_cycles_to_switch=options.check("cycles_to_switch",Value(1)).asInt32();
1000  controller_validation_stiction_yarp=(options.check("stiction_compensation",Value("firmware")).asString()!="firmware");
1002 
1003  if (options.check("stiction"))
1004  {
1005  if (Bottle *pB=options.find("stiction").asList())
1006  {
1007  if (pB->size()>=2)
1008  {
1009  controller_validation_stiction_up=pB->get(0).asFloat64();
1010  controller_validation_stiction_down=pB->get(1).asFloat64();
1011 
1013  pidNew.setStictionValues(controller_validation_stiction_up,
1015  }
1016  }
1017  }
1018 
1020  return PeriodicThread::start();
1021 }
1022 
1023 
1024 /**********************************************************************/
1026 {
1027  if (!configured)
1028  return false;
1029 
1030  return !isRunning();
1031 }
1032 
1033 
1034 /**********************************************************************/
1036 {
1037  if (!configured)
1038  return false;
1039 
1040  unique_lock<mutex> lck(mtx_doneEvent);
1041  cv_doneEvent.wait(lck);
1042 
1043  return isDone();
1044 }
1045 
1046 
1047 /**********************************************************************/
1049 {
1050  if (!configured)
1051  return false;
1052 
1053  results.clear();
1054 
1055  lock_guard<mutex> lck(mtx);
1056  switch (mode)
1057  {
1058  // -----
1059  case plant_estimation:
1060  {
1061  Vector params=plant.get_parameters();
1062  results.put("tau",params[0]);
1063  results.put("K",params[1]);
1064  results.put("tau_mean",meanParams[0]);
1065  results.put("K_mean",meanParams[1]);
1066  break;
1067  }
1068 
1069  // -----
1070  case plant_validation:
1071  {
1072  Vector response=predictor.get_x();
1073  results.put("position",response[0]);
1074  results.put("velocity",response[1]);
1075  break;
1076  }
1077 
1078  // -----
1079  case stiction_estimation:
1080  {
1081  Vector values;
1082  stiction.getResults(values);
1083  ostringstream str;
1084  str<<"( ";
1085  str<<values[0];
1086  str<<" ";
1087  str<<values[1];
1088  str<<" )";
1089  Value val; val.fromString(str.str().c_str());
1090  results.put("stiction",val);
1091  break;
1092  }
1093 
1094  // -----
1095  case controller_validation:
1096  {
1097  Vector info(3);
1098  ipwm->getDutyCycle(joint,&info[0]);
1099  ienc->getEncoder(joint,&info[1]);
1100  ipid->getPidReference(VOCAB_PIDTYPE_POSITION,joint,&info[2]);
1101 
1102  results.put("voltage",info[0]);
1103  results.put("position",info[1]);
1104  results.put("reference",info[2]);
1105  results.put("pid",pidCur==&pidOld?"old":"new");
1106  break;
1107  }
1108  }
1109 
1110  return true;
1111 }
1112 
1113 
1114 /**********************************************************************/
1116 {
1117  port.close();
1118 }
1119 
1120 
1121 
1122 
#define M_PI
Definition: XSensMTx.cpp:24
Basic element for adaptive polynomial fitting.
yarp::sig::Vector estimate()
Execute the algorithm upon the elements list, with the max deviation threshold given by D.
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
void setTs(const double _Ts)
Sets the sample time.
Definition: pids.cpp:96
bool set_Q(const yarp::sig::Matrix &_Q)
Returns the process noise covariance matrix.
Definition: kalman.cpp:168
bool set_R(const yarp::sig::Matrix &_R)
Returns the measurement noise covariance matrix.
Definition: kalman.cpp:181
bool init(const yarp::sig::Vector &_x0, const yarp::sig::Matrix &_P0)
Set initial state and error covariance.
Definition: kalman.cpp:59
const yarp::sig::Vector & predict(const yarp::sig::Vector &u)
Predicts the next state vector given the current input.
bool set_A(const yarp::sig::Matrix &_A)
Returns the state transition matrix.
Definition: kalman.cpp:127
const yarp::sig::Vector & get_x() const
Returns the estimated state.
Definition: kalman.h:153
bool set_B(const yarp::sig::Matrix &_B)
Returns the input matrix.
Definition: kalman.cpp:141
const yarp::sig::Vector & correct(const yarp::sig::Vector &z)
Corrects the current estimation of the state vector given the current measurement.
Definition: kalman.cpp:91
bool set_H(const yarp::sig::Matrix &_H)
Returns the measurement transition matrix.
Definition: kalman.cpp:154
const yarp::sig::Matrix & get_P() const
Returns the estimated state covariance.
Definition: kalman.h:167
virtual bool isDone()
Check the status of the current ongoing operation.
Definition: tuning.cpp:1025
OnlineCompensatorDesign()
Default constructor.
Definition: tuning.cpp:442
virtual bool tuneController(const yarp::os::Property &options, yarp::os::Property &results)
Tune the controller once given the plant characteristics.
Definition: tuning.cpp:838
void commandJoint(double &enc, double &u)
Definition: tuning.cpp:617
yarp::dev::IPidControl * ipid
Definition: tuning.h:359
yarp::dev::IPWMControl * ipwm
Definition: tuning.h:360
virtual bool startStictionEstimation(const yarp::os::Property &options)
Start off the stiction estimation procedure.
Definition: tuning.cpp:953
yarp::dev::IEncoders * ienc
Definition: tuning.h:356
OnlineStictionEstimator stiction
Definition: tuning.h:351
std::condition_variable cv_doneEvent
Definition: tuning.h:368
yarp::dev::IPositionControl * ipos
Definition: tuning.h:357
yarp::dev::IControlMode * imod
Definition: tuning.h:354
yarp::dev::ICurrentControl * icur
Definition: tuning.h:361
virtual bool startPlantEstimation(const yarp::os::Property &options)
Start off the plant estimation procedure.
Definition: tuning.cpp:891
enum iCub::ctrl::OnlineCompensatorDesign::@2 mode
virtual bool startPlantValidation(const yarp::os::Property &options)
Start off the plant validation procedure.
Definition: tuning.cpp:905
virtual bool getResults(yarp::os::Property &results)
Retrieve the results of the current ongoing operation.
Definition: tuning.cpp:1048
virtual bool waitUntilDone()
Wait until the current ongoing operation is accomplished.
Definition: tuning.cpp:1035
yarp::dev::IPositionDirect * idir
Definition: tuning.h:358
yarp::dev::IControlLimits * ilim
Definition: tuning.h:355
OnlineDCMotorEstimator plant
Definition: tuning.h:350
virtual bool configure(yarp::dev::PolyDriver &driver, const yarp::os::Property &options)
Configure the design.
Definition: tuning.cpp:459
yarp::os::BufferedPort< yarp::sig::Vector > port
Definition: tuning.h:369
virtual ~OnlineCompensatorDesign()
Destructor.
Definition: tuning.cpp:1115
virtual bool startControllerValidation(const yarp::os::Property &options)
Start off the controller validation procedure.
Definition: tuning.cpp:971
yarp::sig::Vector meanParams
Definition: tuning.h:372
yarp::sig::Vector estimate(const double u, const double y)
Estimate the state vector given the current input and the current measurement.
Definition: tuning.cpp:84
yarp::sig::Vector get_parameters() const
Return the system parameters.
Definition: tuning.h:140
bool init(const double Ts, const double Q, const double R, const double P0, const yarp::sig::Vector &x0)
Initialize the estimation.
yarp::sig::Vector get_x() const
Return the estimated state.
Definition: tuning.h:126
yarp::dev::IPidControl * ipid
Definition: tuning.h:161
yarp::sig::Vector stiction
Definition: tuning.h:169
virtual void stopEstimation()
Stop the estimation procedure.
Definition: tuning.h:298
virtual bool waitUntilDone()
Wait until the condition |e_mean|<e_thres is met.
Definition: tuning.cpp:405
virtual bool configure(yarp::dev::PolyDriver &driver, const yarp::os::Property &options)
Configure the estimation.
Definition: tuning.cpp:150
virtual bool getResults(yarp::sig::Vector &results)
Retrieve the estimation.
Definition: tuning.cpp:418
enum iCub::ctrl::OnlineStictionEstimator::@1 state
std::condition_variable cv_doneEvent
Definition: tuning.h:167
virtual bool isDone()
Check the current estimation status.
Definition: tuning.cpp:393
yarp::dev::IControlMode * imod
Definition: tuning.h:158
yarp::dev::ICurrentControl * icur
Definition: tuning.h:163
yarp::dev::IControlLimits * ilim
Definition: tuning.h:159
virtual bool getInfo(yarp::os::Property &info)
Retrieve useful information about the estimation experiment.
Definition: tuning.cpp:430
yarp::dev::IPWMControl * ipwm
Definition: tuning.h:162
virtual bool startEstimation()
Start off the estimation procedure.
Definition: tuning.h:278
yarp::dev::IEncoders * ienc
Definition: tuning.h:160
virtual bool reconfigure(const yarp::os::Property &options)
Reconfigure the estimation after first initialization.
Definition: tuning.cpp:199
double getT() const
Get the trajectory reference time in seconds (90% of steady-state value in t=_T, transient extinguish...
Definition: minJerkCtrl.h:315
bool setT(const double _T)
Set the trajectory reference time (90% of steady-state value in t=_T, transient extinguished for t>=1...
const yarp::sig::Vector & getPos() const
Get the current position.
Definition: minJerkCtrl.h:298
bool setTs(const double _Ts)
Set the sample time.
virtual void init(const yarp::sig::Vector &y0)
Initialize the trajectory.
void computeNextValues(const yarp::sig::Vector &yd)
Compute the next position, velocity and acceleration.
General structure of parallel (non-interactive) PID.
Definition: pids.h:211
virtual void reset(const yarp::sig::Vector &u0)
Resets the internal state of integral and derivative part.
Definition: pids.cpp:247
virtual const yarp::sig::Vector & compute(const yarp::sig::Vector &ref, const yarp::sig::Vector &fb)
Computes the PID output.
Definition: pids.cpp:213
exp(-x3 *T)]
_3f_vect_t acc
Definition: dataTypes.h:1
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
bool done
Definition: main.cpp:42
const FSC max
Definition: strain.h:48
const FSC min
Definition: strain.h:49
A
Definition: sine.m:16