iCubmain

In this tutorial we show how to compute the joint torques in a single kinematic chain, using the iDyn library.
Before you read this tutorial you should at least get accustomed with the iDyn library, and the basic of NewtonEuler algorithm (reading the short introduction to iDyn might be useful).
Remember to include YARP and iCub as packages, and to link iDyn library in CMakeLists.txt:
note that in this case we need to look for the packages because we are developing a tutorial which is outside iCub project.
Using iDyn classes, vectors and matrices are used all the times.
We start creating a iDyn::iCubArmDyn, that is a chain with arm and torso links. If you are familiar with iKin, it is exactly the same limb type, with the same kinematic properties of a iCubArm. Of course, iCubArmDyn is more complete, since it basically inherits the kinematics and provides the dynamics.
By default in iCubArmDyn the torso is blocked, as it is in its corresponding iKin limb, iCubArm; we unblock it, so we can also use the torso links. Note that releasing the links we have N==DOF (i.e. the number of links equal to the number of degrees of freedom).
Before any computation, we must prepare the necessary variables and put them in the chain model. First of all, we must set the joint angles in the limb model: in dynamics, we need joint positions, velocities and accelerations.
In iCub we only have position measurements from the encoders, so in order to avoid noisy values on velocity and acceleration, we suggest to apply at least a linear filter on the velocity data computed by differentiation, and a quadratic filter on the acceleration data. As a suggestion, look for adaptWinPolyEstimator in the ctrlLib, or have a look at the module WholeBodyTorqueObserver. Note that if the joint angles exceed the joint limits, their value is saturated to their min/max value automatically (as in iKin). At the moment there's not such a limitation for velocities and accelerations. It must be reminded that the joint limits could be different in the real robot, so it is always a good practice, when using the robot, to call the alignJointsBound method, which queries the robot for the real joints limits.
In order to initialize the kinematic phase of NewtonEuler algorithm, we need the kinematic informations to be set in the base of the chain, where the base is intended to be the first link (first w.r.t the Denavit Hartenberg convention for describing the links, so the torso base in iCubArmDyn). If the robot is fixed and not moving, everything can be simply set to zero: but remember to provide the gravity information!
The endeffector external wrench (force/moment), is necessary to initialize the wrench phase of the NewtonEuler algorithm. If the robot is not touching anything, the external wrench is zero by default. If there's contact... look at the WrenchObserver module.
We can now start using iDyn. In order to perform the kinematic and wrench computation in a chain, we must first tell the limb that we want to use the iDyn::RecursiveNewtonEuler library, by calling a "prepare" method. In fact, before using our method, a new chain, parallel to the iDynChain, is built: it is a OneChainNewtonEuler, and it is made of OneLinkNewtonEuler virtual links, each having its "real" corresponding iDynLink; in addition, a BaseLink and a FinalLink elements are added to the chain, defining the entry points for the kinematic and wrench phases of NewtonEuler computations.
The reason of the existence of the prepareNewtonEuler method is twofold:
When calling prepareNewtonEuler(), we must specify the type of computations we want to perform, which are basically:
Finally we perform the computations, by calling a single function which performs autonomously:
We can now retrieve the results: forces, moments and joint torques. In particular, F and M are 3xN matrices, where the ith column is the ith force/moment (each having 3 components), whereas Tau is a 1xN vector, since the joint torque is a scalar number.
Full working code of this tutorial is available here: