grasp
•All Data Structures Namespaces Functions Modules
handIK.cpp
1 /* Copyright: (C) 2014 iCub Facility - Istituto Italiano di Tecnologia
2  * Authors: Ugo Pattacini
3  * email: ugo.pattacini@iit.it
4  * Permission is granted to copy, distribute, and/or modify this program
5  * under the terms of the GNU General Public License, version 2 or any
6  * later version published by the Free Software Foundation.
7  *
8  * A copy of the license can be found in the file LICENSE located in the
9  * root directory.
10  *
11  * This program is distributed in the hope that it will be useful, but
12  * WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
14  * Public License for more details
15 */
16 
17 
18 #include <cmath>
19 #include <cstdio>
20 #include <yarp/math/Math.h>
21 #include <iCub/ctrl/math.h>
22 #include "handIK.h"
23 
24 #include <IpTNLP.hpp>
25 #include <IpIpoptApplication.hpp>
26 
27 using namespace std;
28 using namespace yarp::sig;
29 using namespace yarp::math;
30 using namespace iCub::ctrl;
31 using namespace iCub::iKin;
32 
33 
34 /****************************************************************/
35 class HandIK_NLP : public Ipopt::TNLP
36 {
37 protected:
38  HandIK_Problem &problem;
39  HandIK_Variables guess;
40  HandIK_Variables solution;
41 
42  Vector rpy,rpy_offs,encoders;
43  Matrix Hc,thumbH,indexH,middleH;
44  Matrix thumbH_deg0,thumbH_deg1,thumbH_deg2;
45  Matrix Hc_thumbH_deg0,Hc_thumbH_deg1,Hc_thumbH_deg2;
46  Vector thumbH_col2,thumbH_col3;
47  Vector indexH_col2,indexH_col3;
48  Vector middleH_col2,middleH_col3;
49 
50  Matrix dHc_dx0,dHc_dx1,dHc_dx2;
51  Matrix dHc_dx3,dHc_dx4,dHc_dx5;
52  Matrix Rz,Ry,Rx;
53  Matrix dRz,dRy,dRx;
54 
55  /****************************************************************/
56  void computeQuantities(const Ipopt::Number *x)
57  {
58  rpy[0]=x[3]+rpy_offs[0];
59  rpy[1]=x[4]+rpy_offs[1];
60  rpy[2]=x[5]+rpy_offs[2];
61  Hc=rpy2dcm(rpy);
62  Hc(0,3)=x[0];
63  Hc(1,3)=x[1];
64  Hc(2,3)=x[2];
65 
66  encoders[0]=x[9];
67  encoders[1]=x[6];
68  encoders[2]=x[7];
69  encoders[3]=x[8];
70  encoders[4]=x[10];
71  encoders[5]=x[11];
72 
73  Vector joints;
74  problem.thumb.getChainJoints(encoders,joints);
75  thumbH=problem.thumb.getH(joints);
76  thumbH_col2=thumbH.getCol(2);
77  thumbH_col3=thumbH.getCol(3);
78  thumbH_deg0=problem.thumb.getH(0,true);
79  thumbH_deg1=problem.thumb.getH(2,true);
80  thumbH_deg2=problem.thumb.getH(3,true);
81 
82  Hc_thumbH_deg0=Hc*thumbH_deg0;
83  Hc_thumbH_deg1=Hc*thumbH_deg1;
84  Hc_thumbH_deg2=Hc*thumbH_deg2;
85 
86  problem.index.getChainJoints(encoders,joints);
87  indexH=problem.index.getH(joints);
88  indexH_col2=indexH.getCol(2);
89  indexH_col3=indexH.getCol(3);
90 
91  if (problem.nFingers==3)
92  {
93  encoders[6]=x[12];
94  encoders[7]=x[13];
95 
96  problem.middle.getChainJoints(encoders,joints);
97  middleH=problem.middle.getH(joints);
98  middleH_col2=middleH.getCol(2);
99  middleH_col3=middleH.getCol(3);
100  }
101 
102  double cr=cos(rpy[0]); double sr=sin(rpy[0]);
103  double cp=cos(rpy[1]); double sp=sin(rpy[1]);
104  double cy=cos(rpy[2]); double sy=sin(rpy[2]);
105 
106  Rz(0,0)=cy; Rz(1,1)=cy; Rz(0,1)=-sy; Rz(1,0)=sy;
107  Ry(0,0)=cp; Ry(2,2)=cp; Ry(0,2)=-sp; Ry(2,0)=sp;
108  Rx(1,1)=cr; Rx(2,2)=cr; Rx(1,2)=-sr; Rx(2,1)=sr;
109 
110  // dHc/dx3 (roll)
111  dRx(1,1)=-sr; dRx(2,2)=-sr; dRx(1,2)=-cr; dRx(2,1)=cr;
112  dHc_dx3=Rz*Ry*dRx;
113 
114  // dHc/dx4 (pitch)
115  dRy(0,0)=-sp; dRy(2,2)=-sp; dRy(0,2)=-cp; dRy(2,0)=cp;
116  dHc_dx4=Rz*dRy*Rx;
117 
118  // dHc/dx5 (yaw)
119  dRz(0,0)=-sy; dRz(1,1)=-sy; dRz(0,1)=-cy; dRz(1,0)=cy;
120  dHc_dx5=dRz*Ry*Rx;
121  }
122 
123 public:
124  /****************************************************************/
125  HandIK_NLP(HandIK_Problem &p) : problem(p), solution(p.nFingers)
126  {
127  encoders.resize(9,0.0);
128  rpy.resize(3,0.0);
129 
130  dHc_dx0=zeros(4,4); dHc_dx0(0,3)=1.0;
131  dHc_dx1=zeros(4,4); dHc_dx1(1,3)=1.0;
132  dHc_dx2=zeros(4,4); dHc_dx2(2,3)=1.0;
133 
134  Rz=Ry=Rx=eye(4,4);
135  dRz=dRy=dRx=zeros(4,4);
136 
137  // ee-rpy offset
138  // they serve to change the frame
139  // from the one relative to the object
140  // to the one relative to the hand
141  // when put palm down on top of the oject
142  rpy_offs.resize(3,0.0);
143  if (problem.hand=="right")
144  {
145  rpy_offs[0]=-M_PI;
146  rpy_offs[1]=0.0;
147  rpy_offs[2]=M_PI/2.0;
148  }
149  else
150  {
151  rpy_offs[0]=0.0;
152  rpy_offs[1]=0.0;
153  rpy_offs[2]=M_PI;
154  }
155  }
156 
157  /****************************************************************/
158  virtual void setInitialGuess(const HandIK_Variables &g)
159  {
160  guess=g;
161  }
162 
163  /****************************************************************/
164  virtual HandIK_Variables getSolution() const
165  {
166  return solution;
167  }
168 
169  /****************************************************************/
170  bool get_nlp_info(Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g,
171  Ipopt::Index &nnz_h_lag, IndexStyleEnum &index_style)
172  {
173  n=problem.nVars;
174  m=3;//+3;
175  nnz_jac_g=3+2*(6+3);//+7+8+9;
176  nnz_h_lag=0;
177 
178  if (problem.nFingers==3)
179  {
180  m+=1;
181  nnz_jac_g+=6+2;
182  }
183 
184  index_style=TNLP::C_STYLE;
185 
186  return true;
187  }
188 
189  /****************************************************************/
190  bool get_bounds_info(Ipopt::Index n, Ipopt::Number *x_l, Ipopt::Number *x_u,
191  Ipopt::Index m, Ipopt::Number *g_l, Ipopt::Number *g_u)
192  {
193  // retrieve the length of the middle finger to be used as relevant distance
194  Vector zeros(9,0.0),joints;
195  problem.middle.getChainJoints(zeros,joints);
196  double d=1.5*norm(problem.middle.EndEffPosition(joints));
197 
198  // ee-xyz
199  if (problem.hand=="right")
200  {
201  x_l[0]=0.0; x_u[0]=problem.dimensions[0]+d;
202  }
203  else
204  {
205  x_l[0]=-problem.dimensions[0]-d; x_u[0]=0.0;
206  }
207  x_l[1]=-problem.dimensions[1]-d; x_u[1]=0.0;//problem.dimensions[1]+d;
208  x_l[2]=0.0;/*-problem.dimensions[2];*/ x_u[2]=problem.dimensions[2]+d;
209 
210  // ee-rpy
211  if (problem.hand=="right")
212  {
213  x_l[3+0]=0.0; x_u[3+0]=M_PI/2.0;
214  x_l[3+1]=-M_PI/2.0; x_u[3+1]=0.0;
215  x_l[3+2]=-M_PI/2.0; x_u[3+2]=M_PI/2.0;
216  }
217  else
218  {
219  x_l[3+0]=-M_PI/2.0; x_u[3+0]=0.0;
220  x_l[3+1]=-M_PI/2.0; x_u[3+1]=0.0;
221  x_l[3+2]=-M_PI/2.0; x_u[3+2]=M_PI/2.0;
222  }
223 
224  // thumb joints
225  iKinChain *chain=problem.thumb.asChain();
226  x_l[6+0]=(*chain)[0].getMin(); x_u[6+0]=(*chain)[0].getMax();
227  x_l[6+1]=(*chain)[2].getMin(); x_u[6+1]=(*chain)[2].getMax();
228  x_l[6+2]=(*chain)[3].getMin(); x_u[6+2]=2.0*(*chain)[3].getMax();
229 
230  // index joints
231  chain=problem.index.asChain();
232  x_l[9+0]=(*chain)[0].getMin(); x_u[9+0]=3.0*(*chain)[0].getMax();
233  x_l[9+1]=(*chain)[1].getMin(); x_u[9+1]=(*chain)[1].getMax();
234  x_l[9+2]=(*chain)[2].getMin(); x_u[9+2]=2.0*(*chain)[2].getMax();
235 
236  // middle joints
237  if (problem.nFingers==3)
238  {
239  chain=problem.middle.asChain();
240  x_l[12+0]=(*chain)[0].getMin(); x_u[12+0]=(*chain)[0].getMax();
241  x_l[12+1]=(*chain)[1].getMin(); x_u[12+1]=2.0*(*chain)[1].getMax();
242  }
243 
244  // ee-xyz out of the ellipsoid
245  g_l[0]=1.0; g_u[0]=1e20;
246 
247  // contact points
248  g_l[1]=g_u[1]=0.0;
249  g_l[2]=g_u[2]=0.0;
250 
251  // thumb out of the ellipsoid
252  /*g_l[3]=1.0; g_u[3]=1e20;
253  g_l[4]=1.0; g_u[4]=1e20;
254  g_l[5]=1.0; g_u[5]=1e20;*/
255 
256  // contact points (middle)
257  if (problem.nFingers==3)
258  g_l[3]=g_u[3]=0.0;
259 
260  return true;
261  }
262 
263  /****************************************************************/
264  bool get_starting_point(Ipopt::Index n, bool init_x, Ipopt::Number *x,
265  bool init_z, Ipopt::Number *z_L, Ipopt::Number *z_U,
266  Ipopt::Index m, bool init_lambda, Ipopt::Number *lambda)
267  {
268  // ee-xyz
269  x[0]=guess.xyz_ee[0];
270  x[1]=guess.xyz_ee[1];
271  x[2]=guess.xyz_ee[2];
272 
273  // ee-rpy
274  x[3+0]=guess.rpy_ee[0]-rpy_offs[0];
275  x[3+1]=guess.rpy_ee[1]-rpy_offs[1];
276  x[3+2]=guess.rpy_ee[2]-rpy_offs[2];
277 
278  // thumb joints
279  x[6+0]=guess.joints[0];
280  x[6+1]=guess.joints[1];
281  x[6+2]=guess.joints[2];
282 
283  // index joints
284  x[9+0]=guess.joints[3+0];
285  x[9+1]=guess.joints[3+1];
286  x[9+2]=guess.joints[3+2];
287 
288  // middle joints
289  if (problem.nFingers==3)
290  {
291  x[12+0]=guess.joints[6+0];
292  x[12+1]=guess.joints[6+1];
293  }
294 
295  return true;
296  }
297 
298  /****************************************************************/
299  bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
300  Ipopt::Number &obj_value)
301  {
302  computeQuantities(x);
303 
304  obj_value=2.0+dot(problem.normalDirs[0],Hc*thumbH_col2)+
305  dot(problem.normalDirs[1],Hc*indexH_col2);
306 
307  //thumb joints far from their maximum
308  /*iKinChain *chain=problem.thumb.asChain();
309  double max_val=2.0*(*chain)[3].getMax();
310  obj_value+=pow(exp(-(max_val-x[6+2])),10);
311 
312  //index joints far from their maximum
313  chain=problem.index.asChain();
314  max_val=2.0*(*chain)[2].getMax();
315  obj_value+=pow(exp(-(max_val-x[9+2])),10);*/
316 
317  if (problem.nFingers==3)
318  {
319  obj_value+=1.0+dot(problem.normalDirs[2],Hc*middleH_col2);
320  //middle joints far from their maximum
321  /*chain=problem.middle.asChain();
322  max_val=2.0*(*chain)[1].getMax();
323  obj_value+=pow(exp(-(max_val-x[12+1])),10);*/
324  }
325 
326  return true;
327  }
328 
329  /****************************************************************/
330  bool eval_grad_f(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
331  Ipopt::Number *grad_f)
332  {
333  computeQuantities(x);
334 
335  // ee-xyz
336  // moving along x,y,z does not affect finger-tips normals
337  grad_f[0]=0.0;
338  grad_f[1]=0.0;
339  grad_f[2]=0.0;
340 
341  // ee-rpy
342  grad_f[3]=dot(problem.normalDirs[0],dHc_dx3*thumbH_col2)+
343  dot(problem.normalDirs[1],dHc_dx3*indexH_col2);
344 
345  grad_f[4]=dot(problem.normalDirs[0],dHc_dx4*thumbH_col2)+
346  dot(problem.normalDirs[1],dHc_dx4*indexH_col2);
347 
348  grad_f[5]=dot(problem.normalDirs[0],dHc_dx5*thumbH_col2)+
349  dot(problem.normalDirs[1],dHc_dx5*indexH_col2);
350 
351  if (problem.nFingers==3)
352  {
353  grad_f[3]+=dot(problem.normalDirs[2],dHc_dx3*middleH_col2);
354  grad_f[4]+=dot(problem.normalDirs[2],dHc_dx4*middleH_col2);
355  grad_f[5]+=dot(problem.normalDirs[2],dHc_dx5*middleH_col2);
356  }
357 
358  // thumb
359  Matrix thumbJ=problem.thumb.AnaJacobian(2).removeRows(4,2);
360  thumbJ.setRow(3,Vector(thumbJ.cols(),0.0));
361  grad_f[6]=dot(problem.normalDirs[0],Hc*thumbJ.getCol(0));
362  grad_f[7]=dot(problem.normalDirs[0],Hc*thumbJ.getCol(1));
363  grad_f[8]=dot(problem.normalDirs[0],Hc*thumbJ.getCol(2))/2.0+
364  dot(problem.normalDirs[0],Hc*thumbJ.getCol(3))/2.0;
365  //thumb joints far from their maximum
366  /*iKinChain *chain=problem.thumb.asChain();
367  double max_val=2.0*(*chain)[3].getMax();
368  grad_f[8]+=-10*pow(exp(-(max_val-x[6+2])),10);*/
369 
370  // index
371  Matrix indexJ=problem.index.AnaJacobian(2).removeRows(4,2);
372  indexJ.setRow(3,Vector(indexJ.cols(),0.0));
373  grad_f[9] =dot(problem.normalDirs[1],Hc*indexJ.getCol(0))/3.0;
374  grad_f[10]=dot(problem.normalDirs[1],Hc*indexJ.getCol(1));
375  grad_f[11]=dot(problem.normalDirs[1],Hc*indexJ.getCol(2))/2.0+
376  dot(problem.normalDirs[1],Hc*indexJ.getCol(3))/2.0;
377  //index joints far from their maximum
378  /*chain=problem.index.asChain();
379  max_val=2.0*(*chain)[2].getMax();
380  grad_f[11]+=-10*pow(exp(-(max_val-x[9+2])),10);*/
381 
382  // middle
383  if (problem.nFingers==3)
384  {
385  Matrix middleJ=problem.middle.AnaJacobian(2).removeRows(4,2);
386  middleJ.setRow(3,Vector(middleJ.cols(),0.0));
387  grad_f[12]=dot(problem.normalDirs[2],Hc*middleJ.getCol(0));
388  grad_f[13]=dot(problem.normalDirs[2],Hc*middleJ.getCol(1))/2.0+
389  dot(problem.normalDirs[2],Hc*middleJ.getCol(2))/2.0;
390  //middle joints far from their maximum
391  /*chain=problem.middle.asChain();
392  max_val=2.0*(*chain)[1].getMax();
393  grad_f[13]+=-10*pow(exp(-(max_val-x[12+1])),10);*/
394  }
395 
396  return true;
397  }
398 
399  /****************************************************************/
400  bool eval_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
401  Ipopt::Index m, Ipopt::Number *g)
402  {
403  computeQuantities(x);
404 
405  // ee-xyz out of the ellipsoid
406  double tmp1,tmp2,tmp3;
407  tmp1=x[0]/problem.dimensions[0];
408  tmp2=x[1]/problem.dimensions[1];
409  tmp3=x[2]/problem.dimensions[2];
410  g[0]=tmp1*tmp1+tmp2*tmp2+tmp3*tmp3;
411 
412  // contact points
413  g[1]=norm2(problem.contactPoints[0]-Hc*thumbH_col3);
414  g[2]=norm2(problem.contactPoints[1]-Hc*indexH_col3);
415 
416  // thumb out of the ellipsoid
417  /*tmp1=Hc_thumbH_deg0(0,3)/problem.dimensions[0];
418  tmp2=Hc_thumbH_deg0(1,3)/problem.dimensions[1];
419  tmp3=Hc_thumbH_deg0(2,3)/problem.dimensions[2];
420  g[3]=tmp1*tmp1+tmp2*tmp2+tmp3*tmp3;
421 
422  tmp1=Hc_thumbH_deg1(0,3)/problem.dimensions[0];
423  tmp2=Hc_thumbH_deg1(1,3)/problem.dimensions[1];
424  tmp3=Hc_thumbH_deg1(2,3)/problem.dimensions[2];
425  g[4]=tmp1*tmp1+tmp2*tmp2+tmp3*tmp3;
426 
427  tmp1=Hc_thumbH_deg2(0,3)/problem.dimensions[0];
428  tmp2=Hc_thumbH_deg2(1,3)/problem.dimensions[1];
429  tmp3=Hc_thumbH_deg2(2,3)/problem.dimensions[2];
430  g[5]=tmp1*tmp1+tmp2*tmp2+tmp3*tmp3;*/
431 
432  // contact points (middle)
433  if (problem.nFingers==3)
434  g[3]=norm2(problem.contactPoints[2]-Hc*middleH_col3);
435 
436  return true;
437  }
438 
439  /****************************************************************/
440  bool eval_jac_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
441  Ipopt::Index m, Ipopt::Index nele_jac, Ipopt::Index *iRow,
442  Ipopt::Index *jCol, Ipopt::Number *values)
443  {
444  int cnt=0;
445 
446  if (values==NULL)
447  {
448  // return the structure of the Jacobian in triplet format
449 
450  // g[0] depends on ee-xyz
451  iRow[cnt]=0; jCol[cnt]=0; cnt++;
452  iRow[cnt]=0; jCol[cnt]=1; cnt++;
453  iRow[cnt]=0; jCol[cnt]=2; cnt++;
454 
455  // g[1] depends on ee-xyz, ee-rpy
456  for (int i=0; i<6; i++, cnt++)
457  {
458  iRow[cnt]=1; jCol[cnt]=i;
459  }
460  // g[1] depends on thumb
461  iRow[cnt]=1; jCol[cnt]=6; cnt++;
462  iRow[cnt]=1; jCol[cnt]=7; cnt++;
463  iRow[cnt]=1; jCol[cnt]=8; cnt++;
464 
465  // g[2] depends on ee-xyz, ee-rpy
466  for (int i=0; i<6; i++, cnt++)
467  {
468  iRow[cnt]=2; jCol[cnt]=i;
469  }
470  // g[2] depends on index
471  iRow[cnt]=2; jCol[cnt]=9; cnt++;
472  iRow[cnt]=2; jCol[cnt]=10; cnt++;
473  iRow[cnt]=2; jCol[cnt]=11; cnt++;
474 
475  // g[3] depends on ee-xyz, ee-rpy
476  /*for (int i=0; i<6; i++, cnt++)
477  {
478  iRow[cnt]=3; jCol[cnt]=i;
479  }
480  // g[3] depends on thumb(0)
481  iRow[cnt]=3; jCol[cnt]=6; cnt++;
482 
483  // g[4] depends on ee-xyz, ee-rpy
484  for (int i=0; i<6; i++, cnt++)
485  {
486  iRow[cnt]=4; jCol[cnt]=i;
487  }
488  // g[4] depends on thumb(0:1)
489  iRow[cnt]=4; jCol[cnt]=6; cnt++;
490  iRow[cnt]=4; jCol[cnt]=7; cnt++;
491 
492  // g[5] depends on ee-xyz, ee-rpy
493  for (int i=0; i<6; i++, cnt++)
494  {
495  iRow[cnt]=5; jCol[cnt]=i;
496  }
497  // g[5] depends on thumb(0:2)
498  iRow[cnt]=5; jCol[cnt]=6; cnt++;
499  iRow[cnt]=5; jCol[cnt]=7; cnt++;
500  iRow[cnt]=5; jCol[cnt]=8; cnt++;*/
501 
502  if (problem.nFingers==3)
503  {
504  // g[6] depends on ee-xyz, ee-rpy
505  for (int i=0; i<6; i++, cnt++)
506  {
507  iRow[cnt]=3; jCol[cnt]=i;
508  }
509  // g[6] depends on middle
510  iRow[cnt]=3; jCol[cnt]=12; cnt++;
511  iRow[cnt]=3; jCol[cnt]=13; cnt++;
512  }
513  }
514  else
515  {
516  computeQuantities(x);
517 
518  // return the values of the Jacobian of the constraints
519  double dim0=(problem.dimensions[0]*problem.dimensions[0])/2.0;
520  double dim1=(problem.dimensions[1]*problem.dimensions[1])/2.0;
521  double dim2=(problem.dimensions[2]*problem.dimensions[2])/2.0;
522 
523  // dg[0]
524  {
525  values[0]=x[0]/dim0;
526  values[1]=x[1]/dim1;
527  values[2]=x[2]/dim2;
528  cnt=3;
529  }
530 
531  // dg[1]
532  {
533  Matrix J=problem.thumb.AnaJacobian(3).removeRows(4,2);
534  J.setRow(3,Vector(J.cols(),0.0));
535  Vector &p=thumbH_col3;
536  Vector tmp=problem.contactPoints[0]-Hc*p;
537 
538  values[cnt+0]=-2.0*dot(tmp,dHc_dx0*p);
539  values[cnt+1]=-2.0*dot(tmp,dHc_dx1*p);
540  values[cnt+2]=-2.0*dot(tmp,dHc_dx2*p);
541  values[cnt+3]=-2.0*dot(tmp,dHc_dx3*p);
542  values[cnt+4]=-2.0*dot(tmp,dHc_dx4*p);
543  values[cnt+5]=-2.0*dot(tmp,dHc_dx5*p);
544  values[cnt+6]=-2.0*dot(tmp,Hc*J.getCol(0));
545  values[cnt+7]=-2.0*dot(tmp,Hc*J.getCol(1));
546  values[cnt+8]=-2.0*dot(tmp,Hc*J.getCol(2))/2.0-dot(tmp,Hc*J.getCol(3))/2.0;
547  cnt+=9;
548  }
549 
550  // dg[2]
551  {
552  Matrix J=problem.index.AnaJacobian(3).removeRows(4,2);
553  J.setRow(3,Vector(J.cols(),0.0));
554  Vector &p=indexH_col3;
555  Vector tmp=problem.contactPoints[1]-Hc*p;
556 
557  values[cnt+0]=-2.0*dot(tmp,dHc_dx0*p);
558  values[cnt+1]=-2.0*dot(tmp,dHc_dx1*p);
559  values[cnt+2]=-2.0*dot(tmp,dHc_dx2*p);
560  values[cnt+3]=-2.0*dot(tmp,dHc_dx3*p);
561  values[cnt+4]=-2.0*dot(tmp,dHc_dx4*p);
562  values[cnt+5]=-2.0*dot(tmp,dHc_dx5*p);
563  values[cnt+6]=-2.0*dot(tmp,Hc*J.getCol(0))/3.0;
564  values[cnt+7]=-2.0*dot(tmp,Hc*J.getCol(1));
565  values[cnt+8]=-2.0*dot(tmp,Hc*J.getCol(2))/2.0-dot(tmp,Hc*J.getCol(3))/2.0;
566  cnt+=9;
567  }
568 
569  // dg[3]
570  /*{
571  Matrix A0=dHc_dx0*thumbH_deg0;
572  Matrix A1=dHc_dx1*thumbH_deg0;
573  Matrix A2=dHc_dx2*thumbH_deg0;
574  Matrix A3=dHc_dx3*thumbH_deg0;
575  Matrix A4=dHc_dx4*thumbH_deg0;
576  Matrix A5=dHc_dx5*thumbH_deg0;
577  Matrix J=problem.thumb.AnaJacobian(0,3).removeRows(4,2);
578  J.setRow(3,Vector(J.cols(),0.0));
579  J=Hc*J;
580 
581  values[cnt+0]=Hc_thumbH_deg0(0,3)*A0(0,3)/dim0+Hc_thumbH_deg0(1,3)*A0(1,3)/dim1+Hc_thumbH_deg0(2,3)*A0(2,3)/dim2;
582  values[cnt+1]=Hc_thumbH_deg0(0,3)*A1(0,3)/dim0+Hc_thumbH_deg0(1,3)*A1(1,3)/dim1+Hc_thumbH_deg0(2,3)*A1(2,3)/dim2;
583  values[cnt+2]=Hc_thumbH_deg0(0,3)*A2(0,3)/dim0+Hc_thumbH_deg0(1,3)*A2(1,3)/dim1+Hc_thumbH_deg0(2,3)*A2(2,3)/dim2;
584  values[cnt+3]=Hc_thumbH_deg0(0,3)*A3(0,3)/dim0+Hc_thumbH_deg0(1,3)*A3(1,3)/dim1+Hc_thumbH_deg0(2,3)*A3(2,3)/dim2;
585  values[cnt+4]=Hc_thumbH_deg0(0,3)*A4(0,3)/dim0+Hc_thumbH_deg0(1,3)*A4(1,3)/dim1+Hc_thumbH_deg0(2,3)*A4(2,3)/dim2;
586  values[cnt+5]=Hc_thumbH_deg0(0,3)*A5(0,3)/dim0+Hc_thumbH_deg0(1,3)*A5(1,3)/dim1+Hc_thumbH_deg0(2,3)*A5(2,3)/dim2;
587  values[cnt+6]=Hc_thumbH_deg0(0,3)*J(0,0)/dim0 +Hc_thumbH_deg0(1,3)*J(1,0)/dim1 +Hc_thumbH_deg0(2,3)*J(2,0)/dim2;
588  cnt+=7;
589  }
590 
591  // dg[4]
592  {
593  Matrix A0=dHc_dx0*thumbH_deg1;
594  Matrix A1=dHc_dx1*thumbH_deg1;
595  Matrix A2=dHc_dx2*thumbH_deg1;
596  Matrix A3=dHc_dx3*thumbH_deg1;
597  Matrix A4=dHc_dx4*thumbH_deg1;
598  Matrix A5=dHc_dx5*thumbH_deg1;
599  Matrix J=problem.thumb.AnaJacobian(2,3).removeRows(4,2);
600  J.setRow(3,Vector(J.cols(),0.0));
601  J=Hc*J;
602 
603  values[cnt+0]=Hc_thumbH_deg1(0,3)*A0(0,3)/dim0+Hc_thumbH_deg1(1,3)*A0(1,3)/dim1+Hc_thumbH_deg1(2,3)*A0(2,3)/dim2;
604  values[cnt+1]=Hc_thumbH_deg1(0,3)*A1(0,3)/dim0+Hc_thumbH_deg1(1,3)*A1(1,3)/dim1+Hc_thumbH_deg1(2,3)*A1(2,3)/dim2;
605  values[cnt+2]=Hc_thumbH_deg1(0,3)*A2(0,3)/dim0+Hc_thumbH_deg1(1,3)*A2(1,3)/dim1+Hc_thumbH_deg1(2,3)*A2(2,3)/dim2;
606  values[cnt+3]=Hc_thumbH_deg1(0,3)*A3(0,3)/dim0+Hc_thumbH_deg1(1,3)*A3(1,3)/dim1+Hc_thumbH_deg1(2,3)*A3(2,3)/dim2;
607  values[cnt+4]=Hc_thumbH_deg1(0,3)*A4(0,3)/dim0+Hc_thumbH_deg1(1,3)*A4(1,3)/dim1+Hc_thumbH_deg1(2,3)*A4(2,3)/dim2;
608  values[cnt+5]=Hc_thumbH_deg1(0,3)*A5(0,3)/dim0+Hc_thumbH_deg1(1,3)*A5(1,3)/dim1+Hc_thumbH_deg1(2,3)*A5(2,3)/dim2;
609  values[cnt+6]=Hc_thumbH_deg1(0,3)*J(0,0)/dim0 +Hc_thumbH_deg1(1,3)*J(1,0)/dim1 +Hc_thumbH_deg1(2,3)*J(2,0)/dim2;
610  values[cnt+7]=Hc_thumbH_deg1(0,3)*J(0,2)/dim0 +Hc_thumbH_deg1(1,3)*J(1,2)/dim1 +Hc_thumbH_deg1(2,3)*J(2,2)/dim2;
611  cnt+=8;
612  }
613 
614  // dg[5]
615  {
616  Matrix A0=dHc_dx0*thumbH_deg2;
617  Matrix A1=dHc_dx1*thumbH_deg2;
618  Matrix A2=dHc_dx2*thumbH_deg2;
619  Matrix A3=dHc_dx3*thumbH_deg2;
620  Matrix A4=dHc_dx4*thumbH_deg2;
621  Matrix A5=dHc_dx5*thumbH_deg2;
622  Matrix J=problem.thumb.AnaJacobian(3,3).removeRows(4,2);
623  J.setRow(3,Vector(J.cols(),0.0));
624  J=Hc*J;
625 
626  values[cnt+0]= Hc_thumbH_deg2(0,3)*A0(0,3)/dim0+Hc_thumbH_deg2(1,3)*A0(1,3)/dim1+Hc_thumbH_deg2(2,3)*A0(2,3)/dim2;
627  values[cnt+1]= Hc_thumbH_deg2(0,3)*A1(0,3)/dim0+Hc_thumbH_deg2(1,3)*A1(1,3)/dim1+Hc_thumbH_deg2(2,3)*A1(2,3)/dim2;
628  values[cnt+2]= Hc_thumbH_deg2(0,3)*A2(0,3)/dim0+Hc_thumbH_deg2(1,3)*A2(1,3)/dim1+Hc_thumbH_deg2(2,3)*A2(2,3)/dim2;
629  values[cnt+3]= Hc_thumbH_deg2(0,3)*A3(0,3)/dim0+Hc_thumbH_deg2(1,3)*A3(1,3)/dim1+Hc_thumbH_deg2(2,3)*A3(2,3)/dim2;
630  values[cnt+4]= Hc_thumbH_deg2(0,3)*A4(0,3)/dim0+Hc_thumbH_deg2(1,3)*A4(1,3)/dim1+Hc_thumbH_deg2(2,3)*A4(2,3)/dim2;
631  values[cnt+5]= Hc_thumbH_deg2(0,3)*A5(0,3)/dim0+Hc_thumbH_deg2(1,3)*A5(1,3)/dim1+Hc_thumbH_deg2(2,3)*A5(2,3)/dim2;
632  values[cnt+6]= Hc_thumbH_deg2(0,3)*J(0,0)/dim0 +Hc_thumbH_deg2(1,3)*J(1,0)/dim1 +Hc_thumbH_deg2(2,3)*J(2,0)/dim2;
633  values[cnt+7]= Hc_thumbH_deg2(0,3)*J(0,2)/dim0 +Hc_thumbH_deg2(1,3)*J(1,2)/dim1 +Hc_thumbH_deg2(2,3)*J(2,2)/dim2;
634  values[cnt+8]=(Hc_thumbH_deg2(0,3)*J(0,3)/dim0 +Hc_thumbH_deg2(1,3)*J(1,3)/dim1 +Hc_thumbH_deg2(2,3)*J(2,3)/dim2)/2.0;
635  cnt+=9;
636  }*/
637 
638  // dg[6]
639  if (problem.nFingers==3)
640  {
641  Matrix J=problem.middle.AnaJacobian(3).removeRows(4,2);
642  J.setRow(3,Vector(J.cols(),0.0));
643  Vector &p=middleH_col3;
644  Vector tmp=problem.contactPoints[2]-Hc*p;
645 
646  values[cnt+0]=-2.0*dot(tmp,dHc_dx0*p);
647  values[cnt+1]=-2.0*dot(tmp,dHc_dx1*p);
648  values[cnt+2]=-2.0*dot(tmp,dHc_dx2*p);
649  values[cnt+3]=-2.0*dot(tmp,dHc_dx3*p);
650  values[cnt+4]=-2.0*dot(tmp,dHc_dx4*p);
651  values[cnt+5]=-2.0*dot(tmp,dHc_dx5*p);
652  values[cnt+6]=-2.0*dot(tmp,Hc*J.getCol(0));
653  values[cnt+7]=-2.0*dot(tmp,Hc*J.getCol(1))/2.0-dot(tmp,Hc*J.getCol(2))/2.0;
654  }
655  }
656 
657  return true;
658  }
659 
660  /****************************************************************/
661  bool eval_h(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
662  Ipopt::Number obj_factor, Ipopt::Index m, const Ipopt::Number *lambda,
663  bool new_lambda, Ipopt::Index nele_hess, Ipopt::Index *iRow,
664  Ipopt::Index *jCol, Ipopt::Number *values)
665  {
666  return true;
667  }
668 
669  /****************************************************************/
670  bool get_scaling_parameters(Ipopt::Number &obj_scaling, bool &use_x_scaling,
671  Ipopt::Index n, Ipopt::Number *x_scaling,
672  bool &use_g_scaling, Ipopt::Index m,
673  Ipopt::Number *g_scaling)
674  {
675  obj_scaling=1.0;
676 
677  use_x_scaling=false;
678  use_g_scaling=true;
679  for (int i=0; i<m; i++)
680  g_scaling[i]=1.0;
681 
682  return true;
683  }
684 
685  /****************************************************************/
686  void finalize_solution(Ipopt::SolverReturn status, Ipopt::Index n,
687  const Ipopt::Number *x, const Ipopt::Number *z_L,
688  const Ipopt::Number *z_U, Ipopt::Index m,
689  const Ipopt::Number *g, const Ipopt::Number *lambda,
690  Ipopt::Number obj_value, const Ipopt::IpoptData *ip_data,
691  Ipopt::IpoptCalculatedQuantities *ip_cq)
692  {
693  // ee-xyz
694  solution.xyz_ee[0]=x[0];
695  solution.xyz_ee[1]=x[1];
696  solution.xyz_ee[2]=x[2];
697 
698  // ee-rpy
699  solution.rpy_ee[0]=x[3+0]+rpy_offs[0];
700  solution.rpy_ee[1]=x[3+1]+rpy_offs[1];
701  solution.rpy_ee[2]=x[3+2]+rpy_offs[2];
702 
703  // thumb joints
704  solution.joints[0]=x[6+0];
705  solution.joints[1]=x[6+1];
706  solution.joints[2]=x[6+2];
707 
708  // index joints
709  solution.joints[3+0]=x[9+0];
710  solution.joints[3+1]=x[9+1];
711  solution.joints[3+2]=x[9+2];
712 
713  // middle joints
714  if (problem.nFingers==3)
715  {
716  solution.joints[6+0]=x[12+0];
717  solution.joints[6+1]=x[12+1];
718  }
719 
720  Ipopt::Number cost_fun;
721  eval_f(n,x,true,cost_fun);
722  solution.cost_fun=cost_fun;
723  }
724 };
725 
726 
727 /****************************************************************/
728 void HandIK_Variables::print()
729 {
730  printf("xyz_ee = (%s) [m]\n",xyz_ee.toString(3,3).c_str());
731  printf("rpy_ee = (%s) [deg]\n",(CTRL_RAD2DEG*rpy_ee).toString(3,3).c_str());
732  printf("joints = (%s) [deg]\n",(CTRL_RAD2DEG*joints).toString(3,3).c_str());
733 }
734 
735 
736 /****************************************************************/
737 bool HandIK_Solver::setInitialGuess(const HandIK_Variables &_guess)
738 {
739  guess=_guess;
740  return true;
741 }
742 
743 
744 /****************************************************************/
745 bool HandIK_Solver::solve(HandIK_Variables &solution)
746 {
747  Ipopt::SmartPtr<Ipopt::IpoptApplication> app=new Ipopt::IpoptApplication;
748  app->Options()->SetNumericValue("tol",1e-8);
749  app->Options()->SetNumericValue("constr_viol_tol",1e-6);
750  app->Options()->SetIntegerValue("acceptable_iter",0);
751  app->Options()->SetStringValue("mu_strategy","adaptive");
752  app->Options()->SetIntegerValue("max_iter",500);
753  //app->Options()->SetStringValue("nlp_scaling_method","gradient-based");
754  //app->Options()->SetStringValue("nlp_scaling_method","none");
755  app->Options()->SetStringValue("nlp_scaling_method","user-scaling");
756  // #####################################
757  //app->Options()->SetStringValue("jacobian_approximation","finite-difference-values");
758  // #####################################
759  app->Options()->SetStringValue("hessian_approximation","limited-memory");
760  //app->Options()->SetIntegerValue("print_level",4);
761  app->Options()->SetIntegerValue("print_level",0);
762  app->Options()->SetStringValue("derivative_test","none");
763  //app->Options()->SetStringValue("derivative_test","first-order");
764  app->Initialize();
765 
766  Ipopt::SmartPtr<HandIK_NLP> nlp=new HandIK_NLP(problem);
767 
768  nlp->setInitialGuess(guess);
769  Ipopt::ApplicationReturnStatus status=app->OptimizeTNLP(GetRawPtr(nlp));
770  solution=nlp->getSolution();
771 
772  return (status==Ipopt::Solve_Succeeded);
773 }
774 
775 
Generated by   doxygen 1.8.12