Add inertial parameters regressor method to KinDynComputations

doc/dcTutorialCpp.md

@@ -1,29 +1,36 @@
 
-# DynamicsComputations tutorial
+# KinDynComputations tutorial
 
-To use the DynamicsComputations class, you have to include its header.
+To use the KinDynComputations class, you have to include its header.
 For this tutorial we also use the appropriate `using` definitions to 
-avoid having to type the namespace of the class. 
+avoid having to type the `iDynTree` namespace.
 
 ~~~cpp
 #include <iostream>
 
-#include <iDynTree/HighLevel/DynamicsComputations.h>
+#include <iDynTree/KinDynComputations.h>
+#include <iDynTree/ModelIO/ModelLoader.h>
 
 using namespace iDynTree;
-using namespace iDynTree::HighLevel;
 ~~~
 
 The first thing that we have to do 
 is to initialize the class by providing a model of the robot. Currently 
 iDynTree supports only URDF models to specify the model. 
+All the logic for loading models, including loading reduced models,
+is contained in the iDynTree::ModelLoader class.
 ~~~cpp
 int main()
 {
-    DynamicsComputations dynComp;
-    dynComp.loadRobotModelFromFile("model.urdf");
-    std::cout << "The loaded model has " << dynComp.getNrOfDegreesOfFreedom() 
-          << "internal degrees of freedom and " << dynComp.getNrOfLinks() 
+    ModelLoader mdlLoader;
+    bool ok = mdlLoader.loadModelFromFile(urdf_filename);
+
+    // Check that ok is actually true
+
+    KinDynComputations kinDynComp;
+    kinDynComp.loadRobotModel(mdlLoader.model());
+    std::cout << "The loaded model has " << kinDynComp.model().getNrOfDOFs()
+          << "internal degrees of freedom and " << kinDynComp.model().getNrOfLinks()
           << "links." << std::endl;
 ~~~
 
@@ -34,37 +41,37 @@ to specify the joint position, velocity and accelerations and the gravity in the
 base frame. 
 
 ~~~cpp
-    unsigned int dofs = dynComp.getNrOfDegreesOfFreedom();
-    VectorDynSize q(dofs), dq(dofs), ddq(dofs);
+    unsigned int dofs = kinDynComp.model();
+    VectorDynSize s(dofs), ds(dofs), dds(dofs);
     for(unsigned int dof = 0; dof < dofs; dof++ )
     {
         // For the sake of the example, we fill the joints
         // vector with gibberish data (remember in any case
         // that all quantities are expressed in radians-based 
         // units 
-        q(dof) = 1.0;
-        dq(dof) = 0.4;
-        ddq(dof) = 0.3;
+        s(dof) = 1.0;
+        ds(dof) = 0.4;
+        dds(dof) = 0.3;
     }
 
-    // The spatial acceleration is a 6d acceleration vector. 
+    // The gravity acceleration is a 3d vector.
+    //
     // For all 6d quantities, we use the linear-angular serialization
     // (the first three value are for the linear quantity, the 
     //  the last  three values are for the angular quantity)
-    SpatialAcc gravity;
-    gravity(3) = -9.81;
-    dynComp.setRobotState(q,dq,ddq,gravity);
+    Vector3 gravity;
+    gravity.zero();
+    gravity(2) = -9.81;
+    kinDynComp.setRobotState(s,ds,gravity);
 ~~~
 
 Once you set the state, you can access the computed dynamical quantities. 
 For example you can get the jacobian for a given frame with name "frameName".
 Note that "frameName" should be the name of a link in the URDF model. 
-Note that the jacobian is the floating base jacobian as described in 
-http://wiki.icub.org/codyco/dox/html/dynamics_notation.html . 
 
 ~~~cpp
     MatrixDynSize jac(6,6+dofs);
-    bool ok = dynComp.getFrameJacobian("frameName",jac);
+    bool ok = kinDynComp.getFrameFreeFloatingJacobian("frameName", jac);
 
     if( !ok )
     {
@@ -79,16 +86,17 @@ http://wiki.icub.org/codyco/dox/html/dynamics_notation.html .
 
 You can also get the dynamics regressor. 
 The dynamics regressor is a (6+dofs) \* (10 \* nrOfLinks) Y matrix such that:
-Y pi = M(q) d(v)/dt + C(q,v)v + g(q) 
-with M(q), C(q,v) and g(q) defined in http://wiki.icub.org/codyco/dox/html/dynamics_notation.html .
+Y pi = M(q) d(v)/dt + C(q,v)v + g(q) .
 The pi vector is a 10 \* nrOfLinks inertial parameters vector, such that the elements of the vector
 from the ((i-1) \* 10)-th to the (i \* 10-1)-th belong to the i-th link. For more details on the inertial
 parameters vector, please check https://hal.archives-ouvertes.fr/hal-01137215/document . 
 
 ~~~cpp
     unsigned int links = dynComp.getNrOfLinks();
     MatrixDynSize regr(6+dofs,10*links);
-    ok = dynComp.getDynamicsRegressor(regr);
+    Vector6 baseAcc;
+    baseAcc.zero();
+    ok = kinDynComp.inverseDynamicsInertialParametersRegressor(baseAcc, dds, regr);
 
     if( !ok )
     {

doc/main.dox

@@ -83,6 +83,13 @@
  */
 
 
+ /**
+ * \defgroup iDynTreeDeprecated Deprecated functions and classes
+ *
+ * \warning The functions and classes in this component should not be used for new code.
+ *
+ */
+
 
 
 
@@ -121,5 +128,11 @@ Until this components are ready to be integrated in a proper part of iDynTree, w
 
 \li  \ref iDynTreeExperimental "Experimental" : Experimental data structures and algorithms, whose interface is not guaranteed to be stable.
 
+iDynTree started as a tiny wrapper of code between [YARP](http://yarp.it/) and [KDL](https://github.com/orocos/orocos_kinematics_dynamics).
+For this reason, in the repository there is still some legacy code, that should not be used by users, as it is going to be removed from iDynTree.
+
+\li  \ref iDynTreeDeprecated "Deprecated" : Deprecated data structures and algorithms, that are going to be removed from iDynTree.
+
+
  *
  */

doc/releases/v0_12.md

@@ -13,6 +13,10 @@ Summary
 Important Changes
 -----------------
 
+#### `high-level`
+* Added method to compute the inverse dynamics inertial parameters regressor in KinDynComputations ( https://github.com/robotology/idyntree/pull/480 ).
+KinDynComputations finally reached feature parity with respect to DynamicsComputations, that will finally be removed in one of the future iDynTree feature releases.
+
 #### `inverse-kinematics`
 * Added method to return the convex hull of the constraint on the projection of the center of mass (https://github.com/robotology/idyntree/pull/478).
 

examples/python/dynamicsComputationTutorial.py

@@ -6,46 +6,52 @@
 """
 
 import iDynTree
-from iDynTree import DynamicsComputations
+from iDynTree import KinDynComputations
+from iDynTree import ModelLoader
+
 
 URDF_FILE = '/home/username/path/robot.urdf';
 
-dynComp = DynamicsComputations();
-dynComp.loadRobotModelFromFile(URDF_FILE);
-print "The loaded model has", dynComp.getNrOfDegreesOfFreedom(), \
-    "internal degrees of freedom and",dynComp.getNrOfLinks(),"links."
+dynComp = KinDynComputations();
+mdlLoader = ModelLoader();
+mdlLoader.loadModelFromFile(URDF_FILE);
+
+dynComp.loadRobotModel(mdlLoader.model());
+
+print "The loaded model has", dynComp.model().getNrOfDOFs(), \
+    "internal degrees of freedom and",dynComp.model().getNrOfLinks(),"links."
 
-dofs = dynComp.getNrOfDegreesOfFreedom();
-q = iDynTree.VectorDynSize(dofs);
-dq = iDynTree.VectorDynSize(dofs);
-ddq = iDynTree.VectorDynSize(dofs);
+dofs = dynComp.model().getNrOfDOFs();
+s = iDynTree.VectorDynSize(dofs);
+ds = iDynTree.VectorDynSize(dofs);
+dds = iDynTree.VectorDynSize(dofs);
 for dof in range(dofs):
     # For the sake of the example, we fill the joints vector with gibberish data (remember in any case
     # that all quantities are expressed in radians-based units 
-    q.setVal(dof, 1.0);
-    dq.setVal(dof, 0.4);
-    ddq.setVal(dof, 0.3);
+    s.setVal(dof, 1.0);
+    ds.setVal(dof, 0.4);
+    dds.setVal(dof, 0.3);
 
 
-# The spatial acceleration is a 6d acceleration vector. 
-# For all 6d quantities, we use the linear-angular serialization
-# (the first three value are for the linear quantity, the 
-#  the last  three values are for the angular quantity)
-gravity = iDynTree.SpatialAcc();
+# The gravity acceleration is a 3d acceleration vector.
+gravity = iDynTree.Vector3();
 gravity.zero();
 gravity.setVal(2, -9.81);
-dynComp.setRobotState(q,dq,ddq,gravity);
+dynComp.setRobotState(s,ds,gravity);
 
 jac = iDynTree.MatrixDynSize(6,6+dofs);
-ok = dynComp.getFrameJacobian("lf_foot", jac);
+ok = dynComp.getFreeFloatingJacobian("lf_foot", jac);
 if( not ok ):
     print "Error in computing jacobian of frame " + "lf_foot";
 else: 
     print "Jacobian of lf_foot is\n" + jac.toString();
-
-links = dynComp.getNrOfLinks();
+
+
+baseAcc = iDynTree.Vector6();
+baseAcc.zero();
+links = dynComp.model().getNrOfLinks();
 regr = iDynTree.MatrixDynSize(6+dofs,10*links);
-ok = dynComp.getDynamicsRegressor(regr);
+ok = dynComp.inverseDynamicsInertialParametersRegressor(baseAcc, dds, regr);
 if( not ok ):
     print "Error in computing the dynamics regressor";
 else :