iCub-main
iKinIpOpt.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 <cstdlib>
12 #include <limits>
13 #include <string>
14 
15 #include <IpTNLP.hpp>
16 #include <IpIpoptApplication.hpp>
17 
18 #include <iCub/iKin/iKinIpOpt.h>
19 
20 #define CAST_IPOPTAPP(x) (static_cast<IpoptApplication*>(x))
21 #define IKINIPOPT_SHOULDER_MAXABDUCTION (100.0*CTRL_DEG2RAD)
22 
23 using namespace std;
24 using namespace yarp::sig;
25 using namespace yarp::math;
26 using namespace iCub::ctrl;
27 using namespace iCub::iKin;
28 using namespace Ipopt;
29 
30 
31 /************************************************************************/
32 iKinLinIneqConstr::iKinLinIneqConstr()
33 {
34  lowerBoundInf=-std::numeric_limits<double>::max();
35  upperBoundInf=std::numeric_limits<double>::max();
36  active=false;
37 }
38 
39 
40 /************************************************************************/
41 iKinLinIneqConstr::iKinLinIneqConstr(const double _lowerBoundInf,
42  const double _upperBoundInf)
43 {
44  lowerBoundInf=_lowerBoundInf;
45  upperBoundInf=_upperBoundInf;
46  active=false;
47 }
48 
49 
50 /************************************************************************/
51 void iKinLinIneqConstr::clone(const iKinLinIneqConstr *obj)
52 {
53  C =obj->C;
54  uB=obj->uB;
55  lB=obj->lB;
56 
57  lowerBoundInf=obj->lowerBoundInf;
58  upperBoundInf=obj->upperBoundInf;
59  active=obj->active;
60 }
61 
62 
63 /************************************************************************/
64 iKinLinIneqConstr::iKinLinIneqConstr(const iKinLinIneqConstr &obj)
65 {
66  clone(&obj);
67 }
68 
69 
70 /************************************************************************/
71 iKinLinIneqConstr &iKinLinIneqConstr::operator=(const iKinLinIneqConstr &obj)
72 {
73  clone(&obj);
74 
75  return *this;
76 }
77 
78 
79 /************************************************************************/
80 void iCubAdditionalArmConstraints::clone(const iKinLinIneqConstr *obj)
81 {
82  iKinLinIneqConstr::clone(obj);
83 
84  const iCubAdditionalArmConstraints *ptr=static_cast<const iCubAdditionalArmConstraints*>(obj);
85 
86  shou_m=ptr->shou_m;
87  shou_n=ptr->shou_n;
88  elb_m=ptr->elb_m;
89  elb_n=ptr->elb_n;
90 
91  chain=ptr->chain;
92  hw_version=ptr->hw_version;
93 }
94 
95 
96 /************************************************************************/
97 iCubAdditionalArmConstraints::iCubAdditionalArmConstraints(iCubArm &arm) :
99 {
100  chain=arm.asChain();
101 
102  size_t underscore=arm.getType().find('_');
103  hw_version=(underscore!=string::npos)?strtod(arm.getType().substr(underscore+2).c_str(),NULL):1.0;
104 
105  double joint1_0, joint1_1;
106  double joint2_0, joint2_1;
107  joint1_0= 28.0*CTRL_DEG2RAD;
108  joint1_1= 23.0*CTRL_DEG2RAD;
109  joint2_0=-37.0*CTRL_DEG2RAD;
110  joint2_1= 80.0*CTRL_DEG2RAD;
111  shou_m=(joint1_1-joint1_0)/(joint2_1-joint2_0);
112  shou_n=joint1_0-shou_m*joint2_0;
113 
114  double joint3_0, joint3_1;
115  double joint4_0, joint4_1;
116  joint3_0= 85.0*CTRL_DEG2RAD;
117  joint3_1=105.0*CTRL_DEG2RAD;
118  joint4_0= 90.0*CTRL_DEG2RAD;
119  joint4_1= 40.0*CTRL_DEG2RAD;
120  elb_m=(joint4_1-joint4_0)/(joint3_1-joint3_0);
121  elb_n=joint4_0-elb_m*joint3_0;
122 
123  update(NULL);
124 }
125 
126 
127 /************************************************************************/
129 {
130  // optimization won't use LinIneqConstr by default
131  setActive(false);
132 
133  yarp::sig::Vector row(chain->getDOF());
134 
135  // these structures will grow as soon as
136  // new constraints will be added up
137  yarp::sig::Matrix _C(0,row.length());
138  yarp::sig::Vector _lB;
139  yarp::sig::Vector _uB;
140 
141  // if shoulder's axes are controlled, constraint them
142  if (!(*chain)[3+0].isBlocked() && !(*chain)[3+1].isBlocked() &&
143  !(*chain)[3+2].isBlocked())
144  {
145  // compute offset to shoulder's axes
146  // given the blocked/released status of
147  // torso links
148  int offs=0;
149  for (int i=0; i<3; i++)
150  if (!(*chain)[i].isBlocked())
151  offs++;
152 
153  // constraints on the cables length
154  // (applies only for hw < 3.0)
155  if (hw_version<3.0)
156  {
157  row.zero();
158  row[offs]=1.71; row[offs+1]=-1.71;
159  _C=pile(_C,row);
160  _lB=cat(_lB,-347.00*CTRL_DEG2RAD);
161  _uB=cat(_uB,upperBoundInf);
162 
163  row.zero();
164  row[offs]=1.71; row[offs+1]=-1.71; row[offs+2]=-1.71;
165  _C=pile(_C,row);
166  _lB=cat(_lB,-366.57*CTRL_DEG2RAD);
167  _uB=cat(_uB,112.42*CTRL_DEG2RAD);
168 
169  row.zero();
170  row[offs+1]=1.0; row[offs+2]=1.0;
171  _C=pile(_C,row);
172  _lB=cat(_lB,-66.60*CTRL_DEG2RAD);
173  _uB=cat(_uB,213.30*CTRL_DEG2RAD);
174  }
175 
176  // constraints to prevent arm from touching torso
177  row.zero();
178  row[offs+1]=1.0; row[offs+2]=-shou_m;
179  _C=pile(_C,row);
180  _lB=cat(_lB,shou_n);
181  _uB=cat(_uB,upperBoundInf);
182 
183  // constraints to limit shoulder abduction
184  row.zero();
185  row[offs+1]=1.0;
186  _C=pile(_C,row);
187  _lB=cat(_lB,lowerBoundInf);
188  _uB=cat(_uB,IKINIPOPT_SHOULDER_MAXABDUCTION);
189 
190  // optimization will use LinIneqConstr
191  getC()=_C;
192  getlB()=_lB;
193  getuB()=_uB;
194  setActive(true);
195  }
196 
197  // if elbow and pronosupination axes are controlled, constraint them
198  if (!(*chain)[3+3].isBlocked() && !(*chain)[3+3+1].isBlocked())
199  {
200  // compute offset to elbow's axis
201  // given the blocked/released status of
202  // previous link
203  int offs=0;
204  for (int i=0; i<(3+3); i++)
205  if (!(*chain)[i].isBlocked())
206  offs++;
207 
208  // constraints to prevent the forearm from hitting the arm
209  row.zero();
210  row[offs]=-elb_m; row[offs+1]=1.0;
211  _C=pile(_C,row);
212  _lB=cat(_lB,lowerBoundInf);
213  _uB=cat(_uB,elb_n);
214 
215  row.zero();
216  row[offs]=elb_m; row[offs+1]=1.0;
217  _C=pile(_C,row);
218  _lB=cat(_lB,-elb_n);
219  _uB=cat(_uB,upperBoundInf);
220 
221  // optimization will use LinIneqConstr
222  getC()=_C;
223  getlB()=_lB;
224  getuB()=_uB;
225  setActive(true);
226  }
227 }
228 
229 
230 /************************************************************************/
231 class iKin_NLP : public TNLP
232 {
233 private:
234  // Copy constructor: not implemented.
235  iKin_NLP(const iKin_NLP&);
236  // Assignment operator: not implemented.
237  iKin_NLP &operator=(const iKin_NLP&);
238 
239 protected:
242 
244 
245  unsigned int dim;
246  unsigned int dim_2nd;
247  unsigned int ctrlPose;
248 
249  yarp::sig::Vector &xd;
250  yarp::sig::Vector &xd_2nd;
251  yarp::sig::Vector &w_2nd;
252  yarp::sig::Vector &qd_3rd;
253  yarp::sig::Vector &w_3rd;
254  yarp::sig::Vector qd;
255  yarp::sig::Vector q0;
256  yarp::sig::Vector q;
257  bool *exhalt;
258 
259  yarp::sig::Vector e_zero;
260  yarp::sig::Vector e_xyz;
261  yarp::sig::Vector e_ang;
262  yarp::sig::Vector e_2nd;
263  yarp::sig::Vector e_3rd;
264 
265  yarp::sig::Matrix J_zero;
266  yarp::sig::Matrix J_xyz;
267  yarp::sig::Matrix J_ang;
268  yarp::sig::Matrix J_2nd;
269 
270  yarp::sig::Vector *e_1st;
271  yarp::sig::Matrix *J_1st;
272  yarp::sig::Vector *e_cst;
273  yarp::sig::Matrix *J_cst;
274 
275  yarp::sig::Vector linC;
276 
278  double __x_scaling;
279  double __g_scaling;
282 
284 
287  bool firstGo;
288 
289  /************************************************************************/
290  virtual void computeQuantities(const Number *x)
291  {
292  yarp::sig::Vector new_q(dim);
293  for (Index i=0; i<(int)dim; i++)
294  new_q[i]=x[i];
295 
296  if (!(q==new_q) || firstGo)
297  {
298  firstGo=false;
299  q=new_q;
300 
301  yarp::sig::Vector v(4,0.0);
302  if (xd.length()>=7)
303  {
304  v[0]=xd[3];
305  v[1]=xd[4];
306  v[2]=xd[5];
307  v[3]=xd[6];
308  }
309 
310  q=chain.setAng(q);
311  yarp::sig::Matrix H=chain.getH();
312  yarp::sig::Matrix E=axis2dcm(v)*H.transposed();
313  v=dcm2axis(E);
314 
315  e_xyz[0]=xd[0]-H(0,3);
316  e_xyz[1]=xd[1]-H(1,3);
317  e_xyz[2]=xd[2]-H(2,3);
318  e_ang[0]=v[3]*v[0];
319  e_ang[1]=v[3]*v[1];
320  e_ang[2]=v[3]*v[2];
321 
322  yarp::sig::Matrix J1=chain.GeoJacobian();
323  submatrix(J1,J_xyz,0,2,0,dim-1);
324  submatrix(J1,J_ang,3,5,0,dim-1);
325 
326  if (weight2ndTask!=0.0)
327  {
328  yarp::sig::Matrix H_2nd=chain2ndTask.getH();
329  e_2nd[0]=w_2nd[0]*(xd_2nd[0]-H_2nd(0,3));
330  e_2nd[1]=w_2nd[1]*(xd_2nd[1]-H_2nd(1,3));
331  e_2nd[2]=w_2nd[2]*(xd_2nd[2]-H_2nd(2,3));
332 
333  yarp::sig::Matrix J2=chain2ndTask.GeoJacobian();
334 
335  for (unsigned int i=0; i<dim_2nd; i++)
336  {
337  J_2nd(0,i)=w_2nd[0]*J2(0,i);
338  J_2nd(1,i)=w_2nd[1]*J2(1,i);
339  J_2nd(2,i)=w_2nd[2]*J2(2,i);
340  }
341  }
342 
343  if (weight3rdTask!=0.0)
344  for (unsigned int i=0; i<dim; i++)
345  e_3rd[i]=w_3rd[i]*(qd_3rd[i]-q[i]);
346 
347  if (LIC.isActive())
348  linC=LIC.getC()*q;
349  }
350  }
351 
352 
353 public:
354  /************************************************************************/
355  iKin_NLP(iKinChain &c, unsigned int _ctrlPose, const yarp::sig::Vector &_q0,
356  yarp::sig::Vector &_xd, double _weight2ndTask, iKinChain &_chain2ndTask,
357  yarp::sig::Vector &_xd_2nd, yarp::sig::Vector &_w_2nd, double _weight3rdTask,
358  yarp::sig::Vector &_qd_3rd, yarp::sig::Vector &_w_3rd, iKinLinIneqConstr &_LIC,
359  bool *_exhalt=NULL) :
360  chain(c), q0(_q0), xd(_xd),
361  chain2ndTask(_chain2ndTask), xd_2nd(_xd_2nd), w_2nd(_w_2nd),
362  weight3rdTask(_weight3rdTask), qd_3rd(_qd_3rd), w_3rd(_w_3rd),
363  LIC(_LIC),
364  exhalt(_exhalt)
365  {
366  dim=chain.getDOF();
367  dim_2nd=chain2ndTask.getDOF();
368 
369  ctrlPose=_ctrlPose;
370 
371  if (ctrlPose>IKINCTRL_POSE_ANG)
372  ctrlPose=IKINCTRL_POSE_ANG;
373 
374  weight2ndTask=dim_2nd>0 ? _weight2ndTask : 0.0;
375 
376  qd.resize(dim);
377 
378  unsigned int n=(unsigned int)q0.length();
379  n=n>dim ? dim : n;
380 
381  unsigned int i;
382  for (i=0; i<n; i++)
383  qd[i]=q0[i];
384 
385  for (; i<dim; i++)
386  qd[i]=0.0;
387 
388  q=qd;
389 
390  e_zero.resize(3,0.0);
391  e_xyz.resize(3,0.0);
392  e_ang.resize(3,0.0);
393  e_2nd.resize(3,0.0);
394  e_3rd.resize(dim,0.0);
395 
396  J_zero.resize(3,dim); J_zero.zero();
397  J_xyz.resize(3,dim); J_xyz.zero();
398  J_ang.resize(3,dim); J_ang.zero();
399  J_2nd.resize(3,dim); J_2nd.zero();
400 
401  if (ctrlPose==IKINCTRL_POSE_FULL)
402  {
403  e_1st=&e_ang;
404  J_1st=&J_ang;
405  }
406  else
407  {
408  e_1st=&e_zero;
409  J_1st=&J_zero;
410  }
411 
412  e_cst=&e_xyz;
413  J_cst=&J_xyz;
414 
415  firstGo=true;
416 
417  __obj_scaling=1.0;
418  __x_scaling =1.0;
419  __g_scaling =1.0;
420 
421  lowerBoundInf=-std::numeric_limits<double>::max();
422  upperBoundInf=std::numeric_limits<double>::max();
423 
424  callback=NULL;
425  }
426 
427  /************************************************************************/
428  yarp::sig::Vector get_qd() { return qd; }
429 
430  /************************************************************************/
431  void set_callback(iKinIterateCallback *_callback) { callback=_callback; }
432 
433  /************************************************************************/
434  void set_scaling(double _obj_scaling, double _x_scaling, double _g_scaling)
435  {
436  __obj_scaling=_obj_scaling;
437  __x_scaling =_x_scaling;
438  __g_scaling =_g_scaling;
439  }
440 
441  /************************************************************************/
442  void set_bound_inf(double lower, double upper)
443  {
444  lowerBoundInf=lower;
445  upperBoundInf=upper;
446  }
447 
448  /************************************************************************/
449  bool set_posePriority(const string &priority)
450  {
451  if (priority=="position")
452  {
453  if (ctrlPose==IKINCTRL_POSE_FULL)
454  {
455  e_1st=&e_ang;
456  J_1st=&J_ang;
457  }
458  else
459  {
460  e_1st=&e_zero;
461  J_1st=&J_zero;
462  }
463 
464  e_cst=&e_xyz;
465  J_cst=&J_xyz;
466  }
467  else if (priority=="orientation")
468  {
469  if (ctrlPose==IKINCTRL_POSE_FULL)
470  {
471  e_1st=&e_xyz;
472  J_1st=&J_xyz;
473  }
474  else
475  {
476  e_1st=&e_zero;
477  J_1st=&J_zero;
478  }
479 
480  e_cst=&e_ang;
481  J_cst=&J_ang;
482  }
483  else
484  return false;
485 
486  return true;
487  }
488 
489  /************************************************************************/
490  bool get_nlp_info(Index& n, Index& m, Index& nnz_jac_g, Index& nnz_h_lag,
491  IndexStyleEnum& index_style)
492  {
493  n=dim;
494  m=1;
495  nnz_jac_g=dim;
496 
497  if (LIC.isActive())
498  {
499  int lenLower=(int)LIC.getlB().length();
500  int lenUpper=(int)LIC.getuB().length();
501 
502  if (lenLower && (lenLower==lenUpper) && (LIC.getC().cols()==dim))
503  {
504  m+=lenLower;
505  nnz_jac_g+=lenLower*dim;
506  }
507  else
508  LIC.setActive(false);
509  }
510 
511  nnz_h_lag=(dim*(dim+1))>>1;
512  index_style=TNLP::C_STYLE;
513 
514  return true;
515  }
516 
517  /************************************************************************/
518  bool get_bounds_info(Index n, Number* x_l, Number* x_u, Index m, Number* g_l,
519  Number* g_u)
520  {
521  for (Index i=0; i<n; i++)
522  {
523  x_l[i]=chain(i).getMin();
524  x_u[i]=chain(i).getMax();
525  }
526 
527  Index offs=0;
528 
529  for (Index i=0; i<m; i++)
530  {
531  if (i==0)
532  {
533  g_l[0]=g_u[0]=0.0;
534  offs=1;
535  }
536  else
537  {
538  g_l[i]=LIC.getlB()[i-offs];
539  g_u[i]=LIC.getuB()[i-offs];
540  }
541  }
542 
543  return true;
544  }
545 
546  /************************************************************************/
547  bool get_starting_point(Index n, bool init_x, Number* x, bool init_z,
548  Number* z_L, Number* z_U, Index m, bool init_lambda,
549  Number* lambda)
550  {
551  for (Index i=0; i<n; i++)
552  x[i]=q0[i];
553 
554  return true;
555  }
556 
557  /************************************************************************/
558  bool eval_f(Index n, const Number* x, bool new_x, Number& obj_value)
559  {
560  computeQuantities(x);
561 
562  obj_value=norm2(*e_1st);
563 
564  if (weight2ndTask!=0.0)
565  obj_value+=weight2ndTask*norm2(e_2nd);
566 
567  if (weight3rdTask!=0.0)
568  obj_value+=weight3rdTask*norm2(e_3rd);
569 
570  return true;
571  }
572 
573  /************************************************************************/
574  bool eval_grad_f(Index n, const Number* x, bool new_x, Number* grad_f)
575  {
576  computeQuantities(x);
577 
578  yarp::sig::Vector grad=-2.0*(J_1st->transposed() * *e_1st);
579 
580  if (weight2ndTask!=0.0)
581  grad=grad-2.0*weight2ndTask*(J_2nd.transposed()*e_2nd);
582 
583  if (weight3rdTask!=0.0)
584  grad=grad-2.0*weight3rdTask*(w_3rd*e_3rd);
585 
586  for (Index i=0; i<n; i++)
587  grad_f[i]=grad[i];
588 
589  return true;
590  }
591 
592  /************************************************************************/
593  bool eval_g(Index n, const Number* x, bool new_x, Index m, Number* g)
594  {
595  computeQuantities(x);
596 
597  Index offs=0;
598 
599  for (Index i=0; i<m; i++)
600  {
601  if (i==0)
602  {
603  g[0]=norm2(*e_cst);
604  offs=1;
605  }
606  else
607  g[i]=linC[i-offs];
608  }
609 
610  return true;
611  }
612 
613  /************************************************************************/
614  bool eval_jac_g(Index n, const Number* x, bool new_x, Index m, Index nele_jac,
615  Index* iRow, Index *jCol, Number* values)
616  {
617  if (m!=0)
618  {
619  if (values==NULL)
620  {
621  Index idx=0;
622 
623  for (Index row=0; row<m; row++)
624  {
625  for (Index col=0; col<n; col++)
626  {
627  iRow[idx]=row;
628  jCol[idx]=col;
629  idx++;
630  }
631  }
632  }
633  else
634  {
635  computeQuantities(x);
636 
637  yarp::sig::Vector grad=-2.0*(J_cst->transposed() * *e_cst);
638 
639  Index idx =0;
640  Index offs=0;
641 
642  for (Index row=0; row<m; row++)
643  {
644  for (Index col=0; col<n; col++)
645  {
646  if (row==0)
647  {
648  values[idx]=grad[idx];
649  offs=1;
650  }
651  else
652  values[idx]=LIC.getC()(row-offs,col);
653 
654  idx++;
655  }
656  }
657  }
658  }
659 
660  return true;
661  }
662 
663  /************************************************************************/
664  bool eval_h(Index n, const Number* x, bool new_x, Number obj_factor,
665  Index m, const Number* lambda, bool new_lambda,
666  Index nele_hess, Index* iRow, Index* jCol, Number* values)
667  {
668  if (values==NULL)
669  {
670  Index idx=0;
671 
672  for (Index row=0; row<n; row++)
673  {
674  for (Index col=0; col<=row; col++)
675  {
676  iRow[idx]=row;
677  jCol[idx]=col;
678  idx++;
679  }
680  }
681  }
682  else
683  {
684  // Given the task: min f(q)=||xd-F(q)||^2
685  // the Hessian Hij is: 2 * (<dF/dqi,dF/dqj> - <d2F/dqidqj,e>)
686  computeQuantities(x);
687  chain.prepareForHessian();
688 
689  if (weight2ndTask!=0.0)
690  chain2ndTask.prepareForHessian();
691 
692  Index idx=0;
693  for (Index row=0; row<n; row++)
694  {
695  for (Index col=0; col<=row; col++)
696  {
697  // warning: row and col are swapped due to asymmetry
698  // of orientation part within the hessian
699  yarp::sig::Vector h=chain.fastHessian_ij(col,row);
700  yarp::sig::Vector h_xyz(3), h_ang(3), h_zero(3,0.0);
701  h_xyz[0]=h[0];
702  h_xyz[1]=h[1];
703  h_xyz[2]=h[2];
704  h_ang[0]=h[3];
705  h_ang[1]=h[4];
706  h_ang[2]=h[5];
707 
708  yarp::sig::Vector *h_1st,*h_cst;
709  if (e_cst==&e_xyz)
710  {
711  h_1st=(ctrlPose==IKINCTRL_POSE_FULL)?&h_ang:&h_zero;
712  h_cst=&h_xyz;
713  }
714  else
715  {
716  h_1st=(ctrlPose==IKINCTRL_POSE_FULL)?&h_xyz:&h_zero;
717  h_cst=&h_ang;
718  }
719 
720  values[idx]=2.0*(obj_factor*(dot(*J_1st,row,*J_1st,col)-dot(*h_1st,*e_1st))+
721  lambda[0]*(dot(*J_cst,row,*J_cst,col)-dot(*h_cst,*e_cst)));
722 
723  if ((weight2ndTask!=0.0) && (row<(int)dim_2nd) && (col<(int)dim_2nd))
724  {
725  // warning: row and col are swapped due to asymmetry
726  // of orientation part within the hessian
727  yarp::sig::Vector h2=chain2ndTask.fastHessian_ij(col,row);
728  yarp::sig::Vector h_2nd(3);
729  h_2nd[0]=(w_2nd[0]*w_2nd[0])*h2[0];
730  h_2nd[1]=(w_2nd[1]*w_2nd[1])*h2[1];
731  h_2nd[2]=(w_2nd[2]*w_2nd[2])*h2[2];
732 
733  values[idx]+=2.0*obj_factor*weight2ndTask*(dot(J_2nd,row,J_2nd,col)-dot(h_2nd,e_2nd));
734  }
735 
736  idx++;
737  }
738  }
739  }
740 
741  return true;
742  }
743 
744  /************************************************************************/
745  bool intermediate_callback(AlgorithmMode mode, Index iter, Number obj_value,
746  Number inf_pr, Number inf_du, Number mu, Number d_norm,
747  Number regularization_size, Number alpha_du, Number alpha_pr,
748  Index ls_trials, const IpoptData* ip_data,
749  IpoptCalculatedQuantities* ip_cq)
750  {
751  if (callback!=NULL)
752  callback->exec(xd,q);
753 
754  if (exhalt!=NULL)
755  return !(*exhalt);
756  else
757  return true;
758  }
759 
760  /************************************************************************/
761  bool get_scaling_parameters(Number& obj_scaling, bool& use_x_scaling, Index n,
762  Number* x_scaling, bool& use_g_scaling, Index m,
763  Number* g_scaling)
764  {
765  obj_scaling=__obj_scaling;
766 
767  for (Index i=0; i<n; i++)
768  x_scaling[i]=__x_scaling;
769 
770  for (Index j=0; j<m; j++)
771  g_scaling[j]=__g_scaling;
772 
773  use_x_scaling=use_g_scaling=true;
774 
775  return true;
776  }
777 
778  /************************************************************************/
779  void finalize_solution(SolverReturn status, Index n, const Number* x,
780  const Number* z_L, const Number* z_U, Index m,
781  const Number* g, const Number* lambda, Number obj_value,
782  const IpoptData* ip_data, IpoptCalculatedQuantities* ip_cq)
783  {
784  for (Index i=0; i<n; i++)
785  qd[i]=x[i];
786 
787  qd=chain.setAng(qd);
788  }
789 
790  /************************************************************************/
791  virtual ~iKin_NLP() { }
792 };
793 
794 
795 /************************************************************************/
796 iKinIpOptMin::iKinIpOptMin(iKinChain &c, const unsigned int _ctrlPose, const double tol,
797  const double constr_tol, const int max_iter,
798  const unsigned int verbose, bool useHessian) :
799  chain(c)
800 {
801  ctrlPose=_ctrlPose;
802  posePriority="position";
803  pLIC=&noLIC;
804 
807 
808  chain.setAllConstraints(false); // this is required since IpOpt initially relaxes constraints
809 
810  App=new IpoptApplication();
811 
812  CAST_IPOPTAPP(App)->Options()->SetNumericValue("tol",tol);
813  CAST_IPOPTAPP(App)->Options()->SetNumericValue("constr_viol_tol",constr_tol);
814  CAST_IPOPTAPP(App)->Options()->SetIntegerValue("acceptable_iter",0);
815  CAST_IPOPTAPP(App)->Options()->SetStringValue("mu_strategy","adaptive");
816  CAST_IPOPTAPP(App)->Options()->SetIntegerValue("print_level",verbose);
817 
819 
820  if (max_iter>0)
821  CAST_IPOPTAPP(App)->Options()->SetIntegerValue("max_iter",max_iter);
822  else
823  CAST_IPOPTAPP(App)->Options()->SetIntegerValue("max_iter",(Index)upperBoundInf);
824 
825  if (!useHessian)
826  CAST_IPOPTAPP(App)->Options()->SetStringValue("hessian_approximation","limited-memory");
827 
828  CAST_IPOPTAPP(App)->Initialize();
829 }
830 
831 
832 /************************************************************************/
833 void iKinIpOptMin::set_ctrlPose(const unsigned int _ctrlPose)
834 {
835  ctrlPose=_ctrlPose;
836 
839 }
840 
841 
842 /************************************************************************/
843 bool iKinIpOptMin::set_posePriority(const string &priority)
844 {
845  if ((priority=="position") || (priority=="orientation"))
846  {
847  posePriority=priority;
848  return true;
849  }
850  else
851  return false;
852 }
853 
854 
855 /************************************************************************/
856 void iKinIpOptMin::specify2ndTaskEndEff(const unsigned int n)
857 {
858  unsigned int _n=n;
859  if (_n>chain.getN())
860  _n=chain.getN();
861 
863  for (unsigned int i=0; i<_n; i++)
864  chain2ndTask<<chain[i];
865 
867  if (_n==chain.getN())
869 }
870 
871 
872 /************************************************************************/
874 {
875  return chain2ndTask;
876 }
877 
878 
879 /************************************************************************/
880 void iKinIpOptMin::setMaxIter(const int max_iter)
881 {
882  if (max_iter>0)
883  CAST_IPOPTAPP(App)->Options()->SetIntegerValue("max_iter",max_iter);
884  else
885  CAST_IPOPTAPP(App)->Options()->SetIntegerValue("max_iter",std::numeric_limits<int>::max());
886 
887  CAST_IPOPTAPP(App)->Initialize();
888 }
889 
890 
891 /************************************************************************/
893 {
894  int max_iter;
895  CAST_IPOPTAPP(App)->Options()->GetIntegerValue("max_iter",max_iter,"");
896  return max_iter;
897 }
898 
899 
900 /************************************************************************/
901 void iKinIpOptMin::setMaxCpuTime(const double max_cpu_time)
902 {
903  CAST_IPOPTAPP(App)->Options()->SetNumericValue("max_cpu_time",max_cpu_time);
904  CAST_IPOPTAPP(App)->Initialize();
905 }
906 
907 
908 /************************************************************************/
910 {
911  double max_cpu_time;
912  CAST_IPOPTAPP(App)->Options()->GetNumericValue("max_cpu_time",max_cpu_time,"");
913  return max_cpu_time;
914 }
915 
916 
917 /************************************************************************/
918 void iKinIpOptMin::setTol(const double tol)
919 {
920  CAST_IPOPTAPP(App)->Options()->SetNumericValue("tol",tol);
921  CAST_IPOPTAPP(App)->Initialize();
922 }
923 
924 
925 /************************************************************************/
926 double iKinIpOptMin::getTol() const
927 {
928  double tol;
929  CAST_IPOPTAPP(App)->Options()->GetNumericValue("tol",tol,"");
930  return tol;
931 }
932 
933 
934 /************************************************************************/
935 void iKinIpOptMin::setConstrTol(const double constr_tol)
936 {
937  CAST_IPOPTAPP(App)->Options()->SetNumericValue("constr_viol_tol",constr_tol);
938  CAST_IPOPTAPP(App)->Initialize();
939 }
940 
941 
942 /************************************************************************/
944 {
945  double constr_tol;
946  CAST_IPOPTAPP(App)->Options()->GetNumericValue("constr_viol_tol",constr_tol,"");
947  return constr_tol;
948 }
949 
950 
951 /************************************************************************/
952 void iKinIpOptMin::setVerbosity(const unsigned int verbose)
953 {
954  CAST_IPOPTAPP(App)->Options()->SetIntegerValue("print_level",verbose);
955 
956  CAST_IPOPTAPP(App)->Initialize();
957 }
958 
959 
960 /************************************************************************/
961 void iKinIpOptMin::setHessianOpt(const bool useHessian)
962 {
963  if (useHessian)
964  CAST_IPOPTAPP(App)->Options()->SetStringValue("hessian_approximation","exact");
965  else
966  CAST_IPOPTAPP(App)->Options()->SetStringValue("hessian_approximation","limited-memory");
967 
968  CAST_IPOPTAPP(App)->Initialize();
969 }
970 
971 
972 /************************************************************************/
973 void iKinIpOptMin::setUserScaling(const bool useUserScaling, const double _obj_scaling,
974  const double _x_scaling, const double _g_scaling)
975 {
976  if (useUserScaling)
977  {
978  obj_scaling=_obj_scaling;
979  x_scaling =_x_scaling;
980  g_scaling =_g_scaling;
981 
982  CAST_IPOPTAPP(App)->Options()->SetStringValue("nlp_scaling_method","user-scaling");
983  }
984  else
985  CAST_IPOPTAPP(App)->Options()->SetStringValue("nlp_scaling_method","gradient-based");
986 
987  CAST_IPOPTAPP(App)->Initialize();
988 }
989 
990 
991 /************************************************************************/
992 void iKinIpOptMin::setDerivativeTest(const bool enableTest, const bool enable2ndDer)
993 {
994  if (enableTest)
995  {
996  if (enable2ndDer)
997  CAST_IPOPTAPP(App)->Options()->SetStringValue("derivative_test","second-order");
998  else
999  CAST_IPOPTAPP(App)->Options()->SetStringValue("derivative_test","first-order");
1000 
1001  CAST_IPOPTAPP(App)->Options()->SetStringValue("derivative_test_print_all","yes");
1002  }
1003  else
1004  CAST_IPOPTAPP(App)->Options()->SetStringValue("derivative_test","none");
1005 
1006  CAST_IPOPTAPP(App)->Initialize();
1007 }
1008 
1009 
1010 /************************************************************************/
1011 void iKinIpOptMin::getBoundsInf(double &lower, double &upper)
1012 {
1013  CAST_IPOPTAPP(App)->Options()->GetNumericValue("nlp_lower_bound_inf",lower,"");
1014  CAST_IPOPTAPP(App)->Options()->GetNumericValue("nlp_upper_bound_inf",upper,"");
1015 }
1016 
1017 
1018 /************************************************************************/
1019 void iKinIpOptMin::setBoundsInf(const double lower, const double upper)
1020 {
1021  CAST_IPOPTAPP(App)->Options()->SetNumericValue("nlp_lower_bound_inf",lower);
1022  CAST_IPOPTAPP(App)->Options()->SetNumericValue("nlp_upper_bound_inf",upper);
1023 
1025  upperBoundInf=upper;
1026 }
1027 
1028 
1029 /************************************************************************/
1030 yarp::sig::Vector iKinIpOptMin::solve(const yarp::sig::Vector &q0, yarp::sig::Vector &xd,
1031  double weight2ndTask, yarp::sig::Vector &xd_2nd,
1032  yarp::sig::Vector &w_2nd, double weight3rdTask,
1033  yarp::sig::Vector &qd_3rd, yarp::sig::Vector &w_3rd,
1034  int *exit_code, bool *exhalt, iKinIterateCallback *iterate)
1035 {
1036  SmartPtr<iKin_NLP> nlp=new iKin_NLP(chain,ctrlPose,q0,xd,
1037  weight2ndTask,chain2ndTask,xd_2nd,w_2nd,
1038  weight3rdTask,qd_3rd,w_3rd,
1039  *pLIC,exhalt);
1040 
1041  nlp->set_scaling(obj_scaling,x_scaling,g_scaling);
1042  nlp->set_bound_inf(lowerBoundInf,upperBoundInf);
1043  nlp->set_posePriority(posePriority);
1044  nlp->set_callback(iterate);
1045 
1046  ApplicationReturnStatus status=CAST_IPOPTAPP(App)->OptimizeTNLP(GetRawPtr(nlp));
1047 
1048  if (exit_code!=NULL)
1049  *exit_code=status;
1050 
1051  return nlp->get_qd();
1052 }
1053 
1054 
1055 /************************************************************************/
1056 yarp::sig::Vector iKinIpOptMin::solve(const yarp::sig::Vector &q0, yarp::sig::Vector &xd,
1057  double weight2ndTask, yarp::sig::Vector &xd_2nd,
1058  yarp::sig::Vector &w_2nd)
1059 {
1060  yarp::sig::Vector dummy(1);
1061  return solve(q0,xd,weight2ndTask,xd_2nd,w_2nd,0.0,dummy,dummy);
1062 }
1063 
1064 
1065 /************************************************************************/
1066 yarp::sig::Vector iKinIpOptMin::solve(const yarp::sig::Vector &q0, yarp::sig::Vector &xd)
1067 {
1068  yarp::sig::Vector dummy(1);
1069  return solve(q0,xd,0.0,dummy,dummy,0.0,dummy,dummy);
1070 }
1071 
1072 
1073 /************************************************************************/
1075 {
1076  delete CAST_IPOPTAPP(App);
1077 }
1078 
1079 
std::string getType() const
Returns the Limb type as a string.
Definition: iKinFwd.h:1019
yarp::sig::Vector fastHessian_ij(const unsigned int i, const unsigned int j)
Returns the 6x1 vector where is the forward kinematic function and is the DOF couple.
Definition: iKinFwd.cpp:1116
yarp::sig::Vector * e_cst
Definition: iKinIpOpt.cpp:272
bool firstGo
Definition: iKinIpOpt.cpp:287
#define IKINIPOPT_SHOULDER_MAXABDUCTION
Definition: iKinIpOpt.cpp:21
bool eval_g(Index n, const Number *x, bool new_x, Index m, Number *g)
Definition: iKinIpOpt.cpp:593
yarp::sig::Vector e_2nd
Definition: iKinIpOpt.cpp:262
iKinLinIneqConstr noLIC
Definition: iKinIpOpt.h:214
iKin_NLP(iKinChain &c, unsigned int _ctrlPose, const yarp::sig::Vector &_q0, yarp::sig::Vector &_xd, double _weight2ndTask, iKinChain &_chain2ndTask, yarp::sig::Vector &_xd_2nd, yarp::sig::Vector &_w_2nd, double _weight3rdTask, yarp::sig::Vector &_qd_3rd, yarp::sig::Vector &_w_3rd, iKinLinIneqConstr &_LIC, bool *_exhalt=NULL)
Definition: iKinIpOpt.cpp:355
static uint32_t idx[BOARD_NUM]
unsigned int ctrlPose
Definition: iKinIpOpt.h:217
unsigned int ctrlPose
Definition: iKinIpOpt.cpp:247
bool setHN(const yarp::sig::Matrix &_HN)
Sets HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
Definition: iKinFwd.cpp:579
bool eval_f(Index n, const Number *x, bool new_x, Number &obj_value)
Definition: iKinIpOpt.cpp:558
double __x_scaling
Definition: iKinIpOpt.cpp:278
void setBoundsInf(const double lower, const double upper)
Sets the lower and upper bounds to represent -inf and +inf.
Definition: iKinIpOpt.cpp:1019
yarp::sig::Vector e_ang
Definition: iKinIpOpt.cpp:261
void setVerbosity(const unsigned int verbose)
Sets Verbosity.
Definition: iKinIpOpt.cpp:952
yarp::sig::Matrix J_2nd
Definition: iKinIpOpt.cpp:268
Class for dealing with additional iCub arm&#39;s constraints.
Definition: iKinIpOpt.h:171
virtual ~iKin_NLP()
Definition: iKinIpOpt.cpp:791
double lowerBoundInf
Definition: iKinIpOpt.cpp:280
yarp::sig::Vector & w_2nd
Definition: iKinIpOpt.cpp:251
STL namespace.
void setMaxIter(const int max_iter)
Sets Maximum Iteration.
Definition: iKinIpOpt.cpp:880
iKinChain & get2ndTaskChain()
Retrieves the 2nd task&#39;s chain.
Definition: iKinIpOpt.cpp:873
#define IKINCTRL_POSE_ANG
Definition: iKinInv.h:39
unsigned int dim
Definition: iKinIpOpt.cpp:245
double dot(const yarp::sig::Matrix &A, int colA, const yarp::sig::Matrix &B, int colB)
Returns the dot product between two vectors given in the form: matrix(:,col).
yarp::sig::Vector & xd_2nd
Definition: iKinIpOpt.cpp:250
unsigned int getDOF() const
Returns the current number of Chain&#39;s DOF.
Definition: iKinFwd.h:557
yarp::sig::Matrix getH0() const
Returns H0, the rigid roto-translation matrix from the root reference frame to the 0th frame...
Definition: iKinFwd.h:564
A Base class for defining a Serial Link Chain.
Definition: iKinFwd.h:354
int n
bool get_bounds_info(Index n, Number *x_l, Number *x_u, Index m, Number *g_l, Number *g_u)
Definition: iKinIpOpt.cpp:518
void specify2ndTaskEndEff(const unsigned int n)
Selects the End-Effector of the 2nd task by giving the ordinal number n of last joint pointing at it...
Definition: iKinIpOpt.cpp:856
void set_bound_inf(double lower, double upper)
Definition: iKinIpOpt.cpp:442
iKinIterateCallback * callback
Definition: iKinIpOpt.cpp:283
yarp::sig::Vector & xd
Definition: iKinIpOpt.cpp:249
int getMaxIter() const
Retrieves the current value of Maximum Iteration.
Definition: iKinIpOpt.cpp:892
yarp::sig::Vector & getuB()
Returns a reference to the upper bounds vector uB.
Definition: iKinIpOpt.h:124
void setHessianOpt(const bool useHessian)
Selects whether to rely on exact Hessian computation or enable Quasi-Newton approximation (Hessian is...
Definition: iKinIpOpt.cpp:961
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:731
#define CTRL_DEG2RAD
Definition: XSensMTx.cpp:26
const FSC max
Definition: strain.h:48
iKinChain & chain
Definition: iKinIpOpt.cpp:240
#define IKINCTRL_POSE_FULL
Definition: iKinInv.h:37
double norm2(const yarp::sig::Matrix &M, int col)
Returns the squared norm of the vector given in the form: matrix(:,col).
Definition: math.h:89
bool get_scaling_parameters(Number &obj_scaling, bool &use_x_scaling, Index n, Number *x_scaling, bool &use_g_scaling, Index m, Number *g_scaling)
Definition: iKinIpOpt.cpp:761
static int v
Definition: iCub_Sim.cpp:42
yarp::sig::Vector qd
Definition: iKinIpOpt.cpp:254
double getTol() const
Retrieves cost function tolerance.
Definition: iKinIpOpt.cpp:926
Class for defining Linear Inequality Constraints of the form lB <= C*q <= uB for the nonlinear proble...
Definition: iKinIpOpt.h:69
void getBoundsInf(double &lower, double &upper)
Returns the lower and upper bounds to represent -inf and +inf.
Definition: iKinIpOpt.cpp:1011
yarp::sig::Vector lB
Definition: iKinIpOpt.h:74
std::string posePriority
Definition: iKinIpOpt.h:224
iKinLinIneqConstr * pLIC
Definition: iKinIpOpt.h:215
void setConstrTol(const double constr_tol)
Sets constraints tolerance.
Definition: iKinIpOpt.cpp:935
virtual iKinLinIneqConstr & operator=(const iKinLinIneqConstr &obj)
Copies a LinIneqConstr object into the current one.
Definition: iKinIpOpt.cpp:71
bool get_nlp_info(Index &n, Index &m, Index &nnz_jac_g, Index &nnz_h_lag, IndexStyleEnum &index_style)
Definition: iKinIpOpt.cpp:490
yarp::sig::Vector e_3rd
Definition: iKinIpOpt.cpp:263
virtual yarp::sig::Vector solve(const yarp::sig::Vector &q0, yarp::sig::Vector &xd, double weight2ndTask, yarp::sig::Vector &xd_2nd, yarp::sig::Vector &w_2nd, double weight3rdTask, yarp::sig::Vector &qd_3rd, yarp::sig::Vector &w_3rd, int *exit_code=NULL, bool *exhalt=NULL, iKinIterateCallback *iterate=NULL)
Executes the IpOpt algorithm trying to converge on target.
Definition: iKinIpOpt.cpp:1030
Class for defining iteration callback.
Definition: iKinIpOpt.h:43
yarp::sig::Matrix & getC()
Returns a reference to the constraints matrix C.
Definition: iKinIpOpt.h:118
virtual void exec(const yarp::sig::Vector &xd, const yarp::sig::Vector &q)=0
Defines the callback body to be called at each iteration.
virtual ~iKinIpOptMin()
Default destructor.
Definition: iKinIpOpt.cpp:1074
yarp::sig::Vector q
Definition: iKinIpOpt.cpp:256
void set_scaling(double _obj_scaling, double _x_scaling, double _g_scaling)
Definition: iKinIpOpt.cpp:434
double weight3rdTask
Definition: iKinIpOpt.cpp:286
unsigned int dim_2nd
Definition: iKinIpOpt.cpp:246
bool lower(Value &a, Value &b)
Definition: main.cpp:337
bool eval_h(Index n, const Number *x, bool new_x, Number obj_factor, Index m, const Number *lambda, bool new_lambda, Index nele_hess, Index *iRow, Index *jCol, Number *values)
Definition: iKinIpOpt.cpp:664
yarp::sig::Matrix J_xyz
Definition: iKinIpOpt.cpp:266
double weight2ndTask
Definition: iKinIpOpt.cpp:285
void setTol(const double tol)
Sets cost function tolerance.
Definition: iKinIpOpt.cpp:918
yarp::sig::Vector * e_1st
Definition: iKinIpOpt.cpp:270
#define CAST_IPOPTAPP(x)
Definition: iKinIpOpt.cpp:20
bool get_starting_point(Index n, bool init_x, Number *x, bool init_z, Number *z_L, Number *z_U, Index m, bool init_lambda, Number *lambda)
Definition: iKinIpOpt.cpp:547
yarp::sig::Matrix J_zero
Definition: iKinIpOpt.cpp:265
yarp::sig::Matrix GeoJacobian(const unsigned int i)
Returns the geometric Jacobian of the ith link.
Definition: iKinFwd.cpp:1011
bool eval_grad_f(Index n, const Number *x, bool new_x, Number *grad_f)
Definition: iKinIpOpt.cpp:574
yarp::sig::Vector & getlB()
Returns a reference to the lower bounds vector lB.
Definition: iKinIpOpt.h:130
yarp::sig::Matrix * J_1st
Definition: iKinIpOpt.cpp:271
iKinChain & chain2ndTask
Definition: iKinIpOpt.cpp:241
yarp::sig::Vector e_zero
Definition: iKinIpOpt.cpp:259
virtual void computeQuantities(const Number *x)
Definition: iKinIpOpt.cpp:290
yarp::sig::Vector uB
Definition: iKinIpOpt.h:73
void setUserScaling(const bool useUserScaling, const double _obj_scaling, const double _x_scaling, const double _g_scaling)
Enables/disables user scaling factors.
Definition: iKinIpOpt.cpp:973
bool intermediate_callback(AlgorithmMode mode, Index iter, Number obj_value, Number inf_pr, Number inf_du, Number mu, Number d_norm, Number regularization_size, Number alpha_du, Number alpha_pr, Index ls_trials, const IpoptData *ip_data, IpoptCalculatedQuantities *ip_cq)
Definition: iKinIpOpt.cpp:745
double getMaxCpuTime() const
Retrieves the current value of Maximum CPU seconds.
Definition: iKinIpOpt.cpp:909
yarp::sig::Matrix * J_cst
Definition: iKinIpOpt.cpp:273
bool eval_jac_g(Index n, const Number *x, bool new_x, Index m, Index nele_jac, Index *iRow, Index *jCol, Number *values)
Definition: iKinIpOpt.cpp:614
void finalize_solution(SolverReturn status, Index n, const Number *x, const Number *z_L, const Number *z_U, Index m, const Number *g, const Number *lambda, Number obj_value, const IpoptData *ip_data, IpoptCalculatedQuantities *ip_cq)
Definition: iKinIpOpt.cpp:779
void setMaxCpuTime(const double max_cpu_time)
Sets Maximum CPU seconds.
Definition: iKinIpOpt.cpp:901
iKinChain * asChain()
Returns a pointer to the Limb seen as Chain object.
Definition: iKinFwd.h:1013
yarp::sig::Vector & w_3rd
Definition: iKinIpOpt.cpp:253
bool * exhalt
Definition: iKinIpOpt.cpp:257
yarp::sig::Vector & qd_3rd
Definition: iKinIpOpt.cpp:252
bool setH0(const yarp::sig::Matrix &_H0)
Sets H0, the rigid roto-translation matrix from the root reference frame to the 0th frame...
Definition: iKinFwd.cpp:561
double upperBoundInf
Definition: iKinIpOpt.cpp:281
void setDerivativeTest(const bool enableTest, const bool enable2ndDer=false)
Enable derivative test at each call to solve method (disabled at start-up by default).
Definition: iKinIpOpt.cpp:992
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition: iKinFwd.h:550
void update(void *)
Updates internal state.
Definition: iKinIpOpt.cpp:128
A class for defining the iCub Arm.
Definition: iKinFwd.h:1081
yarp::sig::Vector q0
Definition: iKinIpOpt.cpp:255
yarp::sig::Matrix J_ang
Definition: iKinIpOpt.cpp:267
void set_callback(iKinIterateCallback *_callback)
Definition: iKinIpOpt.cpp:431
double __obj_scaling
Definition: iKinIpOpt.cpp:277
void set_ctrlPose(const unsigned int _ctrlPose)
Sets the state of Pose control settings.
Definition: iKinIpOpt.cpp:833
bool isActive()
Returns the state of inequality constraints evaluation.
Definition: iKinIpOpt.h:148
yarp::sig::Vector e_xyz
Definition: iKinIpOpt.cpp:260
bool set_posePriority(const std::string &priority)
Sets the Pose priority for weighting more either position or orientation while reaching in full pose...
Definition: iKinIpOpt.cpp:843
double getConstrTol() const
Retrieves constraints tolerance.
Definition: iKinIpOpt.cpp:943
bool set_posePriority(const string &priority)
Definition: iKinIpOpt.cpp:449
void setActive(bool _active)
Sets the state of inequality constraints evaluation.
Definition: iKinIpOpt.h:154
yarp::sig::Matrix getHN() const
Returns HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
Definition: iKinFwd.h:579
GLenum mode
Definition: rendering.cpp:48
double __g_scaling
Definition: iKinIpOpt.cpp:279
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
void prepareForHessian()
Prepares computation for a successive call to fastHessian_ij().
Definition: iKinFwd.cpp:1101
iKinLinIneqConstr & LIC
Definition: iKinIpOpt.cpp:243
yarp::sig::Vector get_qd()
Definition: iKinIpOpt.cpp:428
void clear()
Removes all Links.
Definition: iKinFwd.cpp:352
yarp::sig::Matrix C
Definition: iKinIpOpt.h:72
void setAllConstraints(bool _constrained)
Sets the constraint status of all chain links.
Definition: iKinFwd.cpp:497
yarp::sig::Vector linC
Definition: iKinIpOpt.cpp:275