iCub-main
iKinInv.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 <typeinfo>
12 #include <cstdio>
13 #include <cmath>
14 #include <algorithm>
15 
16 #include <yarp/os/Log.h>
17 #include <yarp/os/Time.h>
18 #include <yarp/math/SVD.h>
19 #include <iCub/iKin/iKinInv.h>
20 
21 #define IKINCTRL_INTARGET_TOL 5e-3
22 #define IKINCTRL_WATCHDOG_TOL 1e-4
23 #define IKINCTRL_WATCHDOG_MAXITER 200
24 
25 using namespace std;
26 using namespace yarp::os;
27 using namespace yarp::sig;
28 using namespace yarp::math;
29 using namespace iCub::ctrl;
30 using namespace iCub::iKin;
31 
32 
33 /************************************************************************/
34 iKinCtrl::iKinCtrl(iKinChain &c, unsigned int _ctrlPose) : chain(c)
35 {
36  dim=chain.getDOF();
37 
38  grad.resize(dim,0.0);
39 
40  q_old=q=chain.getAng();
41 
45 
46  iter=0;
47 
49 
50  set_ctrlPose(_ctrlPose);
51 
52  watchDogOn =false;
53  watchDogCnt=0;
54 
55  e.resize(6);
56  x_set.resize(7,0.0);
58 
59  calc_e();
60 }
61 
62 
63 /************************************************************************/
64 void iKinCtrl::set_ctrlPose(unsigned int _ctrlPose)
65 {
66  ctrlPose=_ctrlPose;
67 
70 }
71 
72 
73 /************************************************************************/
74 void iKinCtrl::set_q(const Vector &q0)
75 {
76  unsigned int n=(unsigned int)q0.length();
77  n=n>dim ? dim : n;
78 
79  for (unsigned int i=0; i<n; i++)
80  q[i]=q0[i];
81 
82  q=chain.setAng(q);
84 }
85 
86 
87 /************************************************************************/
89 {
90  // x must be previously set
91  if (x.length()>6)
92  {
93  Matrix H=chain.getH();
94  e=0.0;
95 
97  {
98  e[0]=x_set[0]-H(0,3);
99  e[1]=x_set[1]-H(1,3);
100  e[2]=x_set[2]-H(2,3);
101  }
102 
104  {
105  Matrix Des=axis2dcm(x_set.subVector(3,6));
106  Vector ax=dcm2axis(Des*H.transposed());
107  e[3]=ax[3]*ax[0];
108  e[4]=ax[3]*ax[1];
109  e[5]=ax[3]*ax[2];
110  }
111  }
112  else
113  {
114  e=x_set-x;
116  e[3]=e[4]=e[5]=0.0;
117  else if (ctrlPose==IKINCTRL_POSE_ANG)
118  e[0]=e[1]=e[2]=0.0;
119  }
120 
121  return e;
122 }
123 
124 
125 /************************************************************************/
127 {
129  {
130  if (isInTarget())
131  {
133  watchDogCnt=0;
134  }
135  else if (watchDogOn)
136  watchDog();
137  }
138  else if (state==IKINCTRL_STATE_INTARGET)
139  {
140  if (!isInTarget())
142 
143  watchDogCnt=0;
144  }
145  else if (state==IKINCTRL_STATE_DEADLOCK)
146  {
147  if (isInTarget())
148  {
150  watchDogCnt=0;
151  }
152  else
153  watchDog();
154  }
155 }
156 
157 
158 /************************************************************************/
159 unsigned int iKinCtrl::printHandling(const unsigned int verbose)
160 {
161  unsigned int hi=(verbose>>16) & 0xffff;
162  unsigned int lo=verbose & 0xffff;
163 
164  if (iter % (hi+1))
165  return 0;
166  else
167  return lo;
168 }
169 
170 
171 /************************************************************************/
173 {
174  if (norm(q-q_old)<watchDogTol)
175  watchDogCnt++;
176  else
177  {
179  watchDogCnt=0;
180  }
181 
183  {
185  watchDogCnt=0;
186  }
187 }
188 
189 
190 /************************************************************************/
191 void iKinCtrl::restart(const Vector &q0)
192 {
194  iter=0;
195  set_q(q0);
196 }
197 
198 
199 /************************************************************************/
200 Vector iKinCtrl::checkVelocity(const Vector &_qdot, double _Ts)
201 {
202  Vector checked_qdot=_qdot;
203  Vector newq =q+checked_qdot*_Ts;
204 
205  for (unsigned int i=0; i<dim; i++)
206  if ((newq[i]<chain(i).getMin()) || (newq[i]>chain(i).getMax()))
207  checked_qdot[i]=0.0;
208 
209  return checked_qdot;
210 }
211 
212 
213 /************************************************************************/
214 Vector iKinCtrl::solve(Vector &xd, const double tol_size, const int max_iter,
215  const unsigned int verbose, int *exit_code, bool *exhalt)
216 {
217  while (true)
218  {
219  iterate(xd,verbose);
220 
221  if (isInTarget())
222  {
223  if (exit_code)
224  *exit_code=IKINCTRL_RET_TOLX;
225 
226  break;
227  }
228 
229  if (test_convergence(tol_size))
230  {
231  if (exit_code)
232  *exit_code=IKINCTRL_RET_TOLSIZE;
233 
234  break;
235  }
236 
238  {
239  if (exit_code)
240  *exit_code=IKINCTRL_RET_TOLQ;
241 
242  break;
243  }
244 
245  if (exhalt!=NULL)
246  {
247  if (*exhalt)
248  {
249  if (exit_code)
250  *exit_code=IKINCTRL_RET_EXHALT;
251 
252  break;
253  }
254  }
255 
256  if (max_iter>0 && (int)iter>=max_iter)
257  {
258  if (exit_code)
259  *exit_code=IKINCTRL_RET_MAXITER;
260 
261  break;
262  }
263  }
264 
265  return q;
266 }
267 
268 
269 /************************************************************************/
270 SteepCtrl::SteepCtrl(iKinChain &c, unsigned int _type, unsigned int _ctrlPose,
271  double _Ts, double _Kp) : iKinCtrl(c,_ctrlPose)
272 {
273  type=_type;
274  constrained=true;
275 
276  qdot.resize(dim,0.0);
277 
278  Matrix lim(dim,2);
279  for (unsigned int i=0; i<dim; i++)
280  {
281  lim(i,0)=chain(i).getMin();
282  lim(i,1)=chain(i).getMax();
283  }
284 
285  Ts=_Ts;
286  I=new Integrator(Ts,chain.getAng(),lim);
287 
288  Kp=_Kp;
289 }
290 
291 
292 /************************************************************************/
293 void SteepCtrl::setChainConstraints(bool _constrained)
294 {
295  constrained=_constrained;
296 
299 }
300 
301 
302 /************************************************************************/
304 {
305  qdot=-Kp*grad;
306 
307  return qdot;
308 }
309 
310 
311 /************************************************************************/
312 Vector SteepCtrl::iterate(Vector &xd, const unsigned int verbose)
313 {
314  x_set=xd;
315 
317  {
318  iter++;
319  q_old=q;
320 
321  calc_e();
322 
323  J =chain.GeoJacobian();
324  Jt=J.transposed();
325 
326  if (J.rows()>=J.cols())
327  pinvJ=pinv(J);
328  else
329  pinvJ=pinv(J.transposed()).transposed();
330 
331  if (type==IKINCTRL_STEEP_JT)
332  grad=-1.0*(Jt*e);
333  else
334  grad=-1.0*(pinvJ*e);
335 
336  gpm=computeGPM();
337 
338  Vector _qdot=update_qdot()+gpm;
339 
340  if (constrained)
341  qdot=checkVelocity(_qdot,Ts);
342  else
343  qdot=_qdot;
344 
346  x=chain.EndEffPose();
347  }
348 
349  update_state();
350 
352  inTargetFcn();
353  else if (state==IKINCTRL_STATE_DEADLOCK)
355 
356  printIter(verbose);
357 
358  return q;
359 }
360 
361 
362 /************************************************************************/
363 void SteepCtrl::restart(const Vector &q0)
364 {
365  iKinCtrl::restart(q0);
366  I->reset(q0);
367  qdot=0.0;
368 }
369 
370 
371 /************************************************************************/
372 void SteepCtrl::printIter(const unsigned int verbose)
373 {
374  // This should be the first line of any printIter method
375  unsigned int _verbose=printHandling(verbose);
376 
377  if (_verbose)
378  {
379  string strState[3];
380 
381  strState[IKINCTRL_STATE_RUNNING] ="running";
382  strState[IKINCTRL_STATE_INTARGET]="inTarget";
383  strState[IKINCTRL_STATE_DEADLOCK]="deadLock";
384 
385  printf("iter #%d\n",iter);
386  printf("state = %s\n",strState[state].c_str());
387  printf("norm(e) = %g\n",dist());
388  printf("q = %s\n",(CTRL_RAD2DEG*q).toString().c_str());
389  printf("x = %s\n",x.toString().c_str());
390 
391  if (_verbose>1)
392  {
393  printf("grad = %s\n",grad.toString().c_str());
394  printf("qdot = %s\n",(CTRL_RAD2DEG*qdot).toString().c_str());
395  }
396 
397  if (_verbose>2)
398  printf("Kp = %g\n",Kp);
399 
400  printf("\n");
401  }
402 }
403 
404 
405 /************************************************************************/
407 {
408  delete I;
409 }
410 
411 
412 /************************************************************************/
413 VarKpSteepCtrl::VarKpSteepCtrl(iKinChain &c, unsigned int _type, unsigned int _ctrlPose, double _Ts,
414  double _Kp0, double _Kp_inc, double _Kp_dec, double _Kp_max,
415  double _max_perf_inc) : SteepCtrl(c,_ctrlPose,_type,_Ts,_Kp0)
416 {
417  Kp_inc =_Kp_inc;
418  Kp_dec =_Kp_dec;
419  Kp_max =_Kp_max;
420  max_perf_inc=_max_perf_inc;
421 
422  dist_old=1.0;
423 
424  Kp0=Kp;
425 }
426 
427 
428 /************************************************************************/
430 {
431  Kp=Kp0;
432  dist_old=1.0;
433 }
434 
435 
436 /************************************************************************/
438 {
439  Vector qdot_old=qdot;
441 
442  double d=dist();
443  double ratio=d/dist_old;
444  double Kp_c=1.0;
445 
446  if (ratio>max_perf_inc && !norm(qdot_old))
447  {
448  qdot=qdot_old;
449  Kp_c=Kp_dec;
450  }
451  else if (ratio<1.0)
452  Kp_c=Kp_inc;
453 
454  Kp=Kp_c*Kp;
455 
456  if (Kp>Kp_max)
457  Kp=Kp_max;
458 
459  dist_old=d;
460 
461  return qdot;
462 }
463 
464 
465 /************************************************************************/
466 LMCtrl::LMCtrl(iKinChain &c, unsigned int _ctrlPose, double _Ts, double _mu0,
467  double _mu_inc, double _mu_dec, double _mu_min, double _mu_max,
468  double _sv_thres) : iKinCtrl(c,_ctrlPose)
469 {
470  constrained=true;
471 
472  qdot.resize(dim,0.0);
473 
474  Matrix lim(dim,2);
475  for (unsigned int i=0; i<dim; i++)
476  {
477  lim(i,0)=chain(i).getMin();
478  lim(i,1)=chain(i).getMax();
479  }
480 
481  Ts=_Ts;
482  I=new Integrator(Ts,chain.getAng(),lim);
483 
484  mu =_mu0;
485  mu0 =_mu0;
486  mu_inc=_mu_inc;
487  mu_dec=_mu_dec;
488  mu_min=_mu_min;
489  mu_max=_mu_max;
490  svThres=_sv_thres;
491 
492  dist_old=1.0;
493 }
494 
495 
496 /************************************************************************/
497 void LMCtrl::setChainConstraints(bool _constrained)
498 {
499  constrained=_constrained;
500 
503 }
504 
505 
506 /************************************************************************/
507 Matrix LMCtrl::pinv(const Matrix &A, const double tol)
508 {
509  int m=(int)A.rows();
510  int n=(int)A.cols();
511  Matrix U(m,n);
512  Vector Sdiag(n);
513  Matrix V(n,n);
514 
515  SVD(A,U,Sdiag,V);
516 
517  Matrix Spinv=zeros(n,n);
518  for (int c=0; c<n; c++)
519  {
520  for (int r=0; r<n; r++)
521  {
522  if (r==c && Sdiag[c]>tol)
523  Spinv(r,c)=1.0/Sdiag[c];
524  else
525  Spinv(r,c)=0.0;
526  }
527  }
528 
529  svMin=Sdiag[n-1];
530  return V*Spinv*U.transposed();
531 }
532 
533 
534 /************************************************************************/
536 {
537  mu=mu0;
538  dist_old=1.0;
539 }
540 
541 
542 /************************************************************************/
544 {
545  if (svMin>svThres)
546  {
547  double d=dist();
548  double ratio=d/dist_old;
549 
550  if (ratio>1.0)
551  mu*=mu_inc;
552  else
553  mu*=mu_dec;
554 
555  mu=mu>mu_max ? mu_max : (mu<mu_min ? mu_min : mu);
556 
557  dist_old=d;
558  }
559  else
560  {
561  mu=mu_max;
562  dist_old=1.0;
563  }
564 
565  return mu;
566 }
567 
568 
569 /************************************************************************/
570 Vector LMCtrl::iterate(Vector &xd, const unsigned int verbose)
571 {
572  x_set=xd;
573 
575  {
576  iter++;
577  q_old=q;
578 
579  calc_e();
580 
581  J=chain.GeoJacobian();
582  Jt=J.transposed();
583  grad=-1.0*(Jt*e);
584 
585  Matrix LM=J*Jt;
586  for (int i=0; i<6; i++)
587  LM(i,i)+=mu*LM(i,i);
588 
589  if (LM.rows()>=LM.cols())
590  pinvLM=Jt*pinv(LM);
591  else
592  pinvLM=Jt*pinv(LM.transposed()).transposed();
593 
594  if (J.rows()>=J.cols())
596  else
597  pinvJ=LMCtrl::pinv(J.transposed()).transposed();
598 
599  gpm=computeGPM();
600 
601  Vector _qdot=pinvLM*e+gpm;
602 
603  if (constrained)
604  qdot=checkVelocity(_qdot,Ts);
605  else
606  qdot=_qdot;
607 
609  x=chain.EndEffPose();
610 
611  mu=update_mu();
612  }
613 
614  update_state();
615 
617  inTargetFcn();
618  else if (state==IKINCTRL_STATE_DEADLOCK)
620 
621  printIter(verbose);
622 
623  return q;
624 }
625 
626 
627 /************************************************************************/
628 void LMCtrl::restart(const Vector &q0)
629 {
630  iKinCtrl::restart(q0);
631  I->reset(q0);
632  qdot=0.0;
633  reset_mu();
634 }
635 
636 
637 /************************************************************************/
638 void LMCtrl::printIter(const unsigned int verbose)
639 {
640  // This should be the first line of any printIter method
641  unsigned int _verbose=printHandling(verbose);
642 
643  if (_verbose)
644  {
645  string strState[3];
646 
647  strState[IKINCTRL_STATE_RUNNING] ="running";
648  strState[IKINCTRL_STATE_INTARGET]="inTarget";
649  strState[IKINCTRL_STATE_DEADLOCK]="deadLock";
650 
651  printf("iter #%d\n",iter);
652  printf("state = %s\n",strState[state].c_str());
653  printf("norm(e) = %g\n",dist());
654  printf("q = %s\n",(CTRL_RAD2DEG*q).toString().c_str());
655  printf("x = %s\n",x.toString().c_str());
656 
657  if (_verbose>1)
658  printf("grad = %s\n",grad.toString().c_str());
659 
660  if (_verbose>2)
661  printf("mu = %g\n",mu);
662 
663  printf("\n");
664  }
665 }
666 
667 
668 /************************************************************************/
670 {
671  delete I;
672 }
673 
674 
675 /************************************************************************/
676 LMCtrl_GPM::LMCtrl_GPM(iKinChain &c, unsigned int _ctrlPose, double _Ts,
677  double _mu0, double _mu_inc, double _mu_dec,
678  double _mu_min, double _mu_max, double _sv_thres) :
679  LMCtrl(c,_ctrlPose,_Ts,_mu0,_mu_inc,_mu_dec,_mu_min,_mu_max,_sv_thres)
680 {
681  span.resize(dim);
682  alpha_min.resize(dim);
683  alpha_max.resize(dim);
684  Eye=eye(dim,dim);
685 
686  set_safeAreaRatio(0.9);
687  K=1.0;
688 }
689 
690 
691 /************************************************************************/
692 void LMCtrl_GPM::set_safeAreaRatio(const double _safeAreaRatio)
693 {
694  safeAreaRatio=_safeAreaRatio<0.0 ? 0.0 : (_safeAreaRatio>1.0 ? 1.0 : _safeAreaRatio);
695 
696  for (unsigned int i=0; i<dim; i++)
697  {
698  span[i]=chain(i).getMax()-chain(i).getMin();
699 
700  double alpha=0.5*(1.0-safeAreaRatio)*span[i];
701  alpha_min[i]=chain(i).getMin()+alpha;
702  alpha_max[i]=chain(i).getMax()-alpha;
703  }
704 }
705 
706 
707 /************************************************************************/
709 {
710  Vector w(dim);
711  Vector d_min=q-alpha_min;
712  Vector d_max=q-alpha_max;
713 
714  for (unsigned int i=0; i<dim; i++)
715  {
716  w[i] =d_min[i]>0.0 ? 0.0 : 2.0*d_min[i]/(span[i]*span[i]);
717  w[i]+=d_max[i]<0.0 ? 0.0 : 2.0*d_max[i]/(span[i]*span[i]);
718  }
719 
720  return (Eye-pinvLM*J)*((-K)*w);
721 }
722 
723 
724 /************************************************************************/
725 MultiRefMinJerkCtrl::MultiRefMinJerkCtrl(iKinChain &c, unsigned int _ctrlPose, double _Ts,
726  bool nonIdealPlant) :
727  iKinCtrl(c,_ctrlPose), Ts(_Ts)
728 {
729  q_set.resize(dim,0.0);
730  qdot.resize(dim,0.0);
731  xdot.resize(6,0.0);
732  compensation.resize(dim,0.0);
733 
734  W=eye(dim,dim);
735  Eye6=eye(6,6);
736 
737  execTime=1.0;
738 
739  Matrix lim(dim,2);
740  for (unsigned int i=0; i<dim; i++)
741  {
742  lim(i,0)=chain(i).getMin();
743  lim(i,1)=chain(i).getMax();
744  }
745 
746  if (nonIdealPlant)
748  else
750 
751  mjCtrlTask=new minJerkVelCtrlForIdealPlant(Ts,(int)e.length());
752  I=new Integrator(Ts,q,lim);
753 
754  gamma=0.05;
755  guardRatio=0.1;
756 
757  qGuard.resize(dim);
758  qGuardMinInt.resize(dim);
759  qGuardMinExt.resize(dim);
760  qGuardMaxInt.resize(dim);
761  qGuardMaxExt.resize(dim);
762  qGuardMinCOG.resize(dim);
763  qGuardMaxCOG.resize(dim);
764 
765  computeGuard();
766 }
767 
768 
769 /************************************************************************/
770 void MultiRefMinJerkCtrl::set_guardRatio(double _guardRatio)
771 {
772  guardRatio=_guardRatio>1.0 ? 1.0 : (_guardRatio<0.0 ? 0.0 : _guardRatio);
773 
774  computeGuard();
775 }
776 
777 
778 /************************************************************************/
780 {
781  for (unsigned int i=0; i<dim; i++)
782  {
783  qGuard[i]=0.25*guardRatio*(chain(i).getMax()-chain(i).getMin());
784 
785  qGuardMinExt[i]=chain(i).getMin()+qGuard[i];
786  qGuardMinInt[i]=qGuardMinExt[i] +qGuard[i];
787  qGuardMinCOG[i]=0.5*(qGuardMinExt[i]+qGuardMinInt[i]);
788 
789  qGuardMaxExt[i]=chain(i).getMax()-qGuard[i];
790  qGuardMaxInt[i]=qGuardMaxExt[i] -qGuard[i];
791  qGuardMaxCOG[i]=0.5*(qGuardMaxExt[i]+qGuardMaxInt[i]);
792  }
793 }
794 
795 
796 /************************************************************************/
798 {
799  for (unsigned int i=0; i<dim; i++)
800  {
801  if ((q[i]>=qGuardMinInt[i]) && (q[i]<=qGuardMaxInt[i]))
802  W(i,i)=gamma;
803  else if ((q[i]<=qGuardMinExt[i]) || (q[i]>=qGuardMaxExt[i]))
804  W(i,i)=0.0;
805  else if (q[i]<qGuardMinInt[i])
806  W(i,i)=0.5*gamma*(1.0+tanh(+10.0*(q[i]-qGuardMinCOG[i])/qGuard[i]));
807  else
808  W(i,i)=0.5*gamma*(1.0+tanh(-10.0*(q[i]-qGuardMaxCOG[i])/qGuard[i]));
809  }
810 }
811 
812 
813 /************************************************************************/
814 Vector MultiRefMinJerkCtrl::iterate(Vector &xd, Vector &qd, Vector *xdot_set,
815  const unsigned int verbose)
816 {
817  x_set=xd;
818  q_set=qd;
819 
821  {
822  iter++;
823  q_old=q;
824 
825  calc_e();
826 
828 
829  Vector _xdot;
830  if (xdot_set!=NULL)
831  {
832  _xdot.resize(6);
833  _xdot[0]=(*xdot_set)[0];
834  _xdot[1]=(*xdot_set)[1];
835  _xdot[2]=(*xdot_set)[2];
836  _xdot[3]=(*xdot_set)[3]*(*xdot_set)[6];
837  _xdot[4]=(*xdot_set)[4]*(*xdot_set)[6];
838  _xdot[5]=(*xdot_set)[5]*(*xdot_set)[6];
839  }
840  else
842 
843  J =chain.GeoJacobian();
844  Jt=J.transposed();
845 
846  computeWeight();
847 
848  qdot=_qdot+W*(Jt*(pinv(Eye6+J*W*Jt)*(_xdot-J*_qdot)));
849  xdot=J*qdot;
851  x=chain.EndEffPose();
852  }
853 
854  update_state();
855 
857  inTargetFcn();
858  else if (state==IKINCTRL_STATE_DEADLOCK)
860 
861  printIter(verbose);
862 
863  compensation=0.0;
864 
865  return q;
866 }
867 
868 
869 /************************************************************************/
870 Vector MultiRefMinJerkCtrl::iterate(Vector &xd, Vector &qd, const unsigned int verbose)
871 {
872  return iterate(xd,qd,NULL,verbose);
873 }
874 
875 
876 /************************************************************************/
877 Vector MultiRefMinJerkCtrl::iterate(Vector &xd, Vector &qd, Vector &xdot_set,
878  const unsigned int verbose)
879 {
880  return iterate(xd,qd,&xdot_set,verbose);
881 }
882 
883 
884 /************************************************************************/
885 void MultiRefMinJerkCtrl::restart(const Vector &q0)
886 {
887  iKinCtrl::restart(q0);
888 
889  qdot=0.0;
890  xdot=0.0;
891 
893  mjCtrlTask->reset(zeros((int)e.length()));
894 }
895 
896 
897 /************************************************************************/
898 void MultiRefMinJerkCtrl::printIter(const unsigned int verbose)
899 {
900  // This should be the first line of any printIter method
901  unsigned int _verbose=printHandling(verbose);
902 
903  if (_verbose)
904  {
905  string strState[3];
906 
907  strState[IKINCTRL_STATE_RUNNING] ="running";
908  strState[IKINCTRL_STATE_INTARGET]="inTarget";
909  strState[IKINCTRL_STATE_DEADLOCK]="deadLock";
910 
911  printf("iter #%d\n",iter);
912  printf("state = %s\n",strState[state].c_str());
913  printf("norm(e) = %g\n",dist());
914  printf("xd = %s\n",x_set.toString().c_str());
915  printf("x = %s\n",x.toString().c_str());
916  printf("qd = %s\n",(CTRL_RAD2DEG*q_set).toString().c_str());
917  printf("q = %s\n",(CTRL_RAD2DEG*q).toString().c_str());
918 
919  if (_verbose>1)
920  {
921  printf("qdot = %s\n",(CTRL_RAD2DEG*qdot).toString().c_str());
922  printf("xdot = %s\n",xdot.toString().c_str());
923  }
924 
925  if (_verbose>2)
926  printf("comp = %s\n",compensation.toString().c_str());
927 
928  printf("\n");
929  }
930 }
931 
932 
933 /************************************************************************/
934 void MultiRefMinJerkCtrl::set_q(const Vector &q0)
935 {
936  iKinCtrl::set_q(q0);
937  I->reset(q);
938 }
939 
940 
941 /************************************************************************/
942 double MultiRefMinJerkCtrl::set_execTime(const double _execTime, const bool warn)
943 {
944  double lowerThres=10.0*Ts;
945 
946  execTime=_execTime>lowerThres ? _execTime : lowerThres;
947 
948  if (warn && (execTime!=_execTime))
949  yWarning("task execution time limited to the lower bound %g",lowerThres);
950 
951  return execTime;
952 }
953 
954 
955 /************************************************************************/
957 {
958  size_t len=std::min(comp.length(),q.length());
959  for (size_t i=0; i<len; i++)
960  compensation[i]=comp[i];
961 }
962 
963 
964 /************************************************************************/
965 void MultiRefMinJerkCtrl::setPlantParameters(const Property &parameters,
966  const string &entryTag)
967 {
968  if (typeid(*mjCtrlJoint)!=typeid(minJerkVelCtrlForNonIdealPlant))
969  {
970  delete mjCtrlJoint;
972  }
973 
974  Bottle ordering;
975  for (unsigned int i=0; i<chain.getN(); i++)
976  if (!chain[i].isBlocked())
977  ordering.addInt32(i);
978 
979  dynamic_cast<minJerkVelCtrlForNonIdealPlant*>(mjCtrlJoint)->setPlantParameters(parameters,entryTag,ordering);
980 }
981 
982 
983 /************************************************************************/
985 {
986  delete mjCtrlJoint;
987  delete mjCtrlTask;
988  delete I;
989 }
990 
991 
992 
iCub::iKin::iKinCtrl::watchDogOn
bool watchDogOn
Definition: iKinInv.h:97
iCub::ctrl::Integrator::integrate
const yarp::sig::Vector & integrate(const yarp::sig::Vector &x)
Executes one-step integration of input vector.
Definition: pids.cpp:115
IKINCTRL_STATE_INTARGET
#define IKINCTRL_STATE_INTARGET
Definition: iKinInv.h:34
iCub::iKin::LMCtrl::mu_dec
double mu_dec
Definition: iKinInv.h:599
iCub::iKin::MultiRefMinJerkCtrl::qGuardMinExt
yarp::sig::Vector qGuardMinExt
Definition: iKinInv.h:786
iCub::iKin::iKinCtrl::chain
iKinChain & chain
Definition: iKinInv.h:75
iCub::iKin::MultiRefMinJerkCtrl::mjCtrlJoint
ctrl::minJerkVelCtrl * mjCtrlJoint
Definition: iKinInv.h:770
IKINCTRL_POSE_XYZ
#define IKINCTRL_POSE_XYZ
Definition: iKinInv.h:38
IKINCTRL_RET_TOLSIZE
#define IKINCTRL_RET_TOLSIZE
Definition: iKinInv.h:45
IKINCTRL_STATE_RUNNING
#define IKINCTRL_STATE_RUNNING
Definition: iKinInv.h:33
iCub::ctrl::Integrator
Definition: pids.h:47
iCub::iKin::iKinCtrl::calc_e
virtual yarp::sig::Vector calc_e()
Computes the error according to the current controller settings (complete pose/translational/rotation...
Definition: iKinInv.cpp:88
iCub::iKin::VarKpSteepCtrl::Kp_max
double Kp_max
Definition: iKinInv.h:526
IKINCTRL_RET_TOLX
#define IKINCTRL_RET_TOLX
Definition: iKinInv.h:44
H
H
Definition: compute_ekf_fast.m:27
iCub::ctrl::minJerkVelCtrlForNonIdealPlant
Definition: minJerkCtrl.h:135
iCub::iKin::VarKpSteepCtrl::dist_old
double dist_old
Definition: iKinInv.h:529
iCub::iKin::iKinCtrl::inTargetTol
double inTargetTol
Definition: iKinInv.h:89
iCub::iKin::LMCtrl_GPM::Eye
yarp::sig::Matrix Eye
Definition: iKinInv.h:712
iCub::iKin::LMCtrl::update_mu
virtual double update_mu()
Definition: iKinInv.cpp:543
iCub::iKin::LMCtrl
Definition: iKinInv.h:576
iCub::iKin::iKinCtrl::watchDogTol
double watchDogTol
Definition: iKinInv.h:90
IKINCTRL_STATE_DEADLOCK
#define IKINCTRL_STATE_DEADLOCK
Definition: iKinInv.h:35
iCub::iKin::MultiRefMinJerkCtrl::inTargetFcn
virtual void inTargetFcn()
Method called whenever in target.
Definition: iKinInv.h:796
iCub::iKin::LMCtrl::mu_min
double mu_min
Definition: iKinInv.h:600
iCub::iKin::LMCtrl::svMin
double svMin
Definition: iKinInv.h:604
iCub::iKin::SteepCtrl::computeGPM
virtual yarp::sig::Vector computeGPM()
Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection...
Definition: iKinInv.h:464
iKinInv.h
iCub::iKin::MultiRefMinJerkCtrl::set_execTime
double set_execTime(const double _execTime, const bool warn=false)
Sets the task execution time in seconds.
Definition: iKinInv.cpp:942
IKINCTRL_INTARGET_TOL
#define IKINCTRL_INTARGET_TOL
Definition: iKinInv.cpp:21
iCub::iKin::MultiRefMinJerkCtrl::qdot
yarp::sig::Vector qdot
Definition: iKinInv.h:775
iCub::iKin::MultiRefMinJerkCtrl::Eye6
yarp::sig::Matrix Eye6
Definition: iKinInv.h:778
iCub::iKin::MultiRefMinJerkCtrl::iterate
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, yarp::sig::Vector &qd, yarp::sig::Vector *xdot_set, const unsigned int verbose)
iCub::iKin::LMCtrl_GPM::computeGPM
virtual yarp::sig::Vector computeGPM()
Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection...
Definition: iKinInv.cpp:708
iCub::iKin::LMCtrl::I
ctrl::Integrator * I
Definition: iKinInv.h:588
IKINCTRL_POSE_ANG
#define IKINCTRL_POSE_ANG
Definition: iKinInv.h:39
iCub::iKin::iKinChain
Definition: iKinFwd.h:354
iCub::iKin::LMCtrl::deadLockRecoveryFcn
virtual void deadLockRecoveryFcn()
Method called whenever the watchDog is triggered.
Definition: iKinInv.h:609
iCub::iKin::MultiRefMinJerkCtrl::set_guardRatio
void set_guardRatio(double _guardRatio)
Sets the guard ratio (in [0 1]).
Definition: iKinInv.cpp:770
iCub::iKin::SteepCtrl::restart
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
Definition: iKinInv.cpp:363
iCub::iKin::iKinCtrl::setChainConstraints
virtual void setChainConstraints(bool _constrained)
Enables/Disables joint angles constraints.
Definition: iKinInv.h:183
iCub::iKin::iKinCtrl::dist
virtual double dist() const
Returns the actual distance from the target in cartesian space (euclidean norm is used).
Definition: iKinInv.h:394
iCub::iKin::MultiRefMinJerkCtrl::compensation
yarp::sig::Vector compensation
Definition: iKinInv.h:789
iCub::iKin::MultiRefMinJerkCtrl::W
yarp::sig::Matrix W
Definition: iKinInv.h:777
iCub::iKin::MultiRefMinJerkCtrl::xdot
yarp::sig::Vector xdot
Definition: iKinInv.h:776
iCub::iKin::iKinCtrl::ctrlPose
unsigned int ctrlPose
Definition: iKinInv.h:76
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
iCub::iKin::LMCtrl::pinvLM
yarp::sig::Matrix pinvLM
Definition: iKinInv.h:594
iCub::iKin::iKinCtrl::e
yarp::sig::Vector e
Definition: iKinInv.h:80
iCub::iKin::SteepCtrl::Ts
double Ts
Definition: iKinInv.h:422
iCub::iKin::iKinChain::getH
yarp::sig::Matrix getH(const unsigned int i, const bool allLink=false)
Returns the rigid roto-translation matrix from the root reference frame to the ith frame in Denavit-H...
Definition: iKinFwd.cpp:740
iCub::iKin::SteepCtrl::iterate
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, const unsigned int verbose=0)
Executes one iteration of the control algorithm.
Definition: iKinInv.cpp:312
iCub::iKin::iKinChain::getDOF
unsigned int getDOF() const
Returns the current number of Chain's DOF.
Definition: iKinFwd.h:557
iCub::iKin::MultiRefMinJerkCtrl::I
ctrl::Integrator * I
Definition: iKinInv.h:772
iCub::iKin::LMCtrl::svThres
double svThres
Definition: iKinInv.h:605
iCub::iKin::iKinCtrl::solve
virtual yarp::sig::Vector solve(yarp::sig::Vector &xd, const double tol_size=IKINCTRL_DISABLED, const int max_iter=IKINCTRL_DISABLED, const unsigned int verbose=0, int *exit_code=NULL, bool *exhalt=NULL)
Iterates the control algorithm trying to converge on the target.
Definition: iKinInv.cpp:214
iCub::iKin::LMCtrl::pinv
virtual yarp::sig::Matrix pinv(const yarp::sig::Matrix &A, const double tol=0.0)
Definition: iKinInv.cpp:507
iCub::iKin::iKinChain::GeoJacobian
yarp::sig::Matrix GeoJacobian(const unsigned int i)
Returns the geometric Jacobian of the ith link.
Definition: iKinFwd.cpp:1020
iCub::iKin::iKinCtrl::watchDogCnt
int watchDogCnt
Definition: iKinInv.h:98
iCub::iKin::LMCtrl::gpm
yarp::sig::Vector gpm
Definition: iKinInv.h:593
iCub::iKin::MultiRefMinJerkCtrl::printIter
virtual void printIter(const unsigned int verbose)
Dumps warning or status messages.
Definition: iKinInv.cpp:898
state
state
Definition: WholeBodyPlayerModule.h:64
iCub::iKin::LMCtrl::~LMCtrl
virtual ~LMCtrl()
Destructor.
Definition: iKinInv.cpp:669
iCub::iKin::LMCtrl::reset_mu
void reset_mu()
Sets the weighting factor mu equal to the initial value.
Definition: iKinInv.cpp:535
iCub::iKin::LMCtrl::restart
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
Definition: iKinInv.cpp:628
iCub::iKin::iKinChain::setAng
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
iCub::iKin::SteepCtrl::deadLockRecoveryFcn
virtual void deadLockRecoveryFcn()
Method called whenever the watchDog is triggered.
Definition: iKinInv.h:433
iCub::iKin::LMCtrl_GPM::K
double K
Definition: iKinInv.h:707
iCub::iKin::SteepCtrl
Definition: iKinInv.h:411
iCub::skinManager::toString
std::string toString(const T &t)
Definition: compensator.h:199
iCub::iKin::MultiRefMinJerkCtrl::qGuardMinCOG
yarp::sig::Vector qGuardMinCOG
Definition: iKinInv.h:786
iCub::ctrl::Integrator::setSaturation
void setSaturation(bool _applySat)
Sets the saturation status.
Definition: pids.cpp:87
IKINCTRL_WATCHDOG_MAXITER
#define IKINCTRL_WATCHDOG_MAXITER
Definition: iKinInv.cpp:23
iCub::iKin::LMCtrl_GPM::alpha_max
yarp::sig::Vector alpha_max
Definition: iKinInv.h:711
iCub::iKin::iKinCtrl::update_state
virtual void update_state()
Updates the control state.
Definition: iKinInv.cpp:126
iCub::iKin::iKinChain::getAng
yarp::sig::Vector getAng()
Returns the current free joint angles values.
Definition: iKinFwd.cpp:619
CTRL_RAD2DEG
#define CTRL_RAD2DEG
Definition: XSensMTx.cpp:25
iCub::iKin::MultiRefMinJerkCtrl::qGuardMaxInt
yarp::sig::Vector qGuardMaxInt
Definition: iKinInv.h:787
iCub::iKin::iKinCtrl::isInTarget
virtual bool isInTarget()
Checks if the End-Effector is in target.
Definition: iKinInv.h:315
iCub::iKin::SteepCtrl::gpm
yarp::sig::Vector gpm
Definition: iKinInv.h:430
iCub::iKin::LMCtrl_GPM::set_safeAreaRatio
void set_safeAreaRatio(const double _safeAreaRatio)
Sets the safe area ratio [0-1], which is for each joint the ratio between the angle span within which...
Definition: iKinInv.cpp:692
iCub::ctrl::minJerkVelCtrl::computeCmd
virtual yarp::sig::Vector computeCmd(const double _T, const yarp::sig::Vector &e)=0
Computes the velocity command.
iCub::iKin
Definition: iKinFwd.h:71
iCub::iKin::iKinCtrl::iterate
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, const unsigned int verbose=0)=0
Executes one iteration of the control algorithm.
iCub::iKin::VarKpSteepCtrl::max_perf_inc
double max_perf_inc
Definition: iKinInv.h:527
iCub::iKin::iKinCtrl::x_set
yarp::sig::Vector x_set
Definition: iKinInv.h:78
iCub::ctrl::Integrator::reset
void reset(const yarp::sig::Vector &y0)
Resets the internal state and sets the output vector to the given value.
Definition: pids.cpp:128
iCub::iKin::SteepCtrl::I
ctrl::Integrator * I
Definition: iKinInv.h:423
iCub::iKin::LMCtrl::printIter
virtual void printIter(const unsigned int verbose)
Dumps warning or status messages.
Definition: iKinInv.cpp:638
iCub::iKin::iKinCtrl::x
yarp::sig::Vector x
Definition: iKinInv.h:79
iCub::iKin::MultiRefMinJerkCtrl::qGuardMaxExt
yarp::sig::Vector qGuardMaxExt
Definition: iKinInv.h:787
iCub::iKin::LMCtrl::mu_max
double mu_max
Definition: iKinInv.h:601
iCub::iKin::LMCtrl::Ts
double Ts
Definition: iKinInv.h:587
iCub::iKin::VarKpSteepCtrl::Kp_inc
double Kp_inc
Definition: iKinInv.h:524
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
iCub::iKin::iKinCtrl::q
yarp::sig::Vector q
Definition: iKinInv.h:81
iCub::iKin::MultiRefMinJerkCtrl::execTime
double execTime
Definition: iKinInv.h:781
n
int n
Definition: debugFunctions.cpp:58
iCub::iKin::iKinCtrl::q_old
yarp::sig::Vector q_old
Definition: iKinInv.h:87
iCub::iKin::MultiRefMinJerkCtrl::gamma
double gamma
Definition: iKinInv.h:782
iCub::iKin::iKinCtrl::watchDog
virtual void watchDog()
Handles the watchDog.
Definition: iKinInv.cpp:172
iCub::iKin::LMCtrl::constrained
bool constrained
Definition: iKinInv.h:590
iCub::ctrl::norm
double norm(const yarp::sig::Matrix &M, int col)
iCub::iKin::iKinCtrl::pinvJ
yarp::sig::Matrix pinvJ
Definition: iKinInv.h:84
iCub::iKin::LMCtrl::qdot
yarp::sig::Vector qdot
Definition: iKinInv.h:592
iCub::ctrl::minJerkVelCtrl::reset
virtual void reset(const yarp::sig::Vector &u0)=0
Resets the controller to a given value.
iCub::iKin::MultiRefMinJerkCtrl::deadLockRecoveryFcn
virtual void deadLockRecoveryFcn()
Method called whenever the watchDog is triggered.
Definition: iKinInv.h:797
iCub::iKin::SteepCtrl::qdot
yarp::sig::Vector qdot
Definition: iKinInv.h:429
iCub::iKin::SteepCtrl::type
unsigned int type
Definition: iKinInv.h:425
iCub::iKin::MultiRefMinJerkCtrl::Ts
double Ts
Definition: iKinInv.h:780
iCub::iKin::iKinCtrl::Jt
yarp::sig::Matrix Jt
Definition: iKinInv.h:83
iCub::iKin::iKinCtrl::restart
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
Definition: iKinInv.cpp:191
iCub::iKin::MultiRefMinJerkCtrl::qGuardMinInt
yarp::sig::Vector qGuardMinInt
Definition: iKinInv.h:786
iCub::iKin::MultiRefMinJerkCtrl::computeWeight
virtual void computeWeight()
Definition: iKinInv.cpp:797
iCub::iKin::LMCtrl::dist_old
double dist_old
Definition: iKinInv.h:603
iCub::iKin::SteepCtrl::constrained
bool constrained
Definition: iKinInv.h:426
iCub::iKin::SteepCtrl::~SteepCtrl
virtual ~SteepCtrl()
Destructor.
Definition: iKinInv.cpp:406
iCub::iKin::MultiRefMinJerkCtrl::~MultiRefMinJerkCtrl
virtual ~MultiRefMinJerkCtrl()
Destructor.
Definition: iKinInv.cpp:984
iCub::iKin::VarKpSteepCtrl::update_qdot
virtual yarp::sig::Vector update_qdot()
Definition: iKinInv.cpp:437
iCub::iKin::iKinCtrl::grad
yarp::sig::Vector grad
Definition: iKinInv.h:85
iCub::iKin::SteepCtrl::printIter
virtual void printIter(const unsigned int verbose)
Dumps warning or status messages.
Definition: iKinInv.cpp:372
IKINCTRL_WATCHDOG_TOL
#define IKINCTRL_WATCHDOG_TOL
Definition: iKinInv.cpp:22
iCub::iKin::MultiRefMinJerkCtrl::q_set
yarp::sig::Vector q_set
Definition: iKinInv.h:774
iCub::iKin::iKinCtrl::set_ctrlPose
void set_ctrlPose(unsigned int _ctrlPose)
Sets the state of Pose control settings.
Definition: iKinInv.cpp:64
iCub::iKin::iKinCtrl::printHandling
unsigned int printHandling(const unsigned int verbose=0)
Method to be called within the printIter routine inherited by children in order to handle the highest...
Definition: iKinInv.cpp:159
iCub::iKin::LMCtrl::iterate
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, const unsigned int verbose=0)
Executes one iteration of the control algorithm.
Definition: iKinInv.cpp:570
A
A
Definition: sine.m:16
iCub::iKin::VarKpSteepCtrl::reset_Kp
void reset_Kp()
Definition: iKinInv.cpp:429
iCub::iKin::LMCtrl_GPM::alpha_min
yarp::sig::Vector alpha_min
Definition: iKinInv.h:710
IKINCTRL_RET_EXHALT
#define IKINCTRL_RET_EXHALT
Definition: iKinInv.h:48
iCub::iKin::MultiRefMinJerkCtrl::qGuardMaxCOG
yarp::sig::Vector qGuardMaxCOG
Definition: iKinInv.h:787
iCub::iKin::SteepCtrl::inTargetFcn
virtual void inTargetFcn()
Method called whenever in target.
Definition: iKinInv.h:432
iCub::iKin::VarKpSteepCtrl::Kp0
double Kp0
Definition: iKinInv.h:523
iCub::iKin::MultiRefMinJerkCtrl::guardRatio
double guardRatio
Definition: iKinInv.h:783
iCub::iKin::iKinCtrl::set_q
virtual void set_q(const yarp::sig::Vector &q0)
Sets the joint angles values.
Definition: iKinInv.cpp:74
iCub::iKin::LMCtrl::mu0
double mu0
Definition: iKinInv.h:597
iCub::iKin::LMCtrl::mu
double mu
Definition: iKinInv.h:596
iCub::ctrl::minJerkVelCtrlForIdealPlant
Definition: minJerkCtrl.h:81
iCub::iKin::MultiRefMinJerkCtrl::mjCtrlTask
ctrl::minJerkVelCtrl * mjCtrlTask
Definition: iKinInv.h:771
iCub::iKin::MultiRefMinJerkCtrl::computeGuard
virtual void computeGuard()
Definition: iKinInv.cpp:779
iCub::iKin::iKinChain::EndEffPose
yarp::sig::Vector EndEffPose(const bool axisRep=true)
Returns the coordinates of end-effector.
Definition: iKinFwd.cpp:858
iCub::iKin::iKinCtrl::test_convergence
virtual bool test_convergence(const double tol_size)=0
Tests convergence by comparing the size of the algorithm internal structure (may be the gradient norm...
iCub::iKin::MultiRefMinJerkCtrl::setPlantParameters
void setPlantParameters(const yarp::os::Property &parameters, const std::string &entryTag="dimension")
Allows user to assign values to the parameters of plant under control (for the configuration space on...
Definition: iKinInv.cpp:965
IKINCTRL_RET_MAXITER
#define IKINCTRL_RET_MAXITER
Definition: iKinInv.h:47
iCub::iKin::LMCtrl::setChainConstraints
virtual void setChainConstraints(bool _constrained)
Enables/Disables joint angles constraints.
Definition: iKinInv.cpp:497
iCub::iKin::MultiRefMinJerkCtrl::qGuard
yarp::sig::Vector qGuard
Definition: iKinInv.h:785
iCub::iKin::LMCtrl::inTargetFcn
virtual void inTargetFcn()
Method called whenever in target.
Definition: iKinInv.h:608
iCub::iKin::SteepCtrl::setChainConstraints
virtual void setChainConstraints(bool _constrained)
Enables/Disables joint angles constraints.
Definition: iKinInv.cpp:293
iCub::iKin::iKinCtrl::iter
unsigned int iter
Definition: iKinInv.h:93
zeros
zeros(2, 2) eye(2
iCub::iKin::iKinCtrl::J
yarp::sig::Matrix J
Definition: iKinInv.h:82
iCub::iKin::iKinCtrl::checkVelocity
virtual yarp::sig::Vector checkVelocity(const yarp::sig::Vector &_qdot, double _Ts)
Checks each joint velocity and sets it to zero if it steers the joint out of range.
Definition: iKinInv.cpp:200
IKINCTRL_STEEP_JT
#define IKINCTRL_STEEP_JT
Definition: iKinInv.h:41
iCub::iKin::LMCtrl::mu_inc
double mu_inc
Definition: iKinInv.h:598
iCub::iKin::iKinCtrl::watchDogMaxIter
int watchDogMaxIter
Definition: iKinInv.h:99
iCub::iKin::SteepCtrl::Kp
double Kp
Definition: iKinInv.h:428
IKINCTRL_RET_TOLQ
#define IKINCTRL_RET_TOLQ
Definition: iKinInv.h:46
iCub::iKin::iKinCtrl::dim
unsigned int dim
Definition: iKinInv.h:92
iCub::iKin::LMCtrl_GPM::span
yarp::sig::Vector span
Definition: iKinInv.h:709
iCub::iKin::MultiRefMinJerkCtrl::set_q
virtual void set_q(const yarp::sig::Vector &q0)
Sets the joint angles values.
Definition: iKinInv.cpp:934
iCub::iKin::VarKpSteepCtrl::Kp_dec
double Kp_dec
Definition: iKinInv.h:525
iCub::iKin::SteepCtrl::update_qdot
virtual yarp::sig::Vector update_qdot()
Definition: iKinInv.cpp:303
iCub::iKin::LMCtrl_GPM::safeAreaRatio
double safeAreaRatio
Definition: iKinInv.h:706
iCub::iKin::iKinCtrl
Definition: iKinInv.h:64
iCub::iKin::MultiRefMinJerkCtrl::add_compensation
void add_compensation(const yarp::sig::Vector &comp)
Adds to the controller input a further compensation term.
Definition: iKinInv.cpp:956
iCub::iKin::LMCtrl::computeGPM
virtual yarp::sig::Vector computeGPM()
Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection...
Definition: iKinInv.h:645
iCub::iKin::MultiRefMinJerkCtrl::restart
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
Definition: iKinInv.cpp:885