iCub-main
main.cpp
Go to the documentation of this file.
1 
15 #include <cmath>
16 #include <iostream>
17 #include <iomanip>
18 
19 #include <yarp/os/SystemClock.h>
20 #include <yarp/sig/Vector.h>
21 #include <yarp/math/Math.h>
22 
23 #include <iCub/iKin/iKinFwd.h>
24 #include <iCub/iKin/iKinIpOpt.h>
25 
26 using namespace std;
27 using namespace yarp::os;
28 using namespace yarp::sig;
29 using namespace yarp::math;
30 using namespace iCub::ctrl;
31 using namespace iCub::iKin;
32 
33 
34 // this inherited class (re-)implements the iCub right arm
35 // but shows at the same time how to handle any generic serial
36 // link chain
37 class genericRightArm : public iKinLimb
38 {
39 public:
41  {
42  allocate("don't care");
43  }
44 
45 protected:
46  virtual void allocate(const string &_type)
47  {
48  // the type is used to discriminate between left and right limb
49 
50  // you have to specify the rototranslational matrix H0 from the origin
51  // to the root reference so as from iCub specs.
52  Matrix H0(4,4);
53  H0.zero();
54  H0(0,1)=-1.0;
55  H0(1,2)=-1.0;
56  H0(2,0)=1.0;
57  H0(3,3)=1.0;
58  setH0(H0);
59 
60  // define the links in standard D-H convention
61  // A, D, alpha, offset(*), min theta, max theta
62  pushLink(new iKinLink( 0.032, 0.0, M_PI/2.0, 0.0, -22.0*CTRL_DEG2RAD, 84.0*CTRL_DEG2RAD));
63  pushLink(new iKinLink( 0.0, -0.0055, M_PI/2.0, -M_PI/2.0, -39.0*CTRL_DEG2RAD, 39.0*CTRL_DEG2RAD));
64  pushLink(new iKinLink(-0.0233647, -0.1433, M_PI/2.0, -105.0*CTRL_DEG2RAD, -59.0*CTRL_DEG2RAD, 59.0*CTRL_DEG2RAD));
65  pushLink(new iKinLink( 0.0, -0.10774, M_PI/2.0, -M_PI/2.0, -95.5*CTRL_DEG2RAD, 5.0*CTRL_DEG2RAD));
66  pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, -M_PI/2.0, 0.0, 160.8*CTRL_DEG2RAD));
67  pushLink(new iKinLink( 0.0, -0.15228, -M_PI/2.0, -105.0*CTRL_DEG2RAD, -37.0*CTRL_DEG2RAD, 90.0*CTRL_DEG2RAD));
68  pushLink(new iKinLink( 0.015, 0.0, M_PI/2.0, 0.0, 5.5*CTRL_DEG2RAD, 106.0*CTRL_DEG2RAD));
69  pushLink(new iKinLink( 0.0, -0.1373, M_PI/2.0, -M_PI/2.0, -90.0*CTRL_DEG2RAD, 90.0*CTRL_DEG2RAD));
70  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, M_PI/2.0, -90.0*CTRL_DEG2RAD, 0.0*CTRL_DEG2RAD));
71  pushLink(new iKinLink( 0.0625, 0.016, 0.0, M_PI, -20.0*CTRL_DEG2RAD, 40.0*CTRL_DEG2RAD));
72  // (*) remind that offset is added to theta before computing the rototranslational matrix
73 
74  // usually the first three links which describes the torso kinematic come
75  // as blocked, i.e. they do not belong to the set of arm's dof.
76  blockLink(0,0.0);
77  blockLink(1,0.0);
78  blockLink(2,0.0);
79  }
80 };
81 
82 
83 int main()
84 {
85  // some useful variables
86  Vector q0,qf,qhat,xf,xhat;
87 
88  // declare the limb
89  genericRightArm genArm;
90 
91  // you can get the same result by creating an iCubArm object;
92  // iKin already provides internally coded limbs for iCub, such as
93  // iCubArm, iCubLeg, iCubEye, ..., along with the proper H0 matrix
94  iCubArm libArm("right");
95 
96  // get a chain on the arm; you can use arm object directly but then some
97  // methods will not be available, such as the access to links through
98  // [] or () operators. This prevent the user from adding/removing links
99  // to iCub limbs as well as changing their properties too easily.
100  // Anyway, arm object is affected by modifications on the chain.
101  iKinChain *chain;
102  if (true) // selector
103  chain=genArm.asChain();
104  else
105  chain=libArm.asChain();
106 
107  // get initial joints configuration
108  q0=chain->getAng();
109 
110  // dump DOF bounds using () operators and set
111  // a second joints configuration in the middle of the compact set.
112  // Remind that angles are expressed in radians
113  qf.resize(chain->getDOF());
114  for (unsigned int i=0; i<chain->getDOF(); i++)
115  {
116  double min=(*chain)(i).getMin();
117  double max=(*chain)(i).getMax();
118  qf[i]=(min+max)/2.0;
119 
120  // last joint set to 1 deg higher than the bound
121  if (i==chain->getDOF()-1)
122  qf[i]=max+1.0*CTRL_DEG2RAD;
123 
124  cout << "joint " << i << " in [" << CTRL_RAD2DEG*min << "," << CTRL_RAD2DEG*max
125  << "] set to " << CTRL_RAD2DEG*qf[i] << endl;
126  }
127 
128  // it is not allowed to overcome the bounds...
129  // ...see the result
130  qf=chain->setAng(qf);
131  cout << "Actual joints set to " << (CTRL_RAD2DEG*qf).toString() << endl;
132  // anyway user can disable the constraints checking by calling
133  // the chain method setAllConstraints(false)
134 
135  // there are three links for the torso which do not belong to the
136  // DOF set since they are blocked. User can access them through [] operators
137  cout << "Torso blocked links at:" << endl;
138  for (unsigned int i=0; i<chain->getN()-chain->getDOF(); i++)
139  cout << CTRL_RAD2DEG*(*chain)[i].getAng() << " ";
140  cout << endl;
141 
142  // user can unblock blocked links augumenting the number of DOF
143  cout << "Unblocking the first torso joint... ";
144  chain->releaseLink(0);
145  cout << chain->getDOF() << " DOFs available" << endl;
146  cout << "Blocking the first torso joint again... ";
147  chain->blockLink(0);
148  cout << chain->getDOF() << " DOFs available" << endl;
149 
150  // retrieve the end-effector pose.
151  // Translational part is in meters.
152  // Rotational part is in axis-angle representation
153  xf=chain->EndEffPose();
154  cout << "Current arm end-effector pose: " << xf.toString() << endl;
155 
156  // go back to the starting joints configuration
157  chain->setAng(q0);
158 
159  // instantiate a IPOPT solver for inverse kinematic
160  // for both translational and rotational part
161  iKinIpOptMin slv(*chain,IKINCTRL_POSE_FULL,1e-3,1e-6,100);
162 
163  // In order to speed up the process, a scaling for the problem
164  // is usually required (a good scaling holds each element of the jacobian
165  // of constraints and the hessian of lagrangian in norm between 0.1 and 10.0).
166  slv.setUserScaling(true,100.0,100.0,100.0);
167 
168  // note how the solver called internally the chain->setAllConstraints(false)
169  // method in order to relax constraints
170  for (unsigned int i=0; i<chain->getN(); i++)
171  {
172  cout << "link " << i << ": " <<
173  (chain->getConstraint(i)?"constrained":"not-constrained") << endl;
174  }
175 
176  // solve for xf starting from current configuration q0
177  double t=SystemClock::nowSystem();
178  qhat=slv.solve(chain->getAng(),xf);
179  double dt=SystemClock::nowSystem()-t;
180 
181  // in general the solved qf is different from the initial qf
182  // due to the redundancy
183  cout << "qhat: " << (CTRL_RAD2DEG*qhat).toString() << endl;
184 
185  // check how much we achieve our goal
186  // note that the chain has been manipulated by the solver,
187  // so it's already in the final configuration
188  xhat=chain->EndEffPose();
189  cout << "Desired arm end-effector pose xf= " << xf.toString() << endl;
190  cout << "Achieved arm end-effector pose K(qhat)= " << xhat.toString() << endl;
191  cout << "||xf-K(qhat)||=" << norm(xf-xhat) << endl;
192  cout << "Solved in " << dt << " [s]" << endl;
193 
194  return 0;
195 }
196 
197 
iKinFwd.h
iCub::iKin::iKinIpOptMin::setUserScaling
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
e
e
Definition: compute_ekf_fast.m:13
iCub::iKin::iKinChain
Definition: iKinFwd.h:354
iCub::iKin::iKinIpOptMin
Definition: iKinIpOpt.h:198
main
int main(int argc, char *argv[])
Definition: main.cpp:31
CTRL_DEG2RAD
#define CTRL_DEG2RAD
Definition: XSensMTx.cpp:26
iCub::iKin::iKinChain::releaseLink
bool releaseLink(const unsigned int i)
Releases the ith Link.
Definition: iKinFwd.cpp:471
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
iCub::iKin::iKinChain::getDOF
unsigned int getDOF() const
Returns the current number of Chain's DOF.
Definition: iKinFwd.h:557
iCub::iKin::iKinChain::setAng
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
iCub::iKin::iCubArm
Definition: iKinFwd.h:1081
IKINCTRL_POSE_FULL
#define IKINCTRL_POSE_FULL
Definition: iKinInv.h:37
iCub::skinManager::toString
std::string toString(const T &t)
Definition: compensator.h:199
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
iCub::iKin::iKinLimb
Definition: iKinFwd.h:873
iCub::iKin::iKinChain::getAng
yarp::sig::Vector getAng()
Returns the current free joint angles values.
Definition: iKinFwd.cpp:619
CTRL_RAD2DEG
#define CTRL_RAD2DEG
Definition: XSensMTx.cpp:25
iCub::iKin
Definition: iKinFwd.h:71
iKinIpOpt.h
iCub::iKin::iKinChain::getConstraint
bool getConstraint(unsigned int i)
Returns the constraint status of ith link.
Definition: iKinFwd.h:523
iCub::iKin::iKinChain::getN
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition: iKinFwd.h:550
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
genericRightArm
Definition: main.cpp:37
xhat
xhat
Definition: compute_ekf_fast.m:15
genericRightArm::genericRightArm
genericRightArm()
Definition: main.cpp:40
iCub::ctrl::norm
double norm(const yarp::sig::Matrix &M, int col)
iCub::iKin::iKinChain::EndEffPose
yarp::sig::Vector EndEffPose(const bool axisRep=true)
Returns the coordinates of end-effector.
Definition: iKinFwd.cpp:858
iCub::iKin::iKinChain::blockLink
bool blockLink(const unsigned int i, double Ang)
Blocks the ith Link at the a certain value of its joint angle.
Definition: iKinFwd.cpp:402
iCub::iKin::iKinIpOptMin::solve
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
iCub::iKin::iKinLimb::asChain
iKinChain * asChain()
Returns a pointer to the Limb seen as Chain object.
Definition: iKinFwd.h:1013
M_PI
#define M_PI
Definition: XSensMTx.cpp:24
genericRightArm::allocate
virtual void allocate(const string &_type)
Definition: main.cpp:46