data: add kinematic Hessian as Tensor field

src/multibody/data.hpp

@@ -17,6 +17,7 @@
 
 #include <iostream>
 #include <Eigen/Cholesky>
+#include <unsupported/Eigen/CXX11/Tensor>
 
 namespace pinocchio
 {
@@ -73,6 +74,9 @@ namespace pinocchio
 
     /// \brief The type of the body regressor
     typedef Eigen::Matrix<Scalar,6,10,Options> BodyRegressorType;
+    
+    /// \brief The type of Tensor for Kinematics and Dynamics second order derivatives
+    typedef Eigen::Tensor<Scalar,3,Options> Tensor3;
 
     /// \brief Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model, 
     /// encapsulated in JointDataAccessor.
@@ -309,7 +313,6 @@ namespace pinocchio
     /// \note This Jacobian maps the joint velocity vector to the velocity of the center of mass, expressed in the inertial frame. In other words, \f$ v_{\text{CoM}} = J_{\text{CoM}} \dot{q}\f$.
     Matrix3x Jcom;
 
-    
     /// \brief Kinetic energy of the model.
     Scalar kinetic_energy;
     
@@ -339,15 +342,18 @@ namespace pinocchio
     /// \brief Lagrange Multipliers corresponding to the contact impulses in pinocchio::impulseDynamics.
     VectorXs impulse_c;
     
-    // Matrix related to static regressor
+    /// \brief Matrix related to static regressor
     Matrix3x staticRegressor;
 
-    // Body regressor
+    /// \brief Body regressor
     BodyRegressorType bodyRegressor;
 
-    // Matrix related to joint torque regressor
+    /// \brief Matrix related to joint torque regressor
     MatrixXs jointTorqueRegressor;
     
+    /// \brief Tensor containing the kinematic Hessian of all the joints.
+    Tensor3 kinematic_hessians;
+    
     ///
     /// \brief Default constructor of pinocchio::Data from a pinocchio::Model.
     ///

src/multibody/data.hxx

@@ -96,10 +96,11 @@ namespace pinocchio
   , impulse_c()
   , staticRegressor(3,4*(model.njoints-1))
   , jointTorqueRegressor(model.nv,10*(model.njoints-1))
+  , kinematic_hessians(6,model.nv,model.nv)
   {
     typedef typename Model::JointIndex JointIndex;
     
-    /* Create data strcture associated to the joints */
+    /* Create data structure associated to the joints */
     for(JointIndex i=0;i<(JointIndex)(model.njoints);++i)
       joints.push_back(CreateJointData<Scalar,Options,JointCollectionTpl>::run(model.joints[i]));
 
@@ -137,6 +138,8 @@ namespace pinocchio
     
     Yaba[0].setZero();
     Ycrb[0].setZero();
+    
+    kinematic_hessians.setZero();
   }
 
   template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>