Verified Commit d7630566 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

data: add kinematic Hessian as Tensor field

parent 4292b3ac
......@@ -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.
///
......
......@@ -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>
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment