iCub-main
affinity.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 <algorithm>
19 
20 #include <yarp/math/Math.h>
21 #include <iCub/ctrl/math.h>
23 
24 #include <IpTNLP.hpp>
25 #include <IpIpoptApplication.hpp>
26 
27 using namespace std;
28 using namespace yarp::os;
29 using namespace yarp::sig;
30 using namespace yarp::math;
31 using namespace iCub::ctrl;
32 using namespace iCub::optimization;
33 
34 
35 namespace iCub
36 {
37 
38 namespace optimization
39 {
40 
41 /****************************************************************/
42 inline Matrix computeA(const Ipopt::Number *x)
43 {
44  Matrix A=eye(4,4);
45  A(0,0)=x[0]; A(0,1)=x[3]; A(0,2)=x[6]; A(0,3)=x[9];
46  A(1,0)=x[1]; A(1,1)=x[4]; A(1,2)=x[7]; A(1,3)=x[10];
47  A(2,0)=x[2]; A(2,1)=x[5]; A(2,2)=x[8]; A(2,3)=x[11];
48 
49  return A;
50 }
51 
52 
53 /****************************************************************/
54 class AffinityWithMatchedPointsNLP : public Ipopt::TNLP
55 {
56 protected:
57  const deque<Vector> &p0;
58  const deque<Vector> &p1;
59 
60  deque<Matrix> dA;
61  Matrix min;
62  Matrix max;
63  Matrix A0;
64  Matrix A;
65 
66 public:
67  /****************************************************************/
68  AffinityWithMatchedPointsNLP(const deque<Vector> &_p0,
69  const deque<Vector> &_p1,
70  const Matrix &_min, const Matrix &_max) :
71  p0(_p0), p1(_p1)
72  {
73  min=_min;
74  max=_max;
75  A0=0.5*(min+max);
76 
77  for (int c=0; c<A0.cols(); c++)
78  {
79  for (int r=0; r<A0.rows()-1; r++)
80  {
81  Matrix dA=zeros(4,4); dA(r,c)=1.0;
82  this->dA.push_back(dA);
83  }
84  }
85  }
86 
87  /****************************************************************/
88  virtual void set_A0(const Matrix &A0)
89  {
90  int row_max=(int)std::min(this->A0.rows()-1,A0.rows()-1);
91  int col_max=(int)std::min(this->A0.cols(),A0.cols());
92  this->A0.setSubmatrix(A0.submatrix(0,row_max,0,col_max),0,0);
93  }
94 
95  /****************************************************************/
96  virtual Matrix get_result() const
97  {
98  return A;
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=12;
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  Ipopt::Index i=0;
117  for (int c=0; c<A0.cols(); c++)
118  {
119  for (int r=0; r<A0.rows()-1; r++)
120  {
121  x_l[i]=min(r,c);
122  x_u[i]=max(r,c);
123  i++;
124  }
125  }
126 
127  return true;
128  }
129 
130  /****************************************************************/
131  bool get_starting_point(Ipopt::Index n, bool init_x, Ipopt::Number *x,
132  bool init_z, Ipopt::Number *z_L, Ipopt::Number *z_U,
133  Ipopt::Index m, bool init_lambda, Ipopt::Number *lambda)
134  {
135  Ipopt::Index i=0;
136  for (int c=0; c<A0.cols(); c++)
137  {
138  for (int r=0; r<A0.rows()-1; r++)
139  x[i++]=A0(r,c);
140  }
141 
142  return true;
143  }
144 
145  /****************************************************************/
146  bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
147  Ipopt::Number &obj_value)
148  {
149  Matrix A=computeA(x);
150 
151  obj_value=0.0;
152  if (p0.size()>0)
153  {
154  for (size_t i=0; i<p0.size(); i++)
155  obj_value+=norm2(p1[i]-A*p0[i]);
156 
157  obj_value/=p0.size();
158  }
159 
160  return true;
161  }
162 
163  /****************************************************************/
164  bool eval_grad_f(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
165  Ipopt::Number *grad_f)
166  {
167  Matrix A=computeA(x);
168  for (Ipopt::Index i=0; i<n; i++)
169  grad_f[i]=0.0;
170 
171  if (p0.size()>0)
172  {
173  for (size_t i=0; i<p0.size(); i++)
174  {
175  Vector d=p1[i]-A*p0[i];
176  for (Ipopt::Index j=0; j<n; j++)
177  grad_f[j]-=2.0*dot(d,(dA[j]*p0[i]));
178  }
179 
180  for (Ipopt::Index i=0; i<n; i++)
181  grad_f[i]/=p0.size();
182  }
183 
184  return true;
185  }
186 
187  /****************************************************************/
188  bool eval_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
189  Ipopt::Index m, Ipopt::Number *g)
190  {
191  return true;
192  }
193 
194  /****************************************************************/
195  bool eval_jac_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
196  Ipopt::Index m, Ipopt::Index nele_jac, Ipopt::Index *iRow,
197  Ipopt::Index *jCol, Ipopt::Number *values)
198  {
199  return true;
200  }
201 
202  /****************************************************************/
203  bool eval_h(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
204  Ipopt::Number obj_factor, Ipopt::Index m, const Ipopt::Number *lambda,
205  bool new_lambda, Ipopt::Index nele_hess, Ipopt::Index *iRow,
206  Ipopt::Index *jCol, Ipopt::Number *values)
207  {
208  return true;
209  }
210 
211  /****************************************************************/
212  void finalize_solution(Ipopt::SolverReturn status, Ipopt::Index n,
213  const Ipopt::Number *x, const Ipopt::Number *z_L,
214  const Ipopt::Number *z_U, Ipopt::Index m,
215  const Ipopt::Number *g, const Ipopt::Number *lambda,
216  Ipopt::Number obj_value, const Ipopt::IpoptData *ip_data,
217  Ipopt::IpoptCalculatedQuantities *ip_cq)
218  {
219  A=computeA(x);
220  }
221 };
222 
223 }
224 
225 }
226 
227 
228 /****************************************************************/
229 AffinityWithMatchedPoints::AffinityWithMatchedPoints()
230 {
231  max_iter=300;
232  tol=1e-8;
233 
234  min=max=eye(4,4);
235  for (int c=0; c<min.cols(); c++)
236  {
237  for (int r=0; r<min.rows()-1; r++)
238  {
239  min(r,c)=-1.0;
240  max(r,c)=+1.0;
241  }
242  }
243 
244  A0=0.5*(min+max);
245 }
246 
247 
248 /****************************************************************/
249 void AffinityWithMatchedPoints::setBounds(const Matrix &min,
250  const Matrix &max)
251 {
252  int row_max,col_max;
253 
254  row_max=(int)std::min(this->min.rows()-1,min.rows()-1);
255  col_max=(int)std::min(this->min.cols(),min.cols());
256  this->min.setSubmatrix(min.submatrix(0,row_max,0,col_max),0,0);
257 
258  row_max=(int)std::min(this->max.rows()-1,max.rows()-1);
259  col_max=(int)std::min(this->max.cols(),max.cols());
260  this->max.setSubmatrix(max.submatrix(0,row_max,0,col_max),0,0);
261 }
262 
263 
264 /****************************************************************/
265 double AffinityWithMatchedPoints::evalError(const Matrix &A)
266 {
267  double error=0.0;
268  if (p0.size()>0)
269  {
270  for (size_t i=0; i<p0.size(); i++)
271  error+=norm(p1[i]-A*p0[i]);
272 
273  error/=p0.size();
274  }
275 
276  return error;
277 }
278 
279 
280 /****************************************************************/
281 bool AffinityWithMatchedPoints::addPoints(const Vector &p0,
282  const Vector &p1)
283 {
284  if ((p0.length()>=3) && (p1.length()>=3))
285  {
286  Vector _p0=p0.subVector(0,2); _p0.push_back(1.0);
287  Vector _p1=p1.subVector(0,2); _p1.push_back(1.0);
288 
289  this->p0.push_back(_p0);
290  this->p1.push_back(_p1);
291 
292  return true;
293  }
294  else
295  return false;
296 }
297 
298 
299 /****************************************************************/
300 void AffinityWithMatchedPoints::getPoints(deque<Vector> &p0,
301  deque<Vector> &p1) const
302 {
303  p0=this->p0;
304  p1=this->p1;
305 }
306 
307 
308 /****************************************************************/
309 void AffinityWithMatchedPoints::clearPoints()
310 {
311  p0.clear();
312  p1.clear();
313 }
314 
315 
316 /****************************************************************/
317 bool AffinityWithMatchedPoints::setInitialGuess(const Matrix &A)
318 {
319  int row_max=(int)std::min(A0.rows()-1,A.rows()-1);
320  int col_max=(int)std::min(A0.cols(),A.cols());
321  A0.setSubmatrix(A.submatrix(0,row_max,0,col_max),0,0);
322 
323  return true;
324 }
325 
326 
327 /****************************************************************/
328 bool AffinityWithMatchedPoints::setCalibrationOptions(const Property &options)
329 {
330  if (options.check("max_iter"))
331  max_iter=options.find("max_iter").asInt32();
332 
333  if (options.check("tol"))
334  tol=options.find("tol").asFloat64();
335 
336  return true;
337 }
338 
339 
340 /****************************************************************/
342 {
343  if (p0.size()>0)
344  {
345  Ipopt::SmartPtr<Ipopt::IpoptApplication> app=new Ipopt::IpoptApplication;
346  app->Options()->SetNumericValue("tol",tol);
347  app->Options()->SetIntegerValue("acceptable_iter",0);
348  app->Options()->SetStringValue("mu_strategy","adaptive");
349  app->Options()->SetIntegerValue("max_iter",max_iter);
350  app->Options()->SetStringValue("nlp_scaling_method","gradient-based");
351  app->Options()->SetStringValue("jac_c_constant","yes");
352  app->Options()->SetStringValue("jac_d_constant","yes");
353  app->Options()->SetStringValue("hessian_constant","yes");
354  app->Options()->SetStringValue("hessian_approximation","limited-memory");
355  app->Options()->SetIntegerValue("print_level",0);
356  app->Options()->SetStringValue("derivative_test","none");
357  app->Initialize();
358 
359  Ipopt::SmartPtr<AffinityWithMatchedPointsNLP> nlp=new AffinityWithMatchedPointsNLP(p0,p1,min,max);
360 
361  nlp->set_A0(A0);
362  Ipopt::ApplicationReturnStatus status=app->OptimizeTNLP(GetRawPtr(nlp));
363 
364  A=nlp->get_result();
365  error=evalError(A);
366 
367  return (status==Ipopt::Solve_Succeeded);
368  }
369  else
370  return false;
371 }
372 
373 
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: affinity.cpp:131
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: affinity.cpp:203
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: affinity.cpp:212
bool get_nlp_info(Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g, Ipopt::Index &nnz_h_lag, IndexStyleEnum &index_style)
Definition: affinity.cpp:102
AffinityWithMatchedPointsNLP(const deque< Vector > &_p0, const deque< Vector > &_p1, const Matrix &_min, const Matrix &_max)
Definition: affinity.cpp:68
bool eval_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Index m, Ipopt::Number *g)
Definition: affinity.cpp:188
bool eval_grad_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number *grad_f)
Definition: affinity.cpp:164
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: affinity.cpp:113
bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number &obj_value)
Definition: affinity.cpp:146
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: affinity.cpp:195
virtual void set_A0(const Matrix &A0)
Definition: affinity.cpp:88
zeros(2, 2) eye(2
int n
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).
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
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
Matrix computeA(const Ipopt::Number *x)
Definition: affinity.cpp:42
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
const FSC max
Definition: strain.h:48
const FSC min
Definition: strain.h:49
A
Definition: sine.m:16