algo/center-of-mass-derivatives: improve efficiency

Number of operations are reduced and it also avoid memory allocations

src/algorithm/center-of-mass-derivatives.hxx

 //
-// Copyright (c) 2019 CNRS
+// Copyright (c) 2019 CNRS INRIA
 //
 
 #ifndef __pinocchio_center_of_mass_derivatives_hxx__
@@ -37,17 +37,20 @@ namespace pinocchio
       typedef typename Data::Motion Motion;
       
       const JointIndex & i = jmodel.id();
+      const JointIndex & parent = model.parents[i];
 
       Matrix3xOut & dvcom_dq = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut,vcom_partial_dq);
       typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix3x>::Type
         dvcom_dqi = jmodel.jointCols(dvcom_dq);
 
-      Motion & vpc = data.v[0] = (model.parents[i]>0) ? (data.v[i]-(Motion)jdata.v()) : Motion::Zero();
+      Motion & vpc = data.v[0] = (parent>0) ? (data.v[i]-(Motion)jdata.v()) : Motion::Zero();
       vpc.linear() -= data.vcom[i]; // vpc = v_{parent+c} = [ v_parent+vc; w_parent ]
-      const Eigen::Matrix<Scalar,6,JointModel::NV,Options> & vxS = vpc.cross(jdata.S());
       
-      dvcom_dqi = data.mass[i]*data.oMi[i].rotation()
-        //*( vxS.template topRows<3>() - cross( data.com[i], vxS.template bottomRows<3>()) );
+      typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6>::Type
+      vxS = data.M6tmp.template leftCols<JointModel::NV>(jmodel.nv());
+      vxS = vpc.cross(jdata.S());
+      
+      dvcom_dqi.noalias() = (data.mass[i]/data.mass[0])*data.oMi[i].rotation()
         *( vxS.template middleRows<3>(Motion::LINEAR) - cross( data.com[i], vxS.template middleRows<3>(Motion::ANGULAR)) );
     }
   };
@@ -70,11 +73,11 @@ namespace pinocchio
       
     typedef CoMVelocityDerivativesForwardStep<Scalar,Options,JointCollectionTpl,Matrix3xOut> Pass1;
     for(JointIndex i = 1; i < (JointIndex)model.njoints; i ++)
-      {
-        Pass1::run(model.joints[i],data.joints[i],
-                   typename Pass1::ArgsType(model,data,dvcom_dq));
-      }
-    dvcom_dq /= data.mass[0];
+    {
+      Pass1::run(model.joints[i],data.joints[i],
+                 typename Pass1::ArgsType(model,data,dvcom_dq));
+    }
+    
     data.v[0].setZero();
   }
 

src/multibody/data.hpp

@@ -67,6 +67,7 @@ namespace pinocchio
     /// \brief The 3d jacobian type (temporary)
     typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x;
     
+    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
     typedef Eigen::Matrix<Scalar,6,6,Eigen::RowMajor | Options> RowMatrix6;
     typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor | Options> RowMatrixXs;
     
@@ -181,6 +182,7 @@ namespace pinocchio
     typename Inertia::Matrix6 Itmp;
     
     /// \brief Temporary for derivative algorithms
+    Matrix6 M6tmp;
     RowMatrix6 M6tmpR;
     RowMatrix6 M6tmpR2;