Merge pull request #405 from jcarpent/topic/ag-variation

Implement time derivative of the centroidal momemtum matrix

bindings/python/algorithm/expose-crba.cpp

@@ -47,6 +47,13 @@ namespace se3
               "Computes the centroidal mapping, the centroidal momentum and the Centroidal Composite Rigid Body Inertia, puts the result in Data and returns the centroidal mapping.",
               bp::return_value_policy<bp::return_by_value>());
       
+      bp::def("dccrba",dccrba,
+              bp::args("Model","Data",
+                       "Joint configuration q (size Model::nq)",
+                       "Joint velocity v (size Model::nv)"),
+              "Computes the time derivative of the centroidal momentum matrix Ag in terms of q and v. It computes also the same information than ccrtba for the same price.",
+              bp::return_value_policy<bp::return_by_value>());
+      
     }
     
   } // namespace python

bindings/python/multibody/data.hpp

@@ -88,6 +88,8 @@ namespace se3
         
         .ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,Ag,
                                  "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.")
         .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,

bindings/python/spatial/motion.hpp

@@ -74,6 +74,9 @@ namespace se3
         .def("se3ActionInverse",&Motion::se3ActionInverse,
              bp::args("M"),"Returns the result of the action of the inverse of M on *this.")
         
+        .add_property("action",&Motion::toActionMatrix,"Returns the action matrix of *this (acting on Motion).")
+        .add_property("dualAction",&Motion::toDualActionMatrix,"Returns the dual action matrix of *this (acting on Force).")
+        
         .def("setZero",&MotionPythonVisitor::setZero,
              "Set the linear and angular components of *this to zero.")
         .def("setRandom",&MotionPythonVisitor::setRandom,

src/algorithm/crba.hpp

@@ -44,26 +44,40 @@ namespace se3
        const Eigen::VectorXd & q);
   
   ///
-  /// \brief Computes the upper triangular part of the joint space inertia matrix M by
-  ///        using the Composite Rigid Body Algorithm (Chapter 6, Rigid-Body Dynamics Algorithms, R. Featherstone, 2008).
-  ///        The result is accessible through data.M.
+  /// \brief Computes the Centroidal Momentum Matrix, the Composite Ridig Body Inertia as well as the centroidal momenta
+  ///        according to the current joint configuration and velocity.
   ///
-  /// \note You can easly get data.M symetric by copying the stricly upper trinangular part
-  ///       in the stricly lower tringular part with
-  ///       data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
   ///
   /// \param[in] model The model structure of the rigid body system.
   /// \param[in] data The data structure of the rigid body system.
   /// \param[in] q The joint configuration vector (dim model.nq).
-  /// \param[in] v The joint configuration vector (dim model.nv).
+  /// \param[in] v The joint velocity vector (dim model.nv).
   ///
-  /// \return The joint space inertia matrix with only the upper triangular part computed.
+  /// \return The Centroidal Momentum Matrix Ag. 
   ///
   inline const Data::Matrix6x &
   ccrba(const Model & model,
         Data & data,
         const Eigen::VectorXd & q,
         const Eigen::VectorXd & v);
+  
+  ///
+  /// \brief Computes the time derivative of the Centroidal Momentum Matrix according to the current configuration and velocity vectors.
+  ///
+  /// \note The computed terms allow to decomposed the spatial momentum variation as following: \f$ \dot{h} = A_g \ddot{q} + \dot{A_g}(q,\dot{q})\dot{q}\f$.
+  ///
+  /// \param[in] model The model structure of the rigid body system.
+  /// \param[in] data The data structure of the rigid body system.
+  /// \param[in] q The joint configuration vector (dim model.nq).
+  /// \param[in] v The joint configuration vector (dim model.nv).
+  ///
+  /// \return The Centroidal Momentum Matrix time derivative dAg
+  ///
+  inline const Data::Matrix6x &
+  dccrba(const Model & model,
+        Data & data,
+        const Eigen::VectorXd & q,
+        const Eigen::VectorXd & v);
 
   DEFINE_ALGO_CHECKER(CRBA);
 

