iCub-main
gazeNlp.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Author: Ugo Pattacini, Alessandro Roncone
4  * email: ugo.pattacini@iit.it, alessandro.roncone@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17 */
18 
19 #include <cmath>
20 #include <limits>
21 #include <algorithm>
22 
23 #include <IpTNLP.hpp>
24 #include <IpIpoptApplication.hpp>
25 
26 #include <iCub/gazeNlp.h>
27 #include <iCub/utils.h>
28 
29 
30 // Describe the nonlinear problem of aligning two vectors
31 // in counterphase for controlling neck movements.
32 class HeadCenter_NLP : public Ipopt::TNLP
33 {
34 private:
35  // Copy constructor: not implemented.
36  HeadCenter_NLP(const HeadCenter_NLP&) = delete;
37  // Assignment operator: not implemented.
38  HeadCenter_NLP &operator=(const HeadCenter_NLP&) = delete;
39 
40 protected:
42  unsigned int dim;
43 
44  Vector &xd;
45  Vector qd;
46  Vector q0;
47  Vector q;
48  Vector qRest;
49  Matrix Hxd;
50  Matrix GeoJacobP;
51  Matrix AnaJacobZ;
52 
53  double mod;
54  double cosAng;
55  double fPitch;
56  double dfPitch;
57 
58  double __obj_scaling;
59  double __x_scaling;
60  double __g_scaling;
61  double lowerBoundInf;
62  double upperBoundInf;
63  bool firstGo;
64 
65  /************************************************************************/
66  void computeQuantities(const Ipopt::Number *x)
67  {
68  Vector new_q(dim);
69  for (Ipopt::Index i=0; i<(int)dim; i++)
70  new_q[i]=x[i];
71 
72  if (!(q==new_q) || firstGo)
73  {
74  firstGo=false;
75  q=new_q;
76 
77  q=chain.setAng(q);
78  Hxd=chain.getH();
79  Hxd(0,3)-=xd[0];
80  Hxd(1,3)-=xd[1];
81  Hxd(2,3)-=xd[2];
82  Hxd(3,3)=0.0;
83  mod=norm(Hxd,3);
84  cosAng=dot(Hxd,2,Hxd,3)/mod;
85 
86  double offset=5.0*CTRL_DEG2RAD;
87  double delta=1.0*CTRL_DEG2RAD;
88  double pitch_cog=chain(0).getMin()+offset+delta/2.0;
89  double c=10.0/delta;
90  double _tanh=tanh(c*(q[0]-pitch_cog));
91 
92  // transition function and its first derivative
93  // to block the roll around qRest[1] when the pitch
94  // approaches its minimum
95  fPitch=0.5*(1.0+_tanh);
96  dfPitch=0.5*c*(1.0-_tanh*_tanh);
97 
100  }
101  }
102 
103 public:
104  /************************************************************************/
105  HeadCenter_NLP(iKinChain &c, const Vector &_q0, Vector &_xd) :
106  chain(c), q0(_q0), xd(_xd)
107  {
108  dim=chain.getDOF();
109  qd.resize(dim,0.0);
110 
111  size_t n=std::min(q0.length(),(size_t)dim);
112  for (size_t i=0; i<n; i++)
113  qd[i]=q0[i];
114 
115  q=qd;
116 
117  firstGo=true;
118 
119  __obj_scaling=1.0;
120  __x_scaling =1.0;
121  __g_scaling =1.0;
122 
125 
126  qRest.resize(dim,0.0);
127  }
128 
129  /************************************************************************/
130  Vector get_qd() { return qd; }
131 
132  /************************************************************************/
133  void set_scaling(double _obj_scaling, double _x_scaling, double _g_scaling)
134  {
135  __obj_scaling=_obj_scaling;
136  __x_scaling =_x_scaling;
137  __g_scaling =_g_scaling;
138  }
139 
140  /************************************************************************/
141  void set_bound_inf(double lower, double upper)
142  {
144  upperBoundInf=upper;
145  }
146 
147  /************************************************************************/
148  bool get_nlp_info(Ipopt::Index& n, Ipopt::Index& m, Ipopt::Index& nnz_jac_g,
149  Ipopt::Index& nnz_h_lag, IndexStyleEnum& index_style) override
150  {
151  n=dim;
152  m=3;
153  nnz_jac_g=n+2*(n-1);
154  nnz_h_lag=0;
155  index_style=TNLP::C_STYLE;
156 
157  return true;
158  }
159 
160  /************************************************************************/
161  bool get_bounds_info(Ipopt::Index n, Ipopt::Number* x_l, Ipopt::Number* x_u,
162  Ipopt::Index m, Ipopt::Number* g_l, Ipopt::Number* g_u) override
163  {
164  for (Ipopt::Index i=0; i<n; i++)
165  {
166  x_l[i]=chain(i).getMin();
167  x_u[i]=chain(i).getMax();
168  }
169 
170  g_l[0]=g_u[0]=-1.0;
171 
172  g_l[1]=g_l[2]=lowerBoundInf;
173  g_u[1]=g_u[2]=0.0;
174 
175  return true;
176  }
177 
178  /************************************************************************/
179  bool get_starting_point(Ipopt::Index n, bool init_x, Ipopt::Number* x, bool init_z,
180  Ipopt::Number* z_L, Ipopt::Number* z_U, Ipopt::Index m,
181  bool init_lambda, Ipopt::Number* lambda) override
182  {
183  for (Ipopt::Index i=0; i<n; i++)
184  x[i]=q0[i];
185 
186  return true;
187  }
188 
189  /************************************************************************/
190  bool eval_f(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
191  Ipopt::Number& obj_value) override
192  {
193  obj_value=0.0;
194 
195  for (Ipopt::Index i=0; i<n; i++)
196  {
197  double tmp=x[i]-qRest[i];
198  obj_value+=tmp*tmp;
199  }
200 
201  obj_value*=0.5;
202 
203  return true;
204  }
205 
206  /************************************************************************/
207  bool eval_grad_f(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
208  Ipopt::Number* grad_f) override
209  {
210  for (Ipopt::Index i=0; i<n; i++)
211  grad_f[i]=x[i]-qRest[i];
212 
213  return true;
214  }
215 
216  /************************************************************************/
217  bool eval_g(Ipopt::Index n, const Ipopt::Number* x, bool new_x, Ipopt::Index m,
218  Ipopt::Number* g) override
219  {
221 
222  g[0]=cosAng;
223  g[1]=(chain(1).getMin()-qRest[1])*fPitch-(x[1]-qRest[1]);
224  g[2]=x[1]-qRest[1]-(chain(1).getMax()-qRest[1])*fPitch;
225 
226  return true;
227  }
228 
229  /************************************************************************/
230  bool eval_jac_g(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
231  Ipopt::Index m, Ipopt::Index nele_jac, Ipopt::Index* iRow,
232  Ipopt::Index *jCol, Ipopt::Number* values) override
233  {
234  if (!values)
235  {
236  iRow[0]=0; jCol[0]=0;
237  iRow[1]=0; jCol[1]=1;
238  iRow[2]=0; jCol[2]=2;
239 
240  iRow[3]=1; jCol[3]=0;
241  iRow[4]=1; jCol[4]=1;
242 
243  iRow[5]=2; jCol[5]=0;
244  iRow[6]=2; jCol[6]=1;
245  }
246  else
247  {
249 
250  // dg[0]/dxi
251  for (Ipopt::Index i=0; i<n; i++)
252  values[i]=(dot(AnaJacobZ,i,Hxd,3)+dot(Hxd,2,GeoJacobP,i))/mod
253  -(cosAng*dot(Hxd,3,GeoJacobP,i))/(mod*mod);
254 
255  // dg[1]/dPitch
256  values[3]=(chain(1).getMin()-qRest[1])*dfPitch;
257 
258  // dg[1]/dRoll
259  values[4]=-1.0;
260 
261  // dg[2]/dPitch
262  values[5]=-(chain(1).getMax()-qRest[1])*dfPitch;
263 
264  // dg[2]/dRoll
265  values[6]=1.0;
266  }
267 
268  return true;
269  }
270 
271  /****************************************************************/
272  bool eval_h(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
273  Ipopt::Number obj_factor, Ipopt::Index m, const Ipopt::Number *lambda,
274  bool new_lambda, Ipopt::Index nele_hess, Ipopt::Index *iRow,
275  Ipopt::Index *jCol, Ipopt::Number *values) override
276  {
277  return true;
278  }
279 
280  /************************************************************************/
281  bool get_scaling_parameters(Ipopt::Number& obj_scaling, bool& use_x_scaling,
282  Ipopt::Index n, Ipopt::Number* x_scaling,
283  bool& use_g_scaling, Ipopt::Index m,
284  Ipopt::Number* g_scaling) override
285  {
286  obj_scaling=__obj_scaling;
287 
288  for (Ipopt::Index i=0; i<n; i++)
289  x_scaling[i]=__x_scaling;
290 
291  for (Ipopt::Index j=0; j<m; j++)
292  g_scaling[j]=__g_scaling;
293 
294  use_x_scaling=use_g_scaling=true;
295 
296  return true;
297  }
298 
299  /************************************************************************/
300  void finalize_solution(Ipopt::SolverReturn status, Ipopt::Index n,
301  const Ipopt::Number* x, const Ipopt::Number* z_L,
302  const Ipopt::Number* z_U, Ipopt::Index m, const Ipopt::Number* g,
303  const Ipopt::Number* lambda, Ipopt::Number obj_value,
304  const Ipopt::IpoptData* ip_data, Ipopt::IpoptCalculatedQuantities* ip_cq) override
305  {
306  for (Ipopt::Index i=0; i<n; i++)
307  qd[i]=x[i];
308 
309  qd=chain.setAng(qd);
310  }
311 
312  /************************************************************************/
313  void setGravityDirection(const Vector &gDir)
314  {
315  Vector gDir_=SE3inv(chain.getH(2,true)).submatrix(0,2,0,2)*gDir;
316 
317  // rest pitch
318  qRest[0]=CTRL_PI/2.0+atan2(gDir_[1],gDir_[0]);
319  qRest[0]=sat(qRest[0],chain(0).getMin(),chain(0).getMax());
320 
321  // rest roll
322  qRest[1]=-CTRL_PI/2.0-atan2(gDir_[1],gDir_[2]);
323  qRest[1]=sat(qRest[1],chain(1).getMin(),chain(1).getMax());
324  }
325 
326  /************************************************************************/
327  virtual ~HeadCenter_NLP() { }
328 };
329 
330 
331 /************************************************************************/
332 Vector GazeIpOptMin::solve(const Vector &q0, Vector &xd, const Vector &gDir)
333 {
334  Ipopt::SmartPtr<HeadCenter_NLP> nlp;
335  nlp=new HeadCenter_NLP(chain,q0,xd);
336 
337  nlp->set_scaling(obj_scaling,x_scaling,g_scaling);
338  nlp->set_bound_inf(lowerBoundInf,upperBoundInf);
339  nlp->setGravityDirection(gDir);
340 
341  static_cast<Ipopt::IpoptApplication*>(App)->OptimizeTNLP(GetRawPtr(nlp));
342  return nlp->get_qd();
343 }
344 
345 
HeadCenter_NLP::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) override
Definition: gazeNlp.cpp:161
HeadCenter_NLP::Hxd
Matrix Hxd
Definition: gazeNlp.cpp:49
python-motor-control.tmp
tmp
Definition: python-motor-control.py:43
iCub::iKin::iKinChain::AnaJacobian
yarp::sig::Matrix AnaJacobian(const unsigned int i, unsigned int col)
Returns the analitical Jacobian of the ith link.
Definition: iKinFwd.cpp:919
HeadCenter_NLP::__obj_scaling
double __obj_scaling
Definition: gazeNlp.cpp:58
HeadCenter_NLP::set_scaling
void set_scaling(double _obj_scaling, double _x_scaling, double _g_scaling)
Definition: gazeNlp.cpp:133
HeadCenter_NLP::GeoJacobP
Matrix GeoJacobP
Definition: gazeNlp.cpp:50
HeadCenter_NLP::HeadCenter_NLP
HeadCenter_NLP(iKinChain &c, const Vector &_q0, Vector &_xd)
Definition: gazeNlp.cpp:105
iCub::iKin::iKinChain
Definition: iKinFwd.h:354
offset
degrees offset
Definition: sine.m:4
CTRL_DEG2RAD
#define CTRL_DEG2RAD
Definition: XSensMTx.cpp:26
HeadCenter_NLP::eval_f
bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number &obj_value) override
Definition: gazeNlp.cpp:190
HeadCenter_NLP::firstGo
bool firstGo
Definition: gazeNlp.cpp:63
iCub::iKin::iKinIpOptMin::x_scaling
double x_scaling
Definition: iKinIpOpt.h:220
HeadCenter_NLP::dfPitch
double dfPitch
Definition: gazeNlp.cpp:56
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
HeadCenter_NLP::qd
Vector qd
Definition: gazeNlp.cpp:45
iCub::iKin::iKinChain::getH
yarp::sig::Matrix getH(const unsigned int i, const bool allLink=false)
Returns the rigid roto-translation matrix from the root reference frame to the ith frame in Denavit-H...
Definition: iKinFwd.cpp:740
iCub::ctrl::dot
double dot(const yarp::sig::Matrix &A, int colA, const yarp::sig::Matrix &B, int colB)
HeadCenter_NLP::dim
unsigned int dim
Definition: gazeNlp.cpp:42
HeadCenter_NLP::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) override
Definition: gazeNlp.cpp:148
iCub::iKin::iKinChain::getDOF
unsigned int getDOF() const
Returns the current number of Chain's DOF.
Definition: iKinFwd.h:557
iCub::iKin::iKinChain::GeoJacobian
yarp::sig::Matrix GeoJacobian(const unsigned int i)
Returns the geometric Jacobian of the ith link.
Definition: iKinFwd.cpp:1020
iCub::iKin::iKinChain::setAng
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
GazeIpOptMin::solve
Vector solve(const Vector &q0, Vector &xd, const Vector &gDir)
Definition: gazeNlp.cpp:332
relationalOperators::lower
bool lower(Value &a, Value &b)
Definition: main.cpp:337
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
iCub::iKin::iKinIpOptMin::g_scaling
double g_scaling
Definition: iKinIpOpt.h:221
HeadCenter_NLP::__g_scaling
double __g_scaling
Definition: gazeNlp.cpp:60
HeadCenter_NLP::q
Vector q
Definition: gazeNlp.cpp:47
HeadCenter_NLP::__x_scaling
double __x_scaling
Definition: gazeNlp.cpp:59
iCub::iKin::iKinIpOptMin::upperBoundInf
double upperBoundInf
Definition: iKinIpOpt.h:223
HeadCenter_NLP::q0
Vector q0
Definition: gazeNlp.cpp:46
iCub::iKin::iKinIpOptMin::obj_scaling
double obj_scaling
Definition: iKinIpOpt.h:219
HeadCenter_NLP::AnaJacobZ
Matrix AnaJacobZ
Definition: gazeNlp.cpp:51
HeadCenter_NLP::upperBoundInf
double upperBoundInf
Definition: gazeNlp.cpp:62
HeadCenter_NLP::computeQuantities
void computeQuantities(const Ipopt::Number *x)
Definition: gazeNlp.cpp:66
n
int n
Definition: debugFunctions.cpp:58
HeadCenter_NLP::fPitch
double fPitch
Definition: gazeNlp.cpp:55
HeadCenter_NLP::get_scaling_parameters
bool get_scaling_parameters(Ipopt::Number &obj_scaling, bool &use_x_scaling, Ipopt::Index n, Ipopt::Number *x_scaling, bool &use_g_scaling, Ipopt::Index m, Ipopt::Number *g_scaling) override
Definition: gazeNlp.cpp:281
iCub::ctrl::norm
double norm(const yarp::sig::Matrix &M, int col)
sat
double sat(const double val, const double min, const double max)
Definition: utils.h:185
x
x
Definition: compute_ekf_sym.m:21
HeadCenter_NLP::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) override
Definition: gazeNlp.cpp:230
iCub::ctrl::CTRL_PI
constexpr double CTRL_PI
The PI constant.
Definition: math.h:56
HeadCenter_NLP::~HeadCenter_NLP
virtual ~HeadCenter_NLP()
Definition: gazeNlp.cpp:327
HeadCenter_NLP::get_qd
Vector get_qd()
Definition: gazeNlp.cpp:130
HeadCenter_NLP::eval_grad_f
bool eval_grad_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number *grad_f) override
Definition: gazeNlp.cpp:207
HeadCenter_NLP::set_bound_inf
void set_bound_inf(double lower, double upper)
Definition: gazeNlp.cpp:141
HeadCenter_NLP::cosAng
double cosAng
Definition: gazeNlp.cpp:54
HeadCenter_NLP::lowerBoundInf
double lowerBoundInf
Definition: gazeNlp.cpp:61
HeadCenter_NLP::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) override
Definition: gazeNlp.cpp:179
HeadCenter_NLP::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) override
Definition: gazeNlp.cpp:272
iCub::iKin::iKinIpOptMin::lowerBoundInf
double lowerBoundInf
Definition: iKinIpOpt.h:222
HeadCenter_NLP::setGravityDirection
void setGravityDirection(const Vector &gDir)
Definition: gazeNlp.cpp:313
HeadCenter_NLP
Definition: gazeNlp.cpp:32
iCub::iKin::iKinIpOptMin::App
void * App
Definition: iKinIpOpt.h:209
iCub::iKin::iKinIpOptMin::chain
iKinChain & chain
Definition: iKinIpOpt.h:211
HeadCenter_NLP::mod
double mod
Definition: gazeNlp.cpp:53
gazeNlp.h
HeadCenter_NLP::chain
iKinChain & chain
Definition: gazeNlp.cpp:41
HeadCenter_NLP::qRest
Vector qRest
Definition: gazeNlp.cpp:48
HeadCenter_NLP::eval_g
bool eval_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Index m, Ipopt::Number *g) override
Definition: gazeNlp.cpp:217
HeadCenter_NLP::xd
Vector & xd
Definition: gazeNlp.cpp:44
HeadCenter_NLP::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) override
Definition: gazeNlp.cpp:300