[Python] Expose RNEA derivatives

bindings/python/CMakeLists.txt

@@ -88,6 +88,7 @@ SET(${PROJECT_NAME}_PYTHON_SOURCES
   algorithm/expose-dynamics.cpp
   algorithm/expose-crba.cpp
   algorithm/expose-rnea.cpp
+  algorithm/expose-rnea-derivatives.cpp
   algorithm/expose-aba.cpp
   algorithm/expose-jacobian.cpp
   algorithm/expose-joints.cpp

bindings/python/algorithm/algorithms.hpp

@@ -40,6 +40,8 @@ namespace se3
     void exposeJacobian();
     void exposeGeometryAlgo();
     void exposeRegressor();
+    
+    void exposeRNEADerivatives();
 
     void exposeAlgorithms();
     

bindings/python/algorithm/expose-algorithms.cpp

@@ -37,6 +37,9 @@ namespace se3
       exposeJacobian();
       exposeGeometryAlgo();
       exposeRegressor();
+      
+      // expose derivative version of the algorithms
+      exposeRNEADerivatives();
     }
     
   } // namespace python

bindings/python/algorithm/expose-rnea-derivatives.cpp

+//
+// Copyright (c) 2018 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
+#include "pinocchio/algorithm/rnea-derivatives.hpp"
+
+namespace se3
+{
+  namespace python
+  {
+    void computeRNEADerivativesDefault(const Model & model, Data & data,
+                                       const Eigen::VectorXd & q,
+                                       const Eigen::VectorXd & v,
+                                       const Eigen::VectorXd & a)
+    {
+      computeRNEADerivatives(model,data,q,v,a);
+    }
+    
+    void exposeRNEADerivatives()
+    {
+      using namespace Eigen;
+      typedef container::aligned_vector<Force> ForceAlignedVector;
+      
+      bp::def("computeRNEADerivatives",
+              computeRNEADerivativesDefault,
+              bp::args("Model","Data",
+                       "Configuration q (size Model::nq)",
+                       "Velocity v (size Model::nv)",
+                       "Acceleration a (size Model::nv)"),
+              "Computes the RNEA derivatives, put the result in data.dtau_dq, data.dtau_dv and data.dtau_da\n"
+              "which correspond to the partial derivative of the torque output with respect to the joint configuration,\n"
+              "velocity and acceleration vectors.");
+    }
+    
+    
+    
+  } // namespace python
+} // namespace se3

bindings/python/multibody/data.hpp

@@ -88,9 +88,9 @@ namespace se3
         .ADD_DATA_PROPERTY(container::aligned_vector<SE3>,iMf,"Body placement wrt to algorithm end effector.")
         
         .ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,Ag,
-                                 "Centroidal matrix which maps from joint velocity to the centroidal momentum.")
+                                            "Centroidal matrix which maps from joint velocity to the centroidal momentum.")
         .ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,dAg,
-                                 "Time derivative of the centroidal momentum matrix Ag.")
+                                            "Time derivative of the centroidal momentum matrix Ag.")
         .ADD_DATA_PROPERTY_READONLY(Force,hg,
                                     "Centroidal momentum (expressed in the frame centered at the CoM and aligned with the inertial frame).")
         .ADD_DATA_PROPERTY_READONLY(Inertia,Ig,
@@ -102,6 +102,9 @@ namespace se3
         .ADD_DATA_PROPERTY(std::vector<double>,mass,"Mass of the subtree starting at joint index i.")
         .ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix3x,Jcom,"Jacobian of center of mass.")
 
+        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,C,"Joint space Coriolis matrix.")
+        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,dtau_dq,"Partial derivative of the joint torque vector with respect to the joint configuration.")
+        .ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,dtau_dv,"Partial derivative of the joint torque vector with respect to the joint velocity.")
         
         .ADD_DATA_PROPERTY_READONLY_BYVALUE(double,kinetic_energy,"Kinetic energy in [J] computed by kineticEnergy(model,data,q,v,True/False)")
         .ADD_DATA_PROPERTY_READONLY_BYVALUE(double,potential_energy,"Potential energy in [J] computed by potentialEnergy(model,data,q,True/False)")

src/multibody/model.hpp

@@ -516,10 +516,10 @@ namespace se3
     /// \brief Variation of the spatial acceleration set with respect to the joint velocity.
     Matrix6x dAdv;
     
-    /// \brief Variation of the joint torque vector with respect to the joint configuration.
+    /// \brief Partial derivative of the joint torque vector with respect to the joint configuration.
     Eigen::MatrixXd dtau_dq;
     
-    /// \brief Variation of the joint torque vector with respect to the joint velocity.
+    /// \brief Partial derivative of the joint torque vector with respect to the joint velocity.
     Eigen::MatrixXd dtau_dv;
     
     /// \brief Vector of joint placements wrt to algorithm end effector.