src/algorithm/crba.hxx

@@ -179,25 +179,25 @@ namespace se3
       forceSet::se3Action(data.oMi[i],jdata.U(),jF);
     }
     
-    static void algo(const se3::JointModelBase<JointModelComposite> & jmodel,
-                     se3::JointDataBase<JointDataComposite> & jdata,
-                     const se3::Model & model,
-                     se3::Data & data)
-    {
-      typedef SizeDepType<JointModel::NV>::ColsReturn<Data::Matrix6x>::Type ColsBlock;
-      
-      const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
-      const Model::Index & parent = model.parents[i];
-      
-      data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
-      
-      jdata.U() = data.Ycrb[i] * jdata.S();
-      
-      ColsBlock jF
-        = data.Ag.middleCols(jmodel.idx_v(), jmodel.nv());
-
-      forceSet::se3Action(data.oMi[i],jdata.U(),jF);
-    }
+//    static void algo(const se3::JointModelBase<JointModelComposite> & jmodel,
+//                     se3::JointDataBase<JointDataComposite> & jdata,
+//                     const se3::Model & model,
+//                     se3::Data & data)
+//    {
+//      typedef SizeDepType<JointModel::NV>::ColsReturn<Data::Matrix6x>::Type ColsBlock;
+//      
+//      const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
+//      const Model::Index & parent = model.parents[i];
+//      
+//      data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
+//      
+//      jdata.U() = data.Ycrb[i] * jdata.S();
+//      
+//      ColsBlock jF
+//        = data.Ag.middleCols(jmodel.idx_v(), jmodel.nv());
+//
+//      forceSet::se3Action(data.oMi[i],jdata.U(),jF);
+//    }
 
   }; // struct CcrbaBackwardStep
   
@@ -235,6 +235,174 @@ namespace se3
     
     return data.Ag;
   }
