Commit 0b0335b1 authored by jcarpent's avatar jcarpent
Browse files

[Algo] Allows an external third argument for RNEA derivatives

parent 60407422
......@@ -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
......
......@@ -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));
}
}
......
......@@ -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)));
......
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