iCub-main
pointing_far.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 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 <string>
19 #include <cmath>
20 #include <limits>
21 
22 #include <IpTNLP.hpp>
23 #include <IpIpoptApplication.hpp>
24 
25 #include <yarp/os/Property.h>
26 #include <yarp/os/LogStream.h>
27 #include <yarp/dev/CartesianControl.h>
28 #include <yarp/sig/Vector.h>
29 #include <yarp/sig/Matrix.h>
30 #include <yarp/math/Math.h>
31 
32 #include <iCub/iKin/iKinFwd.h>
33 #include <iCub/pointing_far.h>
34 
35 #define RAD2DEG (180.0/M_PI)
36 #define DEG2RAD (M_PI/180.0)
37 
38 using namespace std;
39 using namespace yarp::os;
40 using namespace yarp::dev;
41 using namespace yarp::sig;
42 using namespace yarp::math;
43 using namespace iCub::iKin;
44 
45 
46 /*************************************************************************/
47 class PointingFarNLP : public Ipopt::TNLP
48 {
49  iCubArm *finger_tip;
50  iCubArm *finger_base;
51  iCubFinger *finger;
52  Vector point;
53 
54 public:
55  /*********************************************************************/
56  PointingFarNLP(ICartesianControl *iarm) :
57  point(3,0.0)
58  {
59  Bottle info;
60  iarm->getInfo(info);
61  string hand=info.find("arm_type").asString();
62  finger_tip=new iCubArm(hand);
63  finger_base=new iCubArm(hand);
64 
65  size_t underscore=hand.find('_');
66  if (underscore!=string::npos)
67  hand=hand.substr(0,underscore);
68 
69  // update joints limits
70  for (unsigned int i=0; i<finger_tip->getN(); i++)
71  {
72  double min,max;
73  iarm->getLimits(i,&min,&max);
74  (*finger_tip->asChain())[i].setMin(DEG2RAD*min);
75  (*finger_tip->asChain())[i].setMax(DEG2RAD*max);
76  (*finger_base->asChain())[i].setMin(DEG2RAD*min);
77  (*finger_base->asChain())[i].setMax(DEG2RAD*max);
78  }
79 
80  // finger_tip: block torso
81  finger_tip->blockLink(0,0.0);
82  finger_tip->blockLink(1,0.0);
83  finger_tip->blockLink(2,0.0);
84 
85  // finger_tip: block wrist
86  finger_tip->blockLink(8,0.0);
87  finger_tip->blockLink(9,0.0);
88 
89  // finger_base: block torso
90  finger_base->blockLink(0,0.0);
91  finger_base->blockLink(1,0.0);
92  finger_base->blockLink(2,0.0);
93 
94  // finger_base: block wrist
95  finger_base->blockLink(8,0.0);
96  finger_base->blockLink(9,0.0);
97 
98  // remove all constraints
99  finger_tip->asChain()->setAllConstraints(false);
100  finger_base->asChain()->setAllConstraints(false);
101 
102  finger=new iCubFinger(hand+"_index");
103  finger->asChain()->setAllConstraints(false);
104  setFingerJoints(zeros(16));
105  }
106 
107  /*********************************************************************/
108  virtual ~PointingFarNLP()
109  {
110  delete finger_tip;
111  delete finger_base;
112  delete finger;
113  }
114 
115  /*********************************************************************/
116  void setPoint(const Vector& point)
117  {
118  this->point=point;
119  }
120 
121  /*********************************************************************/
122  void setFingerJoints(const Vector& finger_joints)
123  {
124  Vector chain_joints;
125  finger->getChainJoints(finger_joints,chain_joints);
126  finger->setAng(DEG2RAD*chain_joints);
127 
128  // add final transformations
129  finger_tip->asChain()->setHN(finger->getH());
130  finger_base->asChain()->setHN(finger->getH0());
131  }
132 
133  /*********************************************************************/
134  void getResult(Vector &q, Vector &x) const
135  {
136  q.resize(finger_tip->getN());
137  for (unsigned int i=0; i<q.length(); i++)
138  q[i]=RAD2DEG*finger_tip->getAng(i);
139  x=finger_tip->EndEffPose();
140  }
141 
142  /*********************************************************************/
143  bool get_nlp_info(Ipopt::Index& n, Ipopt::Index& m, Ipopt::Index& nnz_jac_g,
144  Ipopt::Index& nnz_h_lag, IndexStyleEnum& index_style)
145  {
146  n=finger_tip->getDOF();
147  m=2;
148  nnz_jac_g=m*n;
149  nnz_h_lag=0;
150  index_style=TNLP::C_STYLE;
151  return true;
152  }
153 
154  /*********************************************************************/
155  bool get_bounds_info(Ipopt::Index n, Ipopt::Number* x_l, Ipopt::Number* x_u,
156  Ipopt::Index m, Ipopt::Number* g_l, Ipopt::Number* g_u)
157  {
158  for (Ipopt::Index i=0; i<n; i++)
159  {
160  x_l[i]=(*finger_tip->asChain())(i).getMin();
161  x_u[i]=(*finger_tip->asChain())(i).getMax();
162  }
163 
164  g_l[0]=1.0;
166 
167  g_l[1]=g_u[1]=0.0;
168  return true;
169  }
170 
171  /*********************************************************************/
172  bool get_starting_point(Ipopt::Index n, bool init_x, Ipopt::Number* x,
173  bool init_z, Ipopt::Number* z_L, Ipopt::Number* z_U,
174  Ipopt::Index m, bool init_lambda, Ipopt::Number* lambda)
175  {
176  for (Ipopt::Index i=0; i<n; i++)
177  x[i]=0.5*((*finger_tip->asChain())(i).getMin()+
178  (*finger_tip->asChain())(i).getMax());
179  return true;
180  }
181 
182  /*********************************************************************/
183  void setAng(const Ipopt::Number* x)
184  {
185  Vector q(finger_tip->getDOF());
186  for (size_t i=0; i<q.length(); i++)
187  q[i]=x[i];
188  finger_tip->setAng(q);
189  finger_base->setAng(q);
190  }
191 
192  /*********************************************************************/
193  bool eval_f(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
194  Ipopt::Number& obj_value)
195  {
196  obj_value=x[3]*x[3];
197  return true;
198  }
199 
200  /*********************************************************************/
201  bool eval_grad_f(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
202  Ipopt::Number* grad_f)
203  {
204  for (Ipopt::Index i=0; i<n; i++)
205  grad_f[i]=(i==3?2.0*x[3]:0.0);
206  return true;
207  }
208 
209  /*********************************************************************/
210  bool eval_g(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
211  Ipopt::Index m, Ipopt::Number* g)
212  {
213  setAng(x);
214 
215  Vector fingerBasePos=finger_base->EndEffPosition();
216  Vector pb_dir=point-fingerBasePos;
217 
218  Matrix tip=finger_tip->getH();
219  Vector tb_dir=tip.getCol(3).subVector(0,2)-fingerBasePos;
220  g[0]=dot(pb_dir,tb_dir)/(norm(pb_dir)*norm(tb_dir));
221 
222  double e=-1.0-tip(2,2);
223  g[1]=e*e;
224  return true;
225  }
226 
227  /*********************************************************************/
228  bool eval_jac_g(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
229  Ipopt::Index m, Ipopt::Index nele_jac, Ipopt::Index* iRow,
230  Ipopt::Index *jCol, Ipopt::Number* values)
231  {
232  if (!values)
233  {
234  Ipopt::Index idx=0;
235  for (Ipopt::Index i=0; i<n; i++,idx++)
236  {
237  iRow[idx]=0;
238  jCol[idx]=i;
239  }
240  for (Ipopt::Index i=0; i<n; i++,idx++)
241  {
242  iRow[idx]=1;
243  jCol[idx]=i;
244  }
245  }
246  else
247  {
248  setAng(x);
249  Vector fingerBasePos=finger_base->EndEffPosition();
250  Vector pb_dir=point-fingerBasePos;
251 
252  Matrix tip=finger_tip->getH();
253  Vector tb_dir=tip.getCol(3).subVector(0,2)-fingerBasePos;
254 
255  Matrix dpb_dir=-1.0*finger_base->AnaJacobian().removeRows(3,3);
256  Matrix dtb_dir=finger_tip->AnaJacobian().removeRows(3,3)+dpb_dir;
257 
258  double npb=norm(pb_dir);
259  double ntb=norm(tb_dir);
260  double nn=npb*ntb;
261  double tmp1=dot(pb_dir,tb_dir);
262  double tmp2=ntb/npb;
263  double tmp3=nn*nn;
264 
265  for (Ipopt::Index i=0; i<n; i++)
266  values[i]=(dot(dpb_dir.getCol(i),tb_dir)+dot(pb_dir,dtb_dir.getCol(i)))/nn-
267  tmp1*(dot(pb_dir,dpb_dir.getCol(i))*tmp2)/tmp3;
268 
269  double e=-1.0-tip(2,2);
270  Matrix dtipZ=finger_tip->AnaJacobian(2);
271  for (Ipopt::Index i=0; i<n; i++)
272  values[n+i]=-2.0*e*dtipZ(2,i);
273  }
274  return true;
275  }
276 
277  /*********************************************************************/
278  bool eval_h(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
279  Ipopt::Number obj_factor, Ipopt::Index m,
280  const Ipopt::Number *lambda, bool new_lambda,
281  Ipopt::Index nele_hess, Ipopt::Index *iRow,
282  Ipopt::Index *jCol, Ipopt::Number *values)
283  {
284  return true;
285  }
286 
287  /*********************************************************************/
288  void finalize_solution(Ipopt::SolverReturn status, Ipopt::Index n,
289  const Ipopt::Number* x, const Ipopt::Number* z_L,
290  const Ipopt::Number* z_U, Ipopt::Index m,
291  const Ipopt::Number* g, const Ipopt::Number* lambda,
292  Ipopt::Number obj_value, const Ipopt::IpoptData* ip_data,
293  Ipopt::IpoptCalculatedQuantities* ip_cq)
294  {
295  }
296 };
297 
298 
299 /*************************************************************************/
300 bool PointingFar::compute(ICartesianControl *iarm, const Property& requirements,
301  Vector& q, Vector& x)
302 {
303  if (iarm==NULL)
304  {
305  yError()<<"Cartesian controller not configured";
306  return false;
307  }
308 
309  Bottle *bPoint=requirements.find("point").asList();
310  if (bPoint==NULL)
311  {
312  yError()<<"Target point not provided";
313  return false;
314  }
315 
316  Vector point(3,0.0);
317  for (int i=0; i<bPoint->size(); i++)
318  point[i]=bPoint->get(i).asFloat64();
319 
320  Ipopt::SmartPtr<Ipopt::IpoptApplication> app=new Ipopt::IpoptApplication;
321  app->Options()->SetNumericValue("tol",0.01);
322  app->Options()->SetNumericValue("constr_viol_tol",0.01);
323  app->Options()->SetIntegerValue("acceptable_iter",0);
324  app->Options()->SetStringValue("mu_strategy","adaptive");
325  app->Options()->SetIntegerValue("max_iter",100);
326  app->Options()->SetStringValue("nlp_scaling_method","gradient-based");
327  app->Options()->SetStringValue("hessian_approximation","limited-memory");
328  app->Options()->SetStringValue("derivative_test","none");
329  app->Options()->SetIntegerValue("print_level",0);
330  app->Initialize();
331 
332  Ipopt::SmartPtr<PointingFarNLP> nlp=new PointingFarNLP(iarm);
333 
334  // specify requirements
335  nlp->setPoint(point);
336  if (Bottle *bFingerJoints=requirements.find("finger-joints").asList())
337  {
338  Vector finger_joints(bFingerJoints->size());
339  for (size_t i=0; i<finger_joints.length(); i++)
340  finger_joints[i]=bFingerJoints->get(i).asFloat64();
341  nlp->setFingerJoints(finger_joints);
342  }
343 
344  Ipopt::ApplicationReturnStatus status=app->OptimizeTNLP(GetRawPtr(nlp));
345  nlp->getResult(q,x);
346  return true;
347 }
348 
349 
350 /*************************************************************************/
351 bool PointingFar::point(ICartesianControl *iarm, const Vector& q, const Vector& x)
352 {
353  if ((q.length()>=10) && (x.length()>=7))
354  {
355  int context;
356  iarm->storeContext(&context);
357 
358  // specify all joints
359  Vector dof(10,1.0);
360  iarm->setDOF(dof,dof);
361  for (unsigned int i=0; i<q.length(); i++)
362  iarm->setLimits(i,q[i],q[i]);
363 
364  double traj;
365  iarm->getTrajTime(&traj);
366  iarm->setInTargetTol(0.02);
367  iarm->goToPoseSync(x.subVector(0,2),x.subVector(3,6));
368  iarm->waitMotionDone(0.2,5.0*traj);
369  iarm->stopControl();
370 
371  iarm->restoreContext(context);
372  iarm->deleteContext(context);
373  return true;
374  }
375  else
376  return false;
377 }
378 
379 
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)
PointingFarNLP(ICartesianControl *iarm)
bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number &obj_value)
virtual ~PointingFarNLP()
bool eval_grad_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number *grad_f)
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)
bool eval_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Index m, Ipopt::Number *g)
void setFingerJoints(const Vector &finger_joints)
void setAng(const Ipopt::Number *x)
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)
void setPoint(const Vector &point)
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)
bool get_nlp_info(Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g, Ipopt::Index &nnz_h_lag, IndexStyleEnum &index_style)
void getResult(Vector &q, Vector &x) const
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)
A class for defining the iCub Arm.
Definition: iKinFwd.h:1194
A class for defining the iCub Finger.
Definition: iKinFwd.h:1234
virtual bool getChainJoints(const yarp::sig::Vector &motorEncoders, yarp::sig::Vector &chainJoints)
Retrieves the vector of actual finger's joint values (to be used in conjuction with the iKinLimb meth...
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:732
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:580
yarp::sig::Vector EndEffPosition()
Returns the 3D coordinates of end-effector position.
Definition: iKinFwd.cpp:894
yarp::sig::Matrix AnaJacobian(const unsigned int i, unsigned int col)
Returns the analitical Jacobian of the ith link.
Definition: iKinFwd.cpp:911
yarp::sig::Vector EndEffPose(const bool axisRep=true)
Returns the coordinates of end-effector.
Definition: iKinFwd.cpp:850
yarp::sig::Vector getAng()
Returns the current free joint angles values.
Definition: iKinFwd.cpp:611
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
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
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition: iKinFwd.h:550
unsigned int getDOF() const
Returns the current number of Chain's DOF.
Definition: iKinFwd.h:557
bool blockLink(const unsigned int i, double Ang)
Blocks the ith Link at the a certain value of its joint angle.
Definition: iKinFwd.cpp:394
void setAllConstraints(bool _constrained)
Sets the constraint status of all chain links.
Definition: iKinFwd.cpp:498
iKinChain * asChain()
Returns a pointer to the Limb seen as Chain object.
Definition: iKinFwd.h:1013
zeros(2, 2) eye(2
int n
static uint32_t idx[BOARD_NUM]
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 norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
const FSC max
Definition: strain.h:48
const FSC min
Definition: strain.h:49
#define RAD2DEG
#define DEG2RAD
static bool compute(yarp::dev::ICartesianControl *iarm, const yarp::os::Property &requirements, yarp::sig::Vector &q, yarp::sig::Vector &x)
static bool point(yarp::dev::ICartesianControl *iarm, const yarp::sig::Vector &q, const yarp::sig::Vector &x)