+  
+  struct DCcrbaForwardStep : public fusion::JointVisitor<DCcrbaForwardStep>
+  {
+    typedef boost::fusion::vector< const se3::Model &,
+    se3::Data &,
+    const Eigen::VectorXd &,
+    const Eigen::VectorXd &
+    > ArgsType;
+    
+    JOINT_VISITOR_INIT(DCcrbaForwardStep);
+    
+    template<typename JointModel>
+    static void algo(const se3::JointModelBase<JointModel> & jmodel,
+                     se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
+                     const se3::Model & model,
+                     se3::Data & data,
+                     const Eigen::VectorXd & q,
+                     const Eigen::VectorXd & v)
+    {
+      const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
+      const Model::Index & parent = model.parents[i];
+      
+      jmodel.calc(jdata.derived(),q,v);
+      
+      data.liMi[i] = model.jointPlacements[i]*jdata.M();
+      data.Ycrb[i] = model.inertias[i];
+      
+      data.v[i] = jdata.v();
+      
+      if (parent>0)
+      {
+        data.oMi[i] = data.oMi[parent]*data.liMi[i];
+        data.v[i] += data.liMi[i].actInv(data.v[parent]);
+      }
+      else data.oMi[i] = data.liMi[i];
+    }
+    
+  }; // struct DCcrbaForwardStep
+  
+  struct DCcrbaBackwardStep : public fusion::JointVisitor<DCcrbaBackwardStep>
+  {
+    typedef boost::fusion::vector< const se3::Model &,
+    se3::Data &
+    > ArgsType;
+    
+    JOINT_VISITOR_INIT(DCcrbaBackwardStep);
+    
+    template<typename JointModel>
+    static void algo(const se3::JointModelBase<JointModel> & jmodel,
+                     se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
+                     const se3::Model & model,
+                     se3::Data & data)
+    {
+      typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
+      typedef typename JointModel::JointDataDerived JointData;
+      typedef typename JointModel::Constraint_t Constraint_t;
+      
+      
+      const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
+      const Model::Index & parent = model.parents[i];
+      const Motion & v = data.v[i];
+      const Inertia & Y = data.Ycrb[i];
+      const Inertia::Matrix6 & dY = data.dYcrb[i];
+      
+      data.Ycrb[parent] += data.liMi[i].act(Y);
+      data.dYcrb[parent] += SE3actOn(data.liMi[i],dY);
+      
+      // Calc Ag
+      ColsBlock Ag_cols = jmodel.jointCols(data.Ag);
+      jdata.U() = Y * jdata.S();
+      forceSet::se3Action(data.oMi[i],jdata.U(),Ag_cols);
+      
+      // Calc dAg = oXi* dU
+      typename JointData::U_t dU(dY * jdata.S()); // TODO: add dU inside jdata.
+      typename Constraint_t::DenseBase dS = jdata.S().variation(v);
+      dU.noalias() += Y.matrix()*dS;
+      
+      ColsBlock dAg_cols = jmodel.jointCols(data.dAg);
+      forceSet::se3Action(data.oMi[i],dU,dAg_cols);
+      
+    }
+    
+    inline static Inertia::Matrix6 SE3actOn(const SE3 & M, const Inertia::Matrix6 & I)
+    {
+      typedef Inertia::Matrix6 Matrix6;
+      typedef SE3::Matrix3 Matrix3;
+      typedef SE3::Vector3 Vector3;
+      typedef Eigen::Block<const Matrix6,3,3> constBlock3;
+      typedef Eigen::Block<Matrix6,3,3> Block3;
+      
+      const constBlock3 & Ai = I.block<3,3> (Inertia::LINEAR, Inertia::LINEAR);
+      const constBlock3 & Bi = I.block<3,3> (Inertia::LINEAR, Inertia::ANGULAR);
+      const constBlock3 & Di = I.block<3,3> (Inertia::ANGULAR, Inertia::ANGULAR);
+      
+      const Matrix3 & R = M.rotation();
+      const Vector3 & t = M.translation();
+      
+      Matrix6 res;
+      Block3 Ao = res.block<3,3> (Inertia::LINEAR, Inertia::LINEAR);
+      Block3 Bo = res.block<3,3> (Inertia::LINEAR, Inertia::ANGULAR);
+      Block3 Co = res.block<3,3> (Inertia::ANGULAR, Inertia::LINEAR);
+      Block3 Do = res.block<3,3> (Inertia::ANGULAR, Inertia::ANGULAR);
+      
+      Ao = R*Ai*R.transpose();
+      Bo = R*Bi*R.transpose();
+      Do.row(0) = t.cross(Bo.col(0));
+      Do.row(1) = t.cross(Bo.col(1));
+      Do.row(2) = t.cross(Bo.col(2));
+      
+      Co.col(0) = t.cross(Ao.col(0));
+      Co.col(1) = t.cross(Ao.col(1));
+      Co.col(2) = t.cross(Ao.col(2));
+      Co += Bo.transpose();
+      
+      Bo = Co.transpose();
+      Do.col(0) += t.cross(Bo.col(0));
+      Do.col(1) += t.cross(Bo.col(1));
+      Do.col(2) += t.cross(Bo.col(2));
+      Do += R*Di*R.transpose();
+      return res;
+    }
+    
+  }; // struct DCcrbaBackwardStep
+  
+  inline const Data::Matrix6x &
+  dccrba(const Model & model, Data & data,
+         const Eigen::VectorXd & q,
+         const Eigen::VectorXd & v)
+  {
+    assert(model.check(data) && "data is not consistent with model.");
+    typedef Eigen::Block <Data::Matrix6x,3,-1> Block3x;
+    
+    forwardKinematics(model,data,q,v);
+    data.Ycrb[0].setZero();
+    for(Model::Index i=1;i<(Model::Index)(model.njoints);++i)
+    {
+      data.Ycrb[i] = model.inertias[i];
+      data.dYcrb[i] = model.inertias[i].variation(data.v[i]); // (vx* I - Ivx)
+    }
+    
+    for(Model::Index i=(Model::Index)(model.njoints-1);i>0;--i)
+    {
+      DCcrbaBackwardStep::run(model.joints[i],data.joints[i],
+                              DCcrbaBackwardStep::ArgsType(model,data));
+    }
+    data.com[0] = data.Ycrb[0].lever();
+    
+    const Block3x Ag_lin = data.Ag.middleRows<3> (Force::LINEAR);
+    Block3x Ag_ang = data.Ag.middleRows<3>  (Force::ANGULAR);
+    for (long i = 0; i<model.nv; ++i)
+      Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);
+    
+    data.hg = data.Ag*v;
+    data.vcom[0] = data.hg.linear()/data.Ycrb[0].mass();
+    
+    const Block3x dAg_lin = data.dAg.middleRows<3>(Force::LINEAR);
+    Block3x dAg_ang = data.dAg.middleRows<3>(Force::ANGULAR);
+    for (long i = 0; i<model.nv; ++i)
+    {
+      dAg_ang.col(i) += dAg_lin.col(i).cross(data.com[0]);// + Ag_lin.col(i).cross(data.vcom[0]);
+    }
+    
+    data.Ig.mass() = data.Ycrb[0].mass();
+    data.Ig.lever().setZero();
+    data.Ig.inertia() = data.Ycrb[0].inertia();
+    
+    return data.dAg;
+  }
 
   // --- CHECKER ---------------------------------------------------------------
   // --- CHECKER ---------------------------------------------------------------

