iCub-main
main.cpp
Go to the documentation of this file.
1 
7 //
8 // The most basic example on the use of iDyn: the computation of forces and torques in a single chain.
9 //
10 // Author: Serena Ivaldi - <serena.ivaldi@iit.it>
11 
12 #include <iostream>
13 #include <iomanip>
14 #include <yarp/sig/Vector.h>
15 #include <yarp/sig/Matrix.h>
16 #include <iCub/iDyn/iDyn.h>
17 
18 using namespace std;
19 using namespace yarp::sig;
20 using namespace iCub::iDyn;
21 
22 
23 
24 // useful print functions
25 // print a matrix nicely
26 void printMatrix(const string &s, const Matrix &m)
27 {
28  cout<<s<<endl;
29  for(int i=0;i<m.rows();i++)
30  {
31  for(int j=0;j<m.cols();j++)
32  cout<<setw(15)<<m(i,j);
33  cout<<endl;
34  }
35 }
36 // print a vector nicely
37 void printVector(const string &s, const Vector &v)
38 {
39  cout<<s<<endl;
40  for(size_t j=0;j<v.length();j++)
41  cout<<setw(15)<<v(j);
42  cout<<endl;
43 }
44 
45 
47 // MAIN
49 
50 int main()
51 {
52  // we create a iDyn::iCubArmDyn = arm+torso
53  // if you are familiar with iKin, it is exactly the same limb type, with the same kinematic properties..
54  // .. but it also has dynamics ^_^
55  iCubArmDyn armTorsoDyn("right");
56 
57  // by default in iCubArmDyn the torso is blocked, as it is in its corresponding iKin limb, iCubArm;
58  // we unblock it, so we can also use the torso links
59  // note: releasing the links we have N==DOF
60  armTorsoDyn.releaseLink(0);
61  armTorsoDyn.releaseLink(1);
62  armTorsoDyn.releaseLink(2);
63 
64  // we prepare the necessary variables for our computations
65  // 1) the joint angles (pos, vel, acc) of the limbs: we can take this values from the encoders..
66  Vector q(armTorsoDyn.getN()); q.zero();
67  Vector dq(q); Vector ddq(q);
68 
69  // 2) the inertial information to initialize the kinematic phase
70  // this information must be set at the base of the chain, where the base is intended to be the first link (first.. in the Denavit
71  // Hartenberg convention)
72  // note: if nothing is moving, it can be simply zero, but ddp0 must provide the gravity information :)
73  Vector w0(3); Vector dw0(3); Vector ddp0(3);
74  w0=dw0=ddp0=0.0; ddp0[2]=9.81;
75 
76  // 3) the end-effector external wrench (force/moment), to initialize the wrench phase
77  // the end-effector wrench is zero by default, if we are not touching anything..
78  Vector Fend(3); Fend.zero();
79  Vector Muend(Fend);
80 
81  // now we set the joints values
82  // note: if the joint angles exceed the joint limits, their value is saturated to the min/max automatically
83  armTorsoDyn.setAng(q);
84  armTorsoDyn.setDAng(dq);
85  armTorsoDyn.setD2Ang(ddq);
86 
87  // we can start using iDyn :)
88  // in order to perform the kinematic and wrench computation in a chain, we must first tell the limb
89  // that we want to use the iDyn::RecursiveNewtonEuler library
90  // the reason is twofold:
91  // 1) one can use a iDynLimb without dynamics, since it includes the kinematics of iKin
92  // for example one may only want to set joint angles and compute the jacobian
93  // 2) one may prefer to use its own dynamic library to compute internal forces/torques, and not
94  // necessarily the Newton-Euler one we provide. to use our method, a new chain, parallel to the
95  // iDynChain, is built: it is a OneChainNewtonEuler, and it is made of OneLinkNewtonEuler virtual
96  // links, each having its "real" corresponding iDynLink: in addition, a BaseLink and a FinalLink
97  // elements are added to the chain, defining the entry points for the kinematic and wrench phases
98  // of Newton-Euler computations.
99  // the method to call is prepareNewtonEuler(), and we must specify the type of computations we want
100  // to perform, which are basically:
101  // - STATIC: we only need q (joints positions)
102  // - DYNAMIC: we need q,dq,ddq (joints position, velocity, acceleration) --> default
103  armTorsoDyn.prepareNewtonEuler(DYNAMIC);
104 
105  // then we perform the computations
106  // 1) Kinematic Phase
107  // 2) Wrench Phase
108  // the computeNewtonEuler() methoddo everything autonomously!
109  armTorsoDyn.computeNewtonEuler(w0,dw0,ddp0,Fend,Muend);
110 
111  // then we can retrieve our results...
112  // forces moments and torques
113  Matrix F = armTorsoDyn.getForces();
114  Matrix Mu = armTorsoDyn.getMoments();
115  Vector Tau = armTorsoDyn.getTorques();
116 
117  // now print the information
118 
119  cout<<"iCubArmDyn has "<<armTorsoDyn.getN()<<" links."<<endl;
120 
121  cout<<"\n\n";
122 
123  printVector("Joint Angles - command ", q);
124  printVector("Joint Angles - real ", armTorsoDyn.getAng());
125 
126  cout<<"\n\n";
127 
128  printMatrix("Forces ",F);
129  printMatrix("Moments ",Mu);
130  printVector("Torques ",Tau);
131 
132  return 0;
133 }
134 
135 
A class for defining the 7-DOF iCub Arm in the iDyn framework.
Definition: iDyn.h:1328
yarp::sig::Matrix getForces() const
Returns the links forces as a matrix, where the i-th col is the i-th force.
Definition: iDyn.cpp:859
yarp::sig::Vector setD2Ang(const yarp::sig::Vector &ddq)
Sets the free joint angles acceleration to values of ddq[i].
void prepareNewtonEuler(const NewEulMode NewEulMode_s=DYNAMIC)
Prepare for the Newton-Euler recursive computation of forces and torques.
Definition: iDyn.cpp:916
yarp::sig::Matrix getMoments() const
Returns the links moments as a matrix, where the i-th col is the i-th moment.
Definition: iDyn.cpp:867
yarp::sig::Vector getTorques() const
Returns the links torque as a vector.
Definition: iDyn.cpp:875
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
yarp::sig::Vector setDAng(const yarp::sig::Vector &dq)
Sets the free joint angles velocity to values of dq[i].
bool computeNewtonEuler(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0, const yarp::sig::Vector &Fend, const yarp::sig::Vector &Muend)
Compute forces and torques with the Newton-Euler recursive algorithm: forward and backward phase are ...
bool releaseLink(const unsigned int i)
Releases the ith Link.
Definition: iKinFwd.cpp:463
yarp::sig::Vector getAng()
Returns the current free joint angles values.
Definition: iKinFwd.cpp:611
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition: iKinFwd.h:550
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
@ DYNAMIC
Definition: iDynInv.h:64