[Algo] Allows an external third argument for RNEA derivatives

src/algorithm/rnea-derivatives.hpp

@@ -52,6 +52,7 @@ namespace se3
   /// \param[in] a The joint acceleration vector (dim model.nv).
   /// \param[out] rnea_partial_dq Partial derivative of the generalized torque vector with respect to the joint configuration.
   /// \param[out] rnea_partial_dv Partial derivative of the generalized torque vector with respect to the joint velocity.
+  /// \param[out] rnea_partial_da Partial derivative of the generalized torque vector with respect to the joint acceleration.
   ///
   /// \remark rnea_partial_dq and rnea_partial_dv must be first initialized with zeros (gravity_partial_dq.setZero).
   ///
@@ -63,7 +64,8 @@ namespace se3
                          const Eigen::VectorXd & v,
                          const Eigen::VectorXd & a,
                          Eigen::MatrixXd & rnea_partial_dq,
-                         Eigen::MatrixXd & rnea_partial_dv);
+                         Eigen::MatrixXd & rnea_partial_dv,
+                         Eigen::MatrixXd & rnea_partial_da);
  
 
 } // namespace se3 

src/algorithm/rnea-derivatives.hxx

@@ -242,6 +242,7 @@ namespace se3
     typedef boost::fusion::vector<const Model &,
     Data &,
     Eigen::MatrixXd &,
+    Eigen::MatrixXd &,
     Eigen::MatrixXd &
     >  ArgsType;
     
@@ -252,7 +253,8 @@ namespace se3
                      const Model & model,
                      Data & data,
                      Eigen::MatrixXd & rnea_partial_dq,
-                     Eigen::MatrixXd & rnea_partial_dv)
+                     Eigen::MatrixXd & rnea_partial_dv,
+                     Eigen::MatrixXd & rnea_partial_da)
     {
       const Model::JointIndex & i = jmodel.id();
       const Model::JointIndex & parent = model.parents[i];
@@ -273,7 +275,7 @@ namespace se3
       
       // dtau/da similar to data.M
       motionSet::inertiaAction(data.oYo[i],J_cols,dFda_cols);
-      data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
+      rnea_partial_da.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
       = J_cols.transpose()*data.dFda.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
       
       // dtau/dv
@@ -332,7 +334,8 @@ namespace se3
                          const Eigen::VectorXd & v,
                          const Eigen::VectorXd & a,
                          Eigen::MatrixXd & rnea_partial_dq,
-                         Eigen::MatrixXd & rnea_partial_dv)
+                         Eigen::MatrixXd & rnea_partial_dv,
+                         Eigen::MatrixXd & rnea_partial_da)
   {
     assert(q.size() == model.nq && "The joint configuration vector is not of right size");
     assert(v.size() == model.nv && "The joint velocity vector is not of right size");
@@ -341,6 +344,8 @@ namespace se3
     assert(rnea_partial_dq.rows() == model.nv);
     assert(rnea_partial_dv.cols() == model.nv);
     assert(rnea_partial_dv.rows() == model.nv);
+    assert(rnea_partial_da.cols() == model.nv);
+    assert(rnea_partial_da.rows() == model.nv);
     assert(model.check(data) && "data is not consistent with model.");
     
     data.oa[0] = -model.gravity;
@@ -354,7 +359,7 @@ namespace se3
     for(size_t i=(size_t) (model.njoints-1);i>0;--i)
     {
       computeRNEADerivativesBackwardStep::run(model.joints[i],
-                                              computeRNEADerivativesBackwardStep::ArgsType(model,data,rnea_partial_dq,rnea_partial_dv));
+                                              computeRNEADerivativesBackwardStep::ArgsType(model,data,rnea_partial_dq,rnea_partial_dv,rnea_partial_da));
     }
   }
   

unittest/rnea-derivatives.cpp

@@ -93,7 +93,8 @@ BOOST_AUTO_TEST_CASE(test_rnea_derivatives)
   /// Check againt computeGeneralizedGravityDerivatives
   MatrixXd rnea_partial_dq(model.nv,model.nv); rnea_partial_dq.setZero();
   MatrixXd rnea_partial_dv(model.nv,model.nv); rnea_partial_dv.setZero();
-  computeRNEADerivatives(model,data,q,0*v,0*a,rnea_partial_dq,rnea_partial_dv);
+  MatrixXd rnea_partial_da(model.nv,model.nv); rnea_partial_da.setZero();
+  computeRNEADerivatives(model,data,q,0*v,0*a,rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
   
   MatrixXd g_partial_dq(model.nv,model.nv); g_partial_dq.setZero();
   computeGeneralizedGravityDerivatives(model,data_ref,q,g_partial_dq);
@@ -134,7 +135,7 @@ BOOST_AUTO_TEST_CASE(test_rnea_derivatives)
   }
   
   rnea_partial_dq.setZero();
-  computeRNEADerivatives(model,data,q,0*v,a,rnea_partial_dq,rnea_partial_dv);
+  computeRNEADerivatives(model,data,q,0*v,a,rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
   forwardKinematics(model,data_ref,q,0*v,a);
   
   for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
@@ -178,7 +179,7 @@ BOOST_AUTO_TEST_CASE(test_rnea_derivatives)
   
   rnea_partial_dq.setZero();
   rnea_partial_dv.setZero();
-  computeRNEADerivatives(model,data,q,v,0*a,rnea_partial_dq,rnea_partial_dv);
+  computeRNEADerivatives(model,data,q,v,0*a,rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
   forwardKinematics(model,data_ref,q,v,0*a);
   
   for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
@@ -224,7 +225,7 @@ BOOST_AUTO_TEST_CASE(test_rnea_derivatives)
   
   rnea_partial_dq.setZero();
   rnea_partial_dv.setZero();
-  computeRNEADerivatives(model,data,q,v,a,rnea_partial_dq,rnea_partial_dv);
+  computeRNEADerivatives(model,data,q,v,a,rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
   forwardKinematics(model,data_ref,q,v,a);
   
   for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
@@ -238,11 +239,11 @@ BOOST_AUTO_TEST_CASE(test_rnea_derivatives)
   BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
   crba(model,data_ref,q);
   
-  data.M.triangularView<Eigen::StrictlyLower>()
-  = data.M.transpose().triangularView<Eigen::StrictlyLower>();
+  rnea_partial_da.triangularView<Eigen::StrictlyLower>()
+  = rnea_partial_da.transpose().triangularView<Eigen::StrictlyLower>();
   data_ref.M.triangularView<Eigen::StrictlyLower>()
   = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
-  BOOST_CHECK(data.M.isApprox(data_ref.M));
+  BOOST_CHECK(rnea_partial_da.isApprox(data_ref.M));
 
   BOOST_CHECK(data.tau.isApprox(tau0));
   BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(alpha)));