iCub-main
main.cpp
Go to the documentation of this file.
1 
7 //
8 // An example on finding a multi-limb Jacobians exploiting iDynNode.
9 // A node with torso, head, a left and right arm is created. Then the Jacobian
10 // and the end-effector pose are computed, for different couples of limbs.
11 //
12 // Author: Serena Ivaldi - <serena.ivaldi@iit.it>
13 
14 #include <cmath>
15 #include <iostream>
16 #include <iomanip>
17 
18 #include <yarp/sig/Vector.h>
19 #include <yarp/sig/Matrix.h>
20 #include <iCub/ctrl/math.h>
21 #include <iCub/iKin/iKinFwd.h>
22 #include <iCub/iDyn/iDyn.h>
23 #include <iCub/iDyn/iDynBody.h>
24 #include <iCub/skinDynLib/common.h>
25 
26 using namespace std;
27 using namespace yarp;
28 using namespace yarp::sig;
29 using namespace iCub::ctrl;
30 using namespace iCub::iKin;
31 using namespace iCub::iDyn;
32 using namespace iCub::skinDynLib;
33 
34 
35 // the class we'll use for this example is inherited from iDynNode, and is basically the same as
36 // iCubUpperBody (see iDynBody).
37 // even if the arms have a FT sensor, in this example we're just using kinematics computations
38 // and we're not using the sensor at all, so there's no need to use a iDynSensorNode.
39 // we start building an empty node, then adding a right arm (index 0), a torso limb (index 1),
40 // a head (index 2) and a left arm (index 3).
41 // it's important to remember the indeces because they identify the limb during computations.
42 // as usual, one can create its own limbs (iDynLimb), but here we use icub arms, torso and head.
43 
44 class UpTorso : public iDynNode
45 {
46 public:
47 
52 
53  // construct the node
55  :iDynNode("node with arms, torso and head")
56  {
57  //first create the limbs
58  arm_right = new iCubArmNoTorsoDyn("right",KINFWD_WREBWD);
59  torso = new iCubTorsoDyn("lower",KINBWD_WREBWD);
61  arm_left = new iCubArmNoTorsoDyn("left",KINFWD_WREBWD);
62 
63  // then create their rigid-body transformations matrices
64  Matrix Harm_right(4,4); Harm_right.eye();
65  Matrix Htorso(4,4); Htorso.eye();
66  Matrix Hhead(4,4); Hhead.eye();
67  Matrix Harm_left(4,4); Harm_left.eye();
68 
69  // an example of RBT matrix
70  double theta = CTRL_DEG2RAD * (180.0-15.0);
71  Harm_right(0,0) = -cos(theta); Harm_right(0,1) = 0.0; Harm_right(0,2) = -sin(theta); Harm_right(0,3) = 0.00294;
72  Harm_right(1,0) = 0.0; Harm_right(1,1) = -1.0; Harm_right(1,2) = 0.0; Harm_right(1,3) = -0.050;
73  Harm_right(2,0) = -sin(theta); Harm_right(2,1) = 0.0; Harm_right(2,2) = cos(theta); Harm_right(2,3) = 0.10997;
74  Harm_right(3,3) = 1.0;
75 
76  // this is important:
77  // iCubTorsoDyn is a standalone limb, with a non-initialized base: its H0 matrix is eye()
78  // conversely, iCubArm and iCubArmDyn are referred to the torso base, with a particular H0 matrix
79  // (0 -1 00; 00 -1 0; 1 000; 0001) by row
80  // if we want to obtain the same Jacobian and pose in the arm as in iCubArm/Dyn (iKin style)
81  // we must set the same H0 in the torso limb before making any computation
82  // note that in iCUbUpperBody H0 has already been set: hence we only set the new H0 in our test
83  // class armWithTorso
84  Matrix H0(4,4);
85  H0.zero(); H0(0,1)=-1.0; H0(1,2)=-1.0; H0(2,0)=1.0; H0(3,3)=1.0;
86  torso->setH0(H0);
87 
88  // now we can add the limbs just setting the roto-translational matrix
89  // since the other parameters (kinematic and wrench flow, sensor flag) are not useful
90  // for our example: the default values are ok
91  addLimb(arm_right,Harm_right);
92  addLimb(torso,Htorso);
93  addLimb(head,Hhead);
94  addLimb(arm_left,Harm_left);
95 
96  // print verbose error messages: useful during debug/tests
97  verbose = VERBOSE;
98  arm_right->setVerbosity(VERBOSE);
99  arm_left->setVerbosity(VERBOSE);
100  torso->setVerbosity(VERBOSE);
101  head->setVerbosity(VERBOSE);
102  }
103 
105  {
106  // remember to destroy everything!!
107  delete arm_right;
108  delete head;
109  delete torso;
110  delete arm_left;
111  }
112 
113  // ridefinitions of the jacobian functions
114  // note that the jacobians starting at the head are the only ones
115  // with JAC_IKIN direction in the first limb (because we start the jacobian
116  // at the end-effector of the head, go throught its base to the node, and then
117  // to the arm)
118  // note the numbers for each limb:
119  // 0 = arm_right
120  // 1 = torso
121  // 2 = head
122  // 3 = arm_left
123  // these numbers are related to the insertion order (addLimb) in the constructor..
124 
125  Matrix Jacobian_TorsoArmRight() { return computeJacobian(1,JAC_KIN, 0,JAC_KIN); }
126  Matrix Jacobian_TorsoArmLeft() { return computeJacobian(1,JAC_KIN, 3,JAC_KIN); }
127  Matrix Jacobian_TorsoHead() { return computeJacobian(1,JAC_KIN, 2,JAC_KIN); }
128  Matrix Jacobian_HeadArmRight() { return computeJacobian(2,JAC_IKIN,0,JAC_KIN); }
129  Matrix Jacobian_HeadArmLeft() { return computeJacobian(2,JAC_IKIN,3,JAC_KIN); }
130  Matrix Jacobian_HeadTorso() { return computeJacobian(2,JAC_IKIN,1,JAC_IKIN); }
131  Matrix Jacobian_ArmLeftArmRight() { return computeJacobian(3,JAC_IKIN,0,JAC_KIN); }
132  Matrix Jacobian_ArmRightArmLeft() { return computeJacobian(0,JAC_IKIN,3,JAC_KIN); }
133 
134  // ridefinitions of the pose function
135  // again the pose is closely related to the sequence of chains, e.g. the pose starting from the
136  // head has JAC_IKIN direction in the first limb
137 
138  Vector Pose_TorsoArmRight(bool axisRep = false) { return computePose(1,JAC_KIN, 0,JAC_KIN, axisRep); }
139  Vector Pose_TorsoArmLeft(bool axisRep = false) { return computePose(1,JAC_KIN, 3,JAC_KIN, axisRep); }
140  Vector Pose_HeadArmRight(bool axisRep = false) { return computePose(2,JAC_IKIN,0,JAC_KIN, axisRep); }
141  Vector Pose_HeadArmLeft(bool axisRep = false) { return computePose(2,JAC_IKIN,3,JAC_KIN, axisRep); }
142  Vector Pose_TorsoHead(bool axisRep = false) { return computePose(1,JAC_KIN, 2,JAC_KIN, axisRep); }
143  Vector Pose_HeadTorso(bool axisRep = false) { return computePose(2,JAC_IKIN,1,JAC_IKIN,axisRep); }
144  Vector Pose_ArmLeftArmRight(bool axisRep = false) { return computePose(3,JAC_IKIN,0,JAC_KIN, axisRep); }
145  Vector Pose_ArmRightArmLeft(bool axisRep = false) { return computePose(0,JAC_IKIN,3,JAC_KIN, axisRep); }
146 
147  // generic print method
148  string toString() { return info; }
149 };
150 
151 // useful print methods
152 void printMatrix(const string &s, const Matrix &m)
153 {
154  cout<<s<<endl;
155  for(int i=0;i<m.rows();i++)
156  {
157  for(int j=0;j<m.cols();j++)
158  cout<<setw(15)<<m(i,j);
159  cout<<endl;
160  }
161 }
162 void printVector(const string &s, const Vector &v)
163 {
164  cout<<s<<endl;
165  for(size_t j=0;j<v.length();j++)
166  cout<<setw(15)<<v(j);
167  cout<<endl;
168 }
169 
170 
172 // MAIN //
174 int main()
175 {
176  // we create the node with arm and torso
177  UpTorso node;
178 
179  cout<<endl<<"Node <"<<node.toString()<<"> created"<<endl;
180 
181  // now we set the joint angles for the two limbs
182  // if connected to the real robot, we can take this values from the encoders
183  Vector q_rarm(node.arm_right->getN()); q_rarm.zero();
184  Vector q_larm(node.arm_left->getN()); q_larm.zero();
185  Vector q_torso(node.torso->getN()); q_torso.zero();
186  Vector q_head(node.head->getN()); q_head.zero();
187 
188  // here we set the joints positions: the only needed for the jacobian
189  node.arm_right->setAng(q_rarm);
190  node.arm_left->setAng(q_rarm);
191  node.torso->setAng(q_torso);
192  node.head->setAng(q_head);
193 
194  // now we can make all the computations we want..
195 
196  Matrix J(6,1); J.zero();
197  Vector pose(6); pose.zero();
198  string sc = "a";
199  char c = 'a';
200  bool ok=true;
201 
202  while( c!= 'q' )
203  {
204  cin.clear();
205  cout<<endl
206  <<"************************\n"
207  <<"* 1 - torso to right arm \n"
208  <<"* 2 - torso to left arm \n"
209  <<"* 3 - torso to head \n"
210  <<"* 4 - head to right arm \n"
211  <<"* 5 - head to left arm \n"
212  <<"* 6 - head to torso \n"
213  <<"* 7 - right arm to left arm \n"
214  <<"* 8 - left arm to right arm \n"
215  <<"* \n"
216  <<"* q - quit \n"
217  <<"* \n"
218  <<"* Enter your choice: ";
219  cin>>sc; c = sc[0];
220 
221  switch(c)
222  {
223  case '1': J = node.Jacobian_TorsoArmRight(); pose = node.Pose_TorsoArmRight(); ok=true; break;
224  case '2': J = node.Jacobian_TorsoArmLeft(); pose = node.Pose_TorsoArmLeft(); ok=true; break;
225  case '3': J = node.Jacobian_TorsoHead(); pose = node.Pose_TorsoHead(); ok=true; break;
226  case '4': J = node.Jacobian_HeadArmRight(); pose = node.Pose_HeadArmRight(); ok=true; break;
227  case '5': J = node.Jacobian_HeadArmLeft(); pose = node.Pose_HeadArmLeft(); ok=true; break;
228  case '6': J = node.Jacobian_HeadTorso(); pose = node.Pose_HeadTorso(); ok=true; break;
229  case '7': J = node.Jacobian_ArmLeftArmRight(); pose = node.Pose_ArmLeftArmRight(); ok=true; break;
230  case '8': J = node.Jacobian_ArmRightArmLeft(); pose = node.Pose_ArmRightArmLeft(); ok=true; break;
231  case 'q': cout<<" .. quitting, bye.\n"<<endl; ok=false; break;
232  default: cout<<" .. this is not a correct choice!!\n"<<endl; ok=false;
233  }
234 
235  if(ok)
236  {
237  cout<<endl<<endl;
238  printMatrix("\nJacobian\n",J);
239  printVector("\nPose\n",pose);
240  cout<<endl;
241  }
242  }
243 
244  cout<<endl<<"\n\n************************\n\n";
245  return 0;
246 }
247 
248 
#define CTRL_DEG2RAD
Definition: XSensMTx.cpp:26
Definition: main.cpp:45
Vector Pose_HeadArmRight(bool axisRep=false)
Definition: main.cpp:140
Vector Pose_TorsoArmLeft(bool axisRep=false)
Definition: main.cpp:139
Matrix Jacobian_TorsoArmLeft()
Definition: main.cpp:126
Matrix Jacobian_TorsoHead()
Definition: main.cpp:127
Vector Pose_HeadArmLeft(bool axisRep=false)
Definition: main.cpp:141
Matrix Jacobian_ArmRightArmLeft()
Definition: main.cpp:132
Matrix Jacobian_TorsoArmRight()
Definition: main.cpp:125
Vector Pose_HeadTorso(bool axisRep=false)
Definition: main.cpp:143
Matrix Jacobian_ArmLeftArmRight()
Definition: main.cpp:131
iDynLimb * torso
Definition: main.cpp:50
iDynLimb * head
Definition: main.cpp:49
Matrix Jacobian_HeadArmLeft()
Definition: main.cpp:129
UpTorso()
Definition: main.cpp:54
iDynLimb * arm_left
Definition: main.cpp:51
iDynLimb * arm_right
Definition: main.cpp:48
Vector Pose_ArmLeftArmRight(bool axisRep=false)
Definition: main.cpp:144
~UpTorso()
Definition: main.cpp:104
Vector Pose_TorsoHead(bool axisRep=false)
Definition: main.cpp:142
Matrix Jacobian_HeadTorso()
Definition: main.cpp:130
string toString()
Definition: main.cpp:148
Vector Pose_TorsoArmRight(bool axisRep=false)
Definition: main.cpp:138
Vector Pose_ArmRightArmLeft(bool axisRep=false)
Definition: main.cpp:145
Matrix Jacobian_HeadArmRight()
Definition: main.cpp:128
A class for defining the 7-DOF iCub Arm in the iDyn framework.
Definition: iDyn.h:1369
A class for defining the 3-DOF Inertia Sensor Kinematics.
Definition: iDyn.h:1533
A class for defining the 3-DOF iCub Torso in the iDyn framework.
Definition: iDyn.h:1410
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
A class for defining a generic Limb within the iDyn framework.
Definition: iDyn.h:1177
A class for connecting two or mutiple limbs and exchanging kinematic and wrench information between l...
Definition: iDynBody.h:532
void setVerbosity(unsigned int _verbose)
Sets the verbosity level of the Chain.
Definition: iKinFwd.h:538
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition: iKinFwd.h:550
bool setH0(const yarp::sig::Matrix &_H0)
Sets H0, the rigid roto-translation matrix from the root reference frame to the 0th frame.
Definition: iKinFwd.cpp:562
int main(int argc, char *argv[])
Definition: main.cpp:31
void printVector(const string &s, const Vector &v)
Definition: main.cpp:162
void printMatrix(const string &s, const Matrix &m)
Definition: main.cpp:152
@ KINFWD_WREBWD
Definition: iDynInv.h:69
@ KINBWD_WREBWD
Definition: iDynInv.h:69
@ JAC_IKIN
Definition: iDynBody.h:99
Copyright (C) 2008 RobotCub Consortium.