src/multibody/constraint.hpp

@@ -189,6 +189,13 @@ namespace se3
     {
       return (m.inverse().toActionMatrix()*S).eval();
     }
+    
+    DenseBase variation(const Motion & v) const
+    {
+      DenseBase res(v.toActionMatrix() * S);
+      
+      return res;
+    }
 
     void disp_impl(std::ostream & os) const { os << "S =\n" << S << std::endl;}
     

src/multibody/joint/joint-free-flyer.hpp

@@ -84,6 +84,8 @@ namespace se3
       
       TransposeConst transpose() const { return TransposeConst(); }
       operator ConstraintXd () const { return ConstraintXd(SE3::Matrix6::Identity()); }
+      
+      DenseBase variation(const Motion & m) const { return m.toActionMatrix(); }
     }; // struct ConstraintIdentity
 
     template<typename D>

src/multibody/joint/joint-planar.hpp

@@ -189,6 +189,20 @@ namespace se3
       return X_subspace;
     }
 
+    DenseBase variation(const Motion & m) const
+    {
+      const Motion::ConstLinear_t v = m.linear();
+      const Motion::ConstAngular_t w = m.angular();
+      DenseBase res(DenseBase::Zero());
+      
+      res(0,1) = -w[2]; res(0,2) = v[1];
+      res(1,0) = w[2]; res(1,2) = -v[0];
+      res(2,0) = -w[1]; res(2,1) = w[0];
+      res(3,2) = w[1];
+      res(4,2) = -w[0];
+      
+      return res;
+    }
   }; // struct ConstraintPlanar
 
   template<typename D>

src/multibody/joint/joint-prismatic-unaligned.hpp

@@ -191,6 +191,14 @@ namespace se3
       	return ConstraintXd(S);
       }
       
+      DenseBase variation(const Motion & m) const
+      {
+        DenseBase res;
+        res << m.angular().cross(axis), Vector3::Zero();
+        
+        return res;
+      }
+      
     }; // struct ConstraintPrismaticUnaligned
 
 

src/multibody/joint/joint-prismatic.hpp

@@ -196,6 +196,14 @@ namespace se3
       S << prismatic::CartesianVector3<axis>(1).vector(), Eigen::Vector3d::Zero();
       return ConstraintXd(S);
     }
+    
+    DenseBase variation(const Motion & m) const
+    {
+      DenseBase res;
+      res << m.angular().cross(prismatic::CartesianVector3<axis>(1).vector()), Vector3::Zero();
+      
+      return res;
+    }
 
   }; // struct ConstraintPrismatic
 

src/multibody/joint/joint-revolute-unaligned.hpp

@@ -192,6 +192,17 @@ namespace se3
       	return ConstraintXd(S);
       }
       
