iCub-main
calibReference.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 iCub Facility - Istituto Italiano di Tecnologia
3  * Author: Ugo Pattacini
4  * email: ugo.pattacini@iit.it
5  * Permission is granted to copy, distribute, and/or modify this program
6  * under the terms of the GNU General Public License, version 2 or any
7  * later version published by the Free Software Foundation.
8  *
9  * A copy of the license can be found at
10  * http://www.robotcub.org/icub/license/gpl.txt
11  *
12  * This program is distributed in the hope that it will be useful, but
13  * WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
15  * Public License for more details
16 */
17 
18 #include <cmath>
19 #include <algorithm>
20 
21 #include <yarp/math/Math.h>
22 #include <iCub/ctrl/math.h>
23 #include <iCub/optimization/calibReference.h>
24 
25 #include <IpTNLP.hpp>
26 #include <IpIpoptApplication.hpp>
27 
28 using namespace std;
29 using namespace yarp::os;
30 using namespace yarp::sig;
31 using namespace yarp::math;
32 using namespace iCub::ctrl;
33 using namespace iCub::optimization;
34 
35 
36 namespace iCub
37 {
38 
39 namespace optimization
40 {
41 
42 /****************************************************************/
43 inline Matrix computeH(const Vector &x)
44 {
45  Matrix H=euler2dcm(x.subVector(3,5));
46  H(0,3)=x[0]; H(1,3)=x[1]; H(2,3)=x[2];
47 
48  return H;
49 }
50 
51 
52 /****************************************************************/
53 inline Matrix computeH(const Ipopt::Number *x)
54 {
55  Vector _x(6);
56  for (size_t i=0; i<_x.length(); i++)
57  _x[i]=x[i];
58 
59  return computeH(_x);
60 }
61 
62 
63 /****************************************************************/
64 class CalibReferenceWithMatchedPointsNLP : public Ipopt::TNLP
65 {
66 protected:
67  const deque<Vector> &p0;
68  const deque<Vector> &p1;
69 
70  Vector min;
71  Vector max;
72  Vector x0;
73  Vector x;
74 
75 public:
76  /****************************************************************/
77  CalibReferenceWithMatchedPointsNLP(const deque<Vector> &_p0,
78  const deque<Vector> &_p1,
79  const Vector &_min, const Vector &_max) :
80  p0(_p0), p1(_p1)
81  {
82  min=_min;
83  max=_max;
84  x0=0.5*(min+max);
85  }
86 
87  /****************************************************************/
88  virtual void set_x0(const Vector &x0)
89  {
90  size_t len=std::min(this->x0.length(),x0.length());
91  for (size_t i=0; i<len; i++)
92  this->x0[i]=x0[i];
93  }
94 
95  /****************************************************************/
96  virtual Vector get_result() const
97  {
98  return x;
99  }
100 
101  /****************************************************************/
102  bool get_nlp_info(Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g,
103  Ipopt::Index &nnz_h_lag, IndexStyleEnum &index_style)
104  {
105  n=6;
106  m=nnz_jac_g=nnz_h_lag=0;
107  index_style=TNLP::C_STYLE;
108 
109  return true;
110  }
111 
112  /****************************************************************/
113  bool get_bounds_info(Ipopt::Index n, Ipopt::Number *x_l, Ipopt::Number *x_u,
114  Ipopt::Index m, Ipopt::Number *g_l, Ipopt::Number *g_u)
115  {
116  for (Ipopt::Index i=0; i<n; i++)
117  {
118  x_l[i]=min[i];
119  x_u[i]=max[i];
120  }
121 
122  return true;
123  }
124 
125  /****************************************************************/
126  bool get_starting_point(Ipopt::Index n, bool init_x, Ipopt::Number *x,
127  bool init_z, Ipopt::Number *z_L, Ipopt::Number *z_U,
128  Ipopt::Index m, bool init_lambda, Ipopt::Number *lambda)
129  {
130  for (Ipopt::Index i=0; i<n; i++)
131  x[i]=x0[i];
132 
133  return true;
134  }
135 
136  /****************************************************************/
137  bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
138  Ipopt::Number &obj_value)
139  {
140  Matrix H=computeH(x);
141 
142  obj_value=0.0;
143  if (p0.size()>0)
144  {
145  for (size_t i=0; i<p0.size(); i++)
146  obj_value+=norm2(p1[i]-H*p0[i]);
147 
148  obj_value/=p0.size();
149  }
150 
151  return true;
152  }
153 
154  /****************************************************************/
155  bool eval_grad_f(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
156  Ipopt::Number *grad_f)
157  {
158  double ca=cos(x[3]); double sa=sin(x[3]);
159  double cb=cos(x[4]); double sb=sin(x[4]);
160  double cg=cos(x[5]); double sg=sin(x[5]);
161 
162  Matrix Rza=eye(4,4);
163  Rza(0,0)=ca; Rza(1,1)=ca; Rza(1,0)=sa; Rza(0,1)=-sa;
164  Matrix dRza=zeros(4,4);
165  dRza(0,0)=-sa; dRza(1,1)=-sa; dRza(1,0)=ca; dRza(0,1)=-ca;
166 
167  Matrix Rzg=eye(4,4);
168  Rzg(0,0)=cg; Rzg(1,1)=cg; Rzg(1,0)=sg; Rzg(0,1)=-sg;
169  Matrix dRzg=zeros(4,4);
170  dRzg(0,0)=-sg; dRzg(1,1)=-sg; dRzg(1,0)=cg; dRzg(0,1)=-cg;
171 
172  Matrix Ryb=eye(4,4);
173  Ryb(0,0)=cb; Ryb(2,2)=cb; Ryb(2,0)=-sb; Ryb(0,2)=sb;
174  Matrix dRyb=zeros(4,4);
175  dRyb(0,0)=-sb; dRyb(2,2)=-sb; dRyb(2,0)=-cb; dRyb(0,2)=cb;
176 
177  Matrix dHdx0=zeros(4,4); dHdx0(0,3)=1.0;
178  Matrix dHdx1=zeros(4,4); dHdx1(1,3)=1.0;
179  Matrix dHdx2=zeros(4,4); dHdx2(2,3)=1.0;
180  Matrix dHdx3=dRza*Ryb*Rzg;
181  Matrix dHdx4=Rza*dRyb*Rzg;
182  Matrix dHdx5=Rza*Ryb*dRzg;
183 
184  Matrix H=computeH(x);
185  grad_f[0]=grad_f[1]=grad_f[2]=0.0;
186  grad_f[3]=grad_f[4]=grad_f[5]=0.0;
187  if (p0.size()>0)
188  {
189  for (size_t i=0; i<p0.size(); i++)
190  {
191  Vector d=p1[i]-H*p0[i];
192  grad_f[0]-=2.0*dot(d,(dHdx0*p0[i]));
193  grad_f[1]-=2.0*dot(d,(dHdx1*p0[i]));
194  grad_f[2]-=2.0*dot(d,(dHdx2*p0[i]));
195  grad_f[3]-=2.0*dot(d,(dHdx3*p0[i]));
196  grad_f[4]-=2.0*dot(d,(dHdx4*p0[i]));
197  grad_f[5]-=2.0*dot(d,(dHdx5*p0[i]));
198  }
199 
200  for (Ipopt::Index i=0; i<n; i++)
201  grad_f[i]/=p0.size();
202  }
203 
204  return true;
205  }
206 
207  /****************************************************************/
208  bool eval_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
209  Ipopt::Index m, Ipopt::Number *g)
210  {
211  return true;
212  }
213 
214  /****************************************************************/
215  bool eval_jac_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
216  Ipopt::Index m, Ipopt::Index nele_jac, Ipopt::Index *iRow,
217  Ipopt::Index *jCol, Ipopt::Number *values)
218  {
219  return true;
220  }
221 
222  /****************************************************************/
223  bool eval_h(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
224  Ipopt::Number obj_factor, Ipopt::Index m, const Ipopt::Number *lambda,
225  bool new_lambda, Ipopt::Index nele_hess, Ipopt::Index *iRow,
226  Ipopt::Index *jCol, Ipopt::Number *values)
227  {
228  return true;
229  }
230 
231  /****************************************************************/
232  void finalize_solution(Ipopt::SolverReturn status, Ipopt::Index n,
233  const Ipopt::Number *x, const Ipopt::Number *z_L,
234  const Ipopt::Number *z_U, Ipopt::Index m,
235  const Ipopt::Number *g, const Ipopt::Number *lambda,
236  Ipopt::Number obj_value, const Ipopt::IpoptData *ip_data,
237  Ipopt::IpoptCalculatedQuantities *ip_cq)
238  {
239  this->x.resize(n);
240  for (Ipopt::Index i=0; i<n; i++)
241  this->x[i]=x[i];
242  }
243 };
244 
245 
246 /****************************************************************/
247 class CalibReferenceWithScaledMatchedPointsNLP : public CalibReferenceWithMatchedPointsNLP
248 {
249 public:
250  /****************************************************************/
251  CalibReferenceWithScaledMatchedPointsNLP(const deque<Vector> &_p0,
252  const deque<Vector> &_p1,
253  const Vector &_min, const Vector &_max) :
254  CalibReferenceWithMatchedPointsNLP(_p0,_p1,_min,_max) { }
255 
256  /****************************************************************/
257  bool get_nlp_info(Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g,
258  Ipopt::Index &nnz_h_lag, IndexStyleEnum &index_style)
259  {
260  CalibReferenceWithMatchedPointsNLP::get_nlp_info(n,m,nnz_jac_g,nnz_h_lag,index_style);
261  n=6+3;
262 
263  return true;
264  }
265 
266  /****************************************************************/
267  bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
268  Ipopt::Number &obj_value)
269  {
270  Matrix H=computeH(x);
271  Vector s(4);
272  s[0]=x[6];
273  s[1]=x[7];
274  s[2]=x[8];
275  s[3]=1.0;
276 
277  obj_value=0.0;
278  if (p0.size()>0)
279  {
280  for (size_t i=0; i<p0.size(); i++)
281  obj_value+=norm2(p1[i]-s*(H*p0[i]));
282 
283  obj_value/=p0.size();
284  }
285 
286  return true;
287  }
288 
289  /****************************************************************/
290  bool eval_grad_f(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
291  Ipopt::Number *grad_f)
292  {
293  double ca=cos(x[3]); double sa=sin(x[3]);
294  double cb=cos(x[4]); double sb=sin(x[4]);
295  double cg=cos(x[5]); double sg=sin(x[5]);
296 
297  Matrix Rza=eye(4,4);
298  Rza(0,0)=ca; Rza(1,1)=ca; Rza(1,0)=sa; Rza(0,1)=-sa;
299  Matrix dRza=zeros(4,4);
300  dRza(0,0)=-sa; dRza(1,1)=-sa; dRza(1,0)=ca; dRza(0,1)=-ca;
301 
302  Matrix Rzg=eye(4,4);
303  Rzg(0,0)=cg; Rzg(1,1)=cg; Rzg(1,0)=sg; Rzg(0,1)=-sg;
304  Matrix dRzg=zeros(4,4);
305  dRzg(0,0)=-sg; dRzg(1,1)=-sg; dRzg(1,0)=cg; dRzg(0,1)=-cg;
306 
307  Matrix Ryb=eye(4,4);
308  Ryb(0,0)=cb; Ryb(2,2)=cb; Ryb(2,0)=-sb; Ryb(0,2)=sb;
309  Matrix dRyb=zeros(4,4);
310  dRyb(0,0)=-sb; dRyb(2,2)=-sb; dRyb(2,0)=-cb; dRyb(0,2)=cb;
311 
312  Matrix dHdx0=zeros(4,4); dHdx0(0,3)=1.0;
313  Matrix dHdx1=zeros(4,4); dHdx1(1,3)=1.0;
314  Matrix dHdx2=zeros(4,4); dHdx2(2,3)=1.0;
315  Matrix dHdx3=dRza*Ryb*Rzg;
316  Matrix dHdx4=Rza*dRyb*Rzg;
317  Matrix dHdx5=Rza*Ryb*dRzg;
318 
319  Matrix H=computeH(x);
320  Matrix dHdx6=zeros(4,4); dHdx6(0,0)=1.0; dHdx6*=H;
321  Matrix dHdx7=zeros(4,4); dHdx7(1,1)=1.0; dHdx7*=H;
322  Matrix dHdx8=zeros(4,4); dHdx8(2,2)=1.0; dHdx8*=H;
323 
324  Vector s(4);
325  s[0]=x[6];
326  s[1]=x[7];
327  s[2]=x[8];
328  s[3]=1.0;
329 
330  grad_f[0]=grad_f[1]=grad_f[2]=0.0;
331  grad_f[3]=grad_f[4]=grad_f[5]=0.0;
332  grad_f[6]=grad_f[7]=grad_f[8]=0.0;
333  if (p0.size()>0)
334  {
335  for (size_t i=0; i<p0.size(); i++)
336  {
337  Vector d=p1[i]-s*(H*p0[i]);
338  grad_f[0]-=2.0*dot(d,(dHdx0*p0[i]));
339  grad_f[1]-=2.0*dot(d,(dHdx1*p0[i]));
340  grad_f[2]-=2.0*dot(d,(dHdx2*p0[i]));
341  grad_f[3]-=2.0*dot(d,(dHdx3*p0[i]));
342  grad_f[4]-=2.0*dot(d,(dHdx4*p0[i]));
343  grad_f[5]-=2.0*dot(d,(dHdx5*p0[i]));
344  grad_f[6]-=2.0*dot(d,(dHdx6*p0[i]));
345  grad_f[7]-=2.0*dot(d,(dHdx7*p0[i]));
346  grad_f[8]-=2.0*dot(d,(dHdx8*p0[i]));
347  }
348 
349  for (Ipopt::Index i=0; i<n; i++)
350  grad_f[i]/=p0.size();
351  }
352 
353  return true;
354  }
355 };
356 
357 
358 /****************************************************************/
359 class CalibReferenceWithScalarScaledMatchedPointsNLP : public CalibReferenceWithMatchedPointsNLP
360 {
361 public:
362  /****************************************************************/
363  CalibReferenceWithScalarScaledMatchedPointsNLP(const deque<Vector> &_p0,
364  const deque<Vector> &_p1,
365  const Vector &_min, const Vector &_max) :
366  CalibReferenceWithMatchedPointsNLP(_p0,_p1,_min,_max) { }
367 
368  /****************************************************************/
369  bool get_nlp_info(Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g,
370  Ipopt::Index &nnz_h_lag, IndexStyleEnum &index_style)
371  {
372  CalibReferenceWithMatchedPointsNLP::get_nlp_info(n,m,nnz_jac_g,nnz_h_lag,index_style);
373  n=6+1;
374 
375  return true;
376  }
377 
378  /****************************************************************/
379  bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
380  Ipopt::Number &obj_value)
381  {
382  Matrix H=computeH(x);
383  Vector s(4);
384  s[0]=s[1]=s[2]=x[6];
385  s[3]=1.0;
386 
387  obj_value=0.0;
388  if (p0.size()>0)
389  {
390  for (size_t i=0; i<p0.size(); i++)
391  obj_value+=norm2(p1[i]-s*(H*p0[i]));
392 
393  obj_value/=p0.size();
394  }
395 
396  return true;
397  }
398 
399  /****************************************************************/
400  bool eval_grad_f(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
401  Ipopt::Number *grad_f)
402  {
403  double ca=cos(x[3]); double sa=sin(x[3]);
404  double cb=cos(x[4]); double sb=sin(x[4]);
405  double cg=cos(x[5]); double sg=sin(x[5]);
406 
407  Matrix Rza=eye(4,4);
408  Rza(0,0)=ca; Rza(1,1)=ca; Rza(1,0)=sa; Rza(0,1)=-sa;
409  Matrix dRza=zeros(4,4);
410  dRza(0,0)=-sa; dRza(1,1)=-sa; dRza(1,0)=ca; dRza(0,1)=-ca;
411 
412  Matrix Rzg=eye(4,4);
413  Rzg(0,0)=cg; Rzg(1,1)=cg; Rzg(1,0)=sg; Rzg(0,1)=-sg;
414  Matrix dRzg=zeros(4,4);
415  dRzg(0,0)=-sg; dRzg(1,1)=-sg; dRzg(1,0)=cg; dRzg(0,1)=-cg;
416 
417  Matrix Ryb=eye(4,4);
418  Ryb(0,0)=cb; Ryb(2,2)=cb; Ryb(2,0)=-sb; Ryb(0,2)=sb;
419  Matrix dRyb=zeros(4,4);
420  dRyb(0,0)=-sb; dRyb(2,2)=-sb; dRyb(2,0)=-cb; dRyb(0,2)=cb;
421 
422  Matrix dHdx0=zeros(4,4); dHdx0(0,3)=1.0;
423  Matrix dHdx1=zeros(4,4); dHdx1(1,3)=1.0;
424  Matrix dHdx2=zeros(4,4); dHdx2(2,3)=1.0;
425  Matrix dHdx3=dRza*Ryb*Rzg;
426  Matrix dHdx4=Rza*dRyb*Rzg;
427  Matrix dHdx5=Rza*Ryb*dRzg;
428 
429  Matrix H=computeH(x);
430  Matrix dHdx6=zeros(4,4); dHdx6(0,0)=1.0; dHdx6*=H;
431 
432  Vector s(4);
433  s[0]=s[1]=s[2]=x[6];
434  s[3]=1.0;
435 
436  grad_f[0]=grad_f[1]=grad_f[2]=0.0;
437  grad_f[3]=grad_f[4]=grad_f[5]=0.0;
438  grad_f[6]=0.0;
439  if (p0.size()>0)
440  {
441  for (size_t i=0; i<p0.size(); i++)
442  {
443  Vector d=p1[i]-s*(H*p0[i]);
444  grad_f[0]-=2.0*dot(d,(dHdx0*p0[i]));
445  grad_f[1]-=2.0*dot(d,(dHdx1*p0[i]));
446  grad_f[2]-=2.0*dot(d,(dHdx2*p0[i]));
447  grad_f[3]-=2.0*dot(d,(dHdx3*p0[i]));
448  grad_f[4]-=2.0*dot(d,(dHdx4*p0[i]));
449  grad_f[5]-=2.0*dot(d,(dHdx5*p0[i]));
450  grad_f[6]-=2.0*dot(d,(dHdx6*p0[i]));
451  }
452 
453  for (Ipopt::Index i=0; i<n; i++)
454  grad_f[i]/=p0.size();
455  }
456 
457  return true;
458  }
459 };
460 
461 }
462 
463 }
464 
465 
466 /****************************************************************/
467 CalibReferenceWithMatchedPoints::CalibReferenceWithMatchedPoints()
468 {
469  max_iter=300;
470  tol=1e-8;
471 
472  min.resize(6); max.resize(6);
473  min[0]=-1.0; max[0]=1.0;
474  min[1]=-1.0; max[1]=1.0;
475  min[2]=-1.0; max[2]=1.0;
476  min[3]=-M_PI; max[3]=M_PI;
477  min[4]=-M_PI; max[4]=M_PI;
478  min[5]=-M_PI; max[5]=M_PI;
479 
480  min_s.resize(3); max_s.resize(3);
481  min_s[0]=0.1; max_s[0]=10.0;
482  min_s[1]=0.1; max_s[1]=10.0;
483  min_s[2]=0.1; max_s[2]=10.0;
484 
485  min_s_scalar=0.1; max_s_scalar=10.0;
486 
487  x0=0.5*(min+max);
488  s0.resize(3,1.0);
489  s0_scalar=1.0;
490 }
491 
492 
493 /****************************************************************/
494 void CalibReferenceWithMatchedPoints::setBounds(const Matrix &min,
495  const Matrix &max)
496 {
497  if ((min.rows()<3) || (min.cols()<3) ||
498  (max.rows()<3) || (max.cols()<3))
499  return;
500 
501  this->min[0]=min(0,3); this->max[0]=max(0,3);
502  this->min[1]=min(1,3); this->max[1]=max(1,3);
503  this->min[2]=min(2,3); this->max[2]=max(2,3);
504 
505  this->min[3]=min(0,0); this->max[3]=max(0,0);
506  this->min[4]=min(0,1); this->max[4]=max(0,1);
507  this->min[5]=min(0,2); this->max[5]=max(0,2);
508 }
509 
510 
511 /****************************************************************/
512 void CalibReferenceWithMatchedPoints::setBounds(const Vector &min,
513  const Vector &max)
514 {
515  size_t len_min=std::min(this->min.length(),min.length());
516  size_t len_max=std::min(this->max.length(),max.length());
517 
518  for (size_t i=0; i<len_min; i++)
519  this->min[i]=min[i];
520 
521  for (size_t i=0; i<len_max; i++)
522  this->max[i]=max[i];
523 }
524 
525 
526 /****************************************************************/
527 void CalibReferenceWithMatchedPoints::setScalingBounds(const Vector &min,
528  const Vector &max)
529 {
530  size_t len_min=std::min(this->min_s.length(),min.length());
531  size_t len_max=std::min(this->max_s.length(),max.length());
532 
533  for (size_t i=0; i<len_min; i++)
534  this->min_s[i]=min[i];
535 
536  for (size_t i=0; i<len_max; i++)
537  this->max_s[i]=max[i];
538 }
539 
540 
541 /****************************************************************/
542 void CalibReferenceWithMatchedPoints::setScalingBounds(const double min,
543  const double max)
544 {
545  min_s_scalar=min;
546  max_s_scalar=max;
547 }
548 
549 
550 /****************************************************************/
551 double CalibReferenceWithMatchedPoints::evalError(const Matrix &H)
552 {
553  double error=0.0;
554  if (p0.size()>0)
555  {
556  for (size_t i=0; i<p0.size(); i++)
557  error+=norm(p1[i]-H*p0[i]);
558 
559  error/=p0.size();
560  }
561 
562  return error;
563 }
564 
565 
566 /****************************************************************/
567 bool CalibReferenceWithMatchedPoints::addPoints(const Vector &p0,
568  const Vector &p1)
569 {
570  if ((p0.length()>=3) && (p1.length()>=3))
571  {
572  Vector _p0=p0.subVector(0,2); _p0.push_back(1.0);
573  Vector _p1=p1.subVector(0,2); _p1.push_back(1.0);
574 
575  this->p0.push_back(_p0);
576  this->p1.push_back(_p1);
577 
578  return true;
579  }
580  else
581  return false;
582 }
583 
584 
585 /****************************************************************/
586 void CalibReferenceWithMatchedPoints::getPoints(deque<Vector> &p0,
587  deque<Vector> &p1) const
588 {
589  p0=this->p0;
590  p1=this->p1;
591 }
592 
593 
594 /****************************************************************/
595 void CalibReferenceWithMatchedPoints::clearPoints()
596 {
597  p0.clear();
598  p1.clear();
599 }
600 
601 
602 /****************************************************************/
603 bool CalibReferenceWithMatchedPoints::setInitialGuess(const Matrix &H)
604 {
605  if ((H.rows()>=4) && (H.cols()>=4))
606  {
607  Vector euler=dcm2euler(H);
608  x0[0]=H(0,3); x0[1]=H(1,3); x0[2]=H(2,3);
609  x0[3]=euler[0]; x0[4]=euler[1]; x0[5]=euler[2];
610 
611  return true;
612  }
613  else
614  return false;
615 }
616 
617 
618 /****************************************************************/
619 bool CalibReferenceWithMatchedPoints::setScalingInitialGuess(const Vector &s)
620 {
621  if (s.length()>=s0.length())
622  {
623  s0=s.subVector(0,(unsigned int)s0.length()-1);
624  return true;
625  }
626  else
627  return false;
628 }
629 
630 
631 /****************************************************************/
632 bool CalibReferenceWithMatchedPoints::setScalingInitialGuess(const double s)
633 {
634  s0_scalar=s;
635  return true;
636 }
637 
638 
639 /****************************************************************/
640 bool CalibReferenceWithMatchedPoints::setCalibrationOptions(const Property &options)
641 {
642  if (options.check("max_iter"))
643  max_iter=options.find("max_iter").asInt32();
644 
645  if (options.check("tol"))
646  tol=options.find("tol").asFloat64();
647 
648  return true;
649 }
650 
651 
652 /****************************************************************/
653 bool CalibReferenceWithMatchedPoints::calibrate(Matrix &H, double &error)
654 {
655  if (p0.size()>0)
656  {
657  Ipopt::SmartPtr<Ipopt::IpoptApplication> app=new Ipopt::IpoptApplication;
658  app->Options()->SetNumericValue("tol",tol);
659  app->Options()->SetIntegerValue("acceptable_iter",0);
660  app->Options()->SetStringValue("mu_strategy","adaptive");
661  app->Options()->SetIntegerValue("max_iter",max_iter);
662  app->Options()->SetStringValue("nlp_scaling_method","gradient-based");
663  app->Options()->SetStringValue("hessian_approximation","limited-memory");
664  app->Options()->SetIntegerValue("print_level",0);
665  app->Options()->SetStringValue("derivative_test","none");
666  app->Initialize();
667 
668  Ipopt::SmartPtr<CalibReferenceWithMatchedPointsNLP> nlp=new CalibReferenceWithMatchedPointsNLP(p0,p1,min,max);
669 
670  nlp->set_x0(x0);
671  Ipopt::ApplicationReturnStatus status=app->OptimizeTNLP(GetRawPtr(nlp));
672 
673  Vector x=nlp->get_result();
674  H=computeH(x);
675  error=evalError(H);
676 
677  return (status==Ipopt::Solve_Succeeded);
678  }
679  else
680  return false;
681 }
682 
683 
684 /****************************************************************/
685 bool CalibReferenceWithMatchedPoints::calibrate(Matrix &H, Vector &s,
686  double &error)
687 {
688  if (p0.size()>0)
689  {
690  Ipopt::SmartPtr<Ipopt::IpoptApplication> app=new Ipopt::IpoptApplication;
691  app->Options()->SetNumericValue("tol",tol);
692  app->Options()->SetIntegerValue("acceptable_iter",0);
693  app->Options()->SetStringValue("mu_strategy","adaptive");
694  app->Options()->SetIntegerValue("max_iter",max_iter);
695  app->Options()->SetStringValue("nlp_scaling_method","gradient-based");
696  app->Options()->SetStringValue("hessian_approximation","limited-memory");
697  app->Options()->SetIntegerValue("print_level",0);
698  app->Options()->SetStringValue("derivative_test","none");
699  app->Initialize();
700 
701  Ipopt::SmartPtr<CalibReferenceWithScaledMatchedPointsNLP> nlp=new CalibReferenceWithScaledMatchedPointsNLP(p0,p1,cat(min,min_s),cat(max,max_s));
702 
703  nlp->set_x0(cat(x0,s0));
704  Ipopt::ApplicationReturnStatus status=app->OptimizeTNLP(GetRawPtr(nlp));
705 
706  Vector x=nlp->get_result();
707  H=computeH(x);
708  s=x.subVector(6,8);
709  Matrix S=eye(4,4);
710  S(0,0)=s[0]; S(1,1)=s[1]; S(2,2)=s[2];
711  error=evalError(S*H);
712 
713  return (status==Ipopt::Solve_Succeeded);
714  }
715  else
716  return false;
717 }
718 
719 
720 /****************************************************************/
721 bool CalibReferenceWithMatchedPoints::calibrate(Matrix &H, double &s,
722  double &error)
723 {
724  if (p0.size()>0)
725  {
726  Ipopt::SmartPtr<Ipopt::IpoptApplication> app=new Ipopt::IpoptApplication;
727  app->Options()->SetNumericValue("tol",tol);
728  app->Options()->SetIntegerValue("acceptable_iter",0);
729  app->Options()->SetStringValue("mu_strategy","adaptive");
730  app->Options()->SetIntegerValue("max_iter",max_iter);
731  app->Options()->SetStringValue("nlp_scaling_method","gradient-based");
732  app->Options()->SetStringValue("hessian_approximation","limited-memory");
733  app->Options()->SetIntegerValue("print_level",0);
734  app->Options()->SetStringValue("derivative_test","none");
735  app->Initialize();
736 
737  Ipopt::SmartPtr<CalibReferenceWithScalarScaledMatchedPointsNLP> nlp=new CalibReferenceWithScalarScaledMatchedPointsNLP(p0,p1,cat(min,min_s_scalar),cat(max,max_s_scalar));
738 
739  nlp->set_x0(cat(x0,s0_scalar));
740  Ipopt::ApplicationReturnStatus status=app->OptimizeTNLP(GetRawPtr(nlp));
741 
742  Vector x=nlp->get_result();
743  H=computeH(x);
744  s=x[6];
745  Matrix S=eye(4,4);
746  S(0,0)=S(1,1)=S(2,2)=s;
747  error=evalError(S*H);
748 
749  return (status==Ipopt::Solve_Succeeded);
750  }
751  else
752  return false;
753 }
754 
755 
M_PI
#define M_PI
Definition: XSensMTx.cpp:24
iCub::optimization::CalibReferenceWithMatchedPointsNLP::get_starting_point
bool get_starting_point(Ipopt::Index n, bool init_x, Ipopt::Number *x, bool init_z, Ipopt::Number *z_L, Ipopt::Number *z_U, Ipopt::Index m, bool init_lambda, Ipopt::Number *lambda)
Definition: calibReference.cpp:126
iCub::optimization::CalibReferenceWithMatchedPointsNLP::get_nlp_info
bool get_nlp_info(Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g, Ipopt::Index &nnz_h_lag, IndexStyleEnum &index_style)
Definition: calibReference.cpp:102
iCub::optimization::CalibReferenceWithMatchedPointsNLP::CalibReferenceWithMatchedPointsNLP
CalibReferenceWithMatchedPointsNLP(const deque< Vector > &_p0, const deque< Vector > &_p1, const Vector &_min, const Vector &_max)
Definition: calibReference.cpp:77
iCub::optimization::CalibReferenceWithMatchedPointsNLP::eval_g
bool eval_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Index m, Ipopt::Number *g)
Definition: calibReference.cpp:208
iCub::optimization::CalibReferenceWithMatchedPointsNLP::eval_h
bool eval_h(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number obj_factor, Ipopt::Index m, const Ipopt::Number *lambda, bool new_lambda, Ipopt::Index nele_hess, Ipopt::Index *iRow, Ipopt::Index *jCol, Ipopt::Number *values)
Definition: calibReference.cpp:223
iCub::optimization::CalibReferenceWithMatchedPointsNLP::finalize_solution
void finalize_solution(Ipopt::SolverReturn status, Ipopt::Index n, const Ipopt::Number *x, const Ipopt::Number *z_L, const Ipopt::Number *z_U, Ipopt::Index m, const Ipopt::Number *g, const Ipopt::Number *lambda, Ipopt::Number obj_value, const Ipopt::IpoptData *ip_data, Ipopt::IpoptCalculatedQuantities *ip_cq)
Definition: calibReference.cpp:232
iCub::optimization::CalibReferenceWithMatchedPointsNLP::eval_grad_f
bool eval_grad_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number *grad_f)
Definition: calibReference.cpp:155
iCub::optimization::CalibReferenceWithMatchedPointsNLP::eval_jac_g
bool eval_jac_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Index m, Ipopt::Index nele_jac, Ipopt::Index *iRow, Ipopt::Index *jCol, Ipopt::Number *values)
Definition: calibReference.cpp:215
iCub::optimization::CalibReferenceWithMatchedPointsNLP::eval_f
bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number &obj_value)
Definition: calibReference.cpp:137
iCub::optimization::CalibReferenceWithMatchedPointsNLP::get_bounds_info
bool get_bounds_info(Ipopt::Index n, Ipopt::Number *x_l, Ipopt::Number *x_u, Ipopt::Index m, Ipopt::Number *g_l, Ipopt::Number *g_u)
Definition: calibReference.cpp:113
iCub::optimization::CalibReferenceWithScalarScaledMatchedPointsNLP::eval_grad_f
bool eval_grad_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number *grad_f)
Definition: calibReference.cpp:400
iCub::optimization::CalibReferenceWithScalarScaledMatchedPointsNLP::CalibReferenceWithScalarScaledMatchedPointsNLP
CalibReferenceWithScalarScaledMatchedPointsNLP(const deque< Vector > &_p0, const deque< Vector > &_p1, const Vector &_min, const Vector &_max)
Definition: calibReference.cpp:363
iCub::optimization::CalibReferenceWithScalarScaledMatchedPointsNLP::get_nlp_info
bool get_nlp_info(Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g, Ipopt::Index &nnz_h_lag, IndexStyleEnum &index_style)
Definition: calibReference.cpp:369
iCub::optimization::CalibReferenceWithScalarScaledMatchedPointsNLP::eval_f
bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number &obj_value)
Definition: calibReference.cpp:379
iCub::optimization::CalibReferenceWithScaledMatchedPointsNLP::get_nlp_info
bool get_nlp_info(Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g, Ipopt::Index &nnz_h_lag, IndexStyleEnum &index_style)
Definition: calibReference.cpp:257
iCub::optimization::CalibReferenceWithScaledMatchedPointsNLP::CalibReferenceWithScaledMatchedPointsNLP
CalibReferenceWithScaledMatchedPointsNLP(const deque< Vector > &_p0, const deque< Vector > &_p1, const Vector &_min, const Vector &_max)
Definition: calibReference.cpp:251
iCub::optimization::CalibReferenceWithScaledMatchedPointsNLP::eval_grad_f
bool eval_grad_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number *grad_f)
Definition: calibReference.cpp:290
iCub::optimization::CalibReferenceWithScaledMatchedPointsNLP::eval_f
bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number &obj_value)
Definition: calibReference.cpp:267
e
e
Definition: compute_ekf_fast.m:13
H
H
Definition: compute_ekf_fast.m:27
zeros
zeros(2, 2) eye(2
x
x
Definition: compute_ekf_sym.m:21
n
int n
Definition: debugFunctions.cpp:58
iCub::ctrl::dot
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).
iCub::ctrl::norm2
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:91
iCub::ctrl::norm
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
iCub::optimization::computeH
Matrix computeH(const Ipopt::Number *x)
Definition: calibReference.cpp:53
iCub
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
Definition: emotionInterface.h:17
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
Generated on Wed Nov 13 2024 21:11:41 for iCub-main by doxygen 1.9.1