iCub-main
ipopt.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Department of Robotics Brain and Cognitive Sciences, Istituto Italiano di Tecnologie
3  * Author: Ugo Pattacini
4  * CopyPolicy: Released under the terms of the GNU GPL v2.0.
5  *
6  */
7 
8 // This code is just a white test to verify Ipopt installation
9 
10 #include <cstdio>
11 
12 #include <yarp/os/Time.h>
13 #include <yarp/sig/Vector.h>
14 #include <yarp/math/Math.h>
15 
16 #include <iCub/iKin/iKinFwd.h>
17 #include <iCub/iKin/iKinIpOpt.h>
18 
19 using namespace std;
20 using namespace yarp::sig;
21 using namespace yarp::math;
22 using namespace iCub::iKin;
23 
24 
25 int main()
26 {
27  printf("Ipopt: testing correct installation...\n");
28 
29  iCubArm arm("right");
30  iKinChain *chain=arm.asChain();
31 
32  iKinIpOptMin slv(*chain,IKINCTRL_POSE_XYZ,1e-3,1e-6,100);
33  slv.setUserScaling(true,100.0,100.0,100.0);
34 
35  Vector xf(7,0.0);
36  xf[0]=-0.3;
37  xf[1]=+0.1;
38  xf[2]=+0.1;
39 
40  slv.solve(chain->getAng(),xf);
41  Vector xhat=chain->EndEffPose().subVector(0,2);
42  xf=xf.subVector(0,2);
43  double dist=norm(xf-xhat);
44  bool ok=dist<1e-3;
45 
46  printf("target position = (%s) [m]\n",xf.toString(5,5).c_str());
47  printf("solved position = (%s) [m]\n",xhat.toString(5,5).c_str());
48  printf("distance to target = %g [m] ... ",dist);
49  if (ok)
50  {
51  printf("test successful!\n");
52  return 0;
53  }
54  else
55  {
56  printf("test failed!\n");
57  return 1;
58  }
59 }
STL namespace.
A Base class for defining a Serial Link Chain.
Definition: iKinFwd.h:354
yarp::sig::Vector EndEffPose(const bool axisRep=true)
Returns the coordinates of end-effector.
Definition: iKinFwd.cpp:849
yarp::sig::Vector getAng()
Returns the current free joint angles values.
Definition: iKinFwd.cpp:610
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
virtual yarp::sig::Vector solve(const yarp::sig::Vector &q0, yarp::sig::Vector &xd, double weight2ndTask, yarp::sig::Vector &xd_2nd, yarp::sig::Vector &w_2nd, double weight3rdTask, yarp::sig::Vector &qd_3rd, yarp::sig::Vector &w_3rd, int *exit_code=NULL, bool *exhalt=NULL, iKinIterateCallback *iterate=NULL)
Executes the IpOpt algorithm trying to converge on target.
Definition: iKinIpOpt.cpp:1030
#define IKINCTRL_POSE_XYZ
Definition: iKinInv.h:38
void setUserScaling(const bool useUserScaling, const double _obj_scaling, const double _x_scaling, const double _g_scaling)
Enables/disables user scaling factors.
Definition: iKinIpOpt.cpp:973
iKinChain * asChain()
Returns a pointer to the Limb seen as Chain object.
Definition: iKinFwd.h:1013
A class for defining the iCub Arm.
Definition: iKinFwd.h:1081
int main()
Definition: ipopt.cpp:25
Class for inverting chain&#39;s kinematics based on IpOpt lib.
Definition: iKinIpOpt.h:198