+      DenseBase variation(const Motion & m) const
+      {
+        const Motion::ConstLinear_t v = m.linear();
+        const Motion::ConstAngular_t w = m.angular();
+        
+        DenseBase res;
+        res << v.cross(axis), w.cross(axis);
+        
+        return res;
+      }
+      
     }; // struct ConstraintRevoluteUnaligned
 
 

src/multibody/joint/joint-revolute.hpp

@@ -198,6 +198,18 @@ namespace se3
       S << Eigen::Vector3d::Zero(), revolute::CartesianVector3<axis>(1).vector();
       return ConstraintXd(S);
     }
+    
+    DenseBase variation(const Motion & m) const
+    {
+      const typename Motion::ConstLinear_t v = m.linear();
+      const typename Motion::ConstAngular_t w = m.angular();
+      
+      const Vector3 a(revolute::CartesianVector3<axis>(1).vector());
+      DenseBase res;
+      res << v.cross(a), w.cross(a);
+      
+      return res;
+    }
   }; // struct ConstraintRevolute
 
   template<int axis> 

src/multibody/joint/joint-spherical-ZYX.hpp

@@ -97,6 +97,7 @@ namespace se3
       typedef Eigen::Matrix <_Scalar,3,3,_Options> Matrix3;
       typedef Eigen::Matrix <_Scalar,3,1,_Options> Vector3;
       typedef Eigen::Matrix <_Scalar,6,3,_Options> ConstraintDense;
+      typedef Eigen::Matrix <_Scalar,6,3,_Options> DenseBase;
 
       Matrix3 S_minimal;
 
@@ -187,6 +188,18 @@ namespace se3
                                  
         return result;
       }
+      
+      DenseBase variation(const Motion & m) const
+      {
+        const typename Motion::ConstLinear_t v = m.linear();
+        const typename Motion::ConstAngular_t w = m.angular();
+        
+        DenseBase res;
+        res.template middleRows<3>(Motion::LINEAR) = cross(v,S_minimal);
+        res.template middleRows<3>(Motion::ANGULAR) = cross(w,S_minimal);
+        
+        return res;
+      }
 
     }; // struct ConstraintRotationalSubspace
 
@@ -246,6 +259,13 @@ namespace se3
   {
     return Y.template block<6,3> (0,Inertia::ANGULAR,0,3) * S.S_minimal;
   }
+  
+  inline Eigen::Matrix<double,6,3>
+  operator*(const Inertia::Matrix6 & Y,
+            const JointSphericalZYX::ConstraintRotationalSubspace & S)
+  {
+    return Y.block<6,3> (0,Inertia::ANGULAR,0,3) * S.S_minimal;
+  }
 
   namespace internal
   {

src/multibody/joint/joint-spherical.hpp

@@ -156,11 +156,23 @@ namespace se3
     Eigen::Matrix <double,6,3> se3Action (const SE3 & m) const
     {
       Eigen::Matrix <double,6,3> X_subspace;
-      X_subspace.block <3,3> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation ();
+      X_subspace.block <3,3> (Motion::LINEAR, 0) = cross(m.translation(),m.rotation ());
       X_subspace.block <3,3> (Motion::ANGULAR, 0) = m.rotation ();
 
       return X_subspace;
     }
+    
+    DenseBase variation(const Motion & m) const
+    {
+      const Motion::ConstLinear_t v = m.linear();
+      const Motion::ConstAngular_t w = m.angular();
+      
+      DenseBase res;
+      res.middleRows<3>(LINEAR) = skew(v);
+      res.middleRows<3>(ANGULAR) = skew(w);
+      
+      return res;
+    }
 
   }; // struct ConstraintRotationalSubspace
 

src/multibody/joint/joint-translation.hpp

@@ -172,6 +172,17 @@ namespace se3
 
       return M;
     }
+    
+    DenseBase variation(const Motion & m) const
+    {
+      const Motion::ConstAngular_t w = m.angular();
+      
+      DenseBase res;
+      res.middleRows<3>(LINEAR) = skew(w);
+      res.middleRows<3>(ANGULAR).setZero();
+      
+      return res;
+    }
 
   }; // struct ConstraintTranslationSubspace
 

src/multibody/model.hpp

@@ -402,6 +402,9 @@ namespace se3
     /// \brief Vector of sub-tree composite rigid body inertias, i.e. the apparent inertia of the subtree supported by the joint.
     container::aligned_vector<Inertia> Ycrb;
     
+    /// \brief Vector of sub-tree composite rigid body inertia time derivatives \f$ \dot{Y}_{crb}$\f. See Data::Ycrb for more details.
+    container::aligned_vector<Inertia::Matrix6> dYcrb; // TODO: change with dense symmetric matrix6
+    
     /// \brief The joint space inertia matrix (a square matrix of dim model.nv).
     Eigen::MatrixXd M;
     
@@ -410,18 +413,23 @@ namespace se3
     
     // ABA internal data
     /// \brief Inertia matrix of the subtree expressed as dense matrix [ABA]
-    container::aligned_vector<Inertia::Matrix6> Yaba;
+    container::aligned_vector<Inertia::Matrix6> Yaba;  // TODO: change with dense symmetric matrix6
     
     /// \brief Intermediate quantity corresponding to apparent torque [ABA]
     Eigen::VectorXd u;                  // Joint Inertia
     
     // CCRBA return quantities
     /// \brief Centroidal Momentum Matrix
-    /// \note \f$ hg = Ag \dot{q}\f$ maps the joint velocity set to the centroidal momentum.
+    /// \note \f$ hg = A_g \dot{q}\f$ maps the joint velocity set to the centroidal momentum.
     Matrix6x Ag;
     
+    // dCCRBA return quantities
+    /// \brief Centroidal Momentum Matrix Time Variation
+    /// \note \f$ \dot{h_g} = A_g \ddot{q}\ + \dot{A_g}\dot{q}f$ maps the joint velocity and acceleration vectors to the time variation of the centroidal momentum.
+    Matrix6x dAg;
+    
     /// \brief Centroidal momentum quantity.
-    /// \note The centroidal momentum is expressed in the frame centered at the CoM and aligned with the inertial frame.
+    /// \note The centroidal momentum is expressed in the frame centered at the CoM and aligned with the inertial frame (i.e. the world frame).
     ///
     Force hg;
     

src/multibody/model.hxx

@@ -238,11 +238,13 @@ namespace se3
     ,nle(model.nv)
     ,oMf((std::size_t)model.nframes)
     ,Ycrb((std::size_t)model.njoints)
+    ,dYcrb((std::size_t)model.njoints)
     ,M(model.nv,model.nv)
     ,ddq(model.nv)
     ,Yaba((std::size_t)model.njoints)
     ,u(model.nv)
     ,Ag(6,model.nv)
+    ,dAg(6,model.nv)
     ,Fcrb((std::size_t)model.njoints)
     ,lastChild((std::size_t)model.njoints)
     ,nvSubtree((std::size_t)model.njoints)

src/spatial/inertia.hpp

 //
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2015-2017 CNRS
 // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
 //
 // This file is part of Pinocchio
@@ -61,6 +61,7 @@ namespace se3
     Force operator*(const Motion & v) const    { return derived().__mult__(v); }
 
     Scalar vtiv(const Motion & v) const { return derived().vtiv_impl(v); }
+    Matrix6 variation(const Motion & v) const { return derived().variation_impl(v); }
 
     void setZero() { derived().setZero(); }
     void setIdentity() { derived().setIdentity(); }
@@ -119,6 +120,8 @@ namespace se3
     SPATIAL_TYPEDEF_TEMPLATE(InertiaTpl);
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     
+    typedef typename Symmetric3::AlphaSkewSquare AlphaSkewSquare;
+    
   public:
     // Constructors
     InertiaTpl() : m(), c(), I() {}
@@ -230,12 +233,12 @@ namespace se3
     Matrix6 matrix_impl() const
     {
       Matrix6 M;
-      const Matrix3 & c_cross = (skew(c));
+      
       M.template block<3,3>(LINEAR, LINEAR ).setZero ();
       M.template block<3,3>(LINEAR, LINEAR ).diagonal ().fill (m);
-      M.template block<3,3>(ANGULAR,LINEAR ) = m * c_cross;