Commit 17b856fa authored by jcarpent's avatar jcarpent
Browse files

[Algo] Add default quantities in Data to store results of rnea-derivatives

parent 813947c8
......@@ -54,7 +54,8 @@ namespace se3
/// \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).
/// \remark rnea_partial_dq, rnea_partial_dv and rnea_partial_da must be first initialized with zeros (gravity_partial_dq.setZero).
/// As for se3::crba, only the upper triangular part of rnea_partial_da is filled.
///
/// \sa se3::rnea
///
......@@ -66,7 +67,35 @@ namespace se3
Eigen::MatrixXd & rnea_partial_dq,
Eigen::MatrixXd & rnea_partial_dv,
Eigen::MatrixXd & rnea_partial_da);
///
/// \brief Computes the derivatives of the Recursive Newton Euler Algorithms
/// with respect to the joint configuration and the joint velocity.
///
/// \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 velocity vector (dim model.nv).
/// \param[in] a The joint acceleration vector (dim model.nv).
///
/// \returns The results are stored in data.dtau_dq, data.tau_dv and data.dtau_da which respectively correspond
/// to the partial derivatives of the joint torque vector with respect to the joint configuration, velocity and acceleration.
/// data.dtau_da is a reference on data.M. And as for se3::crba, only the upper triangular part of data.M is filled.
///
/// \remark rnea_partial_dq and rnea_partial_dv must be first initialized with zeros (gravity_partial_dq.setZero).
///
/// \sa se3::rnea, se3::crba, se3::cholesky::decompose
///
inline void
computeRNEADerivatives(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a)
{
computeRNEADerivatives(model,data,q,v,a,
data.dtau_dq, data.dtau_dv, data.dtau_da);
}
} // namespace se3
......
......@@ -516,6 +516,15 @@ namespace se3
/// \brief Variation of the spatial acceleration set with respect to the joint velocity.
Matrix6x dAdv;
/// \brief Variation of the joint torque vector with respect to the joint configuration.
Eigen::MatrixXd dtau_dq;
/// \brief Variation of the joint torque vector with respect to the joint velocity.
Eigen::MatrixXd dtau_dv;
/// \brief Variation of the joint torque vector with respect to the joint acceleration. This is only a reference on data.M.
Eigen::MatrixXd & dtau_da;
/// \brief Vector of joint placements wrt to algorithm end effector.
container::aligned_vector<SE3> iMf;
......
......@@ -276,6 +276,9 @@ namespace se3
,dVdq(6,model.nv)
,dAdq(6,model.nv)
,dAdv(6,model.nv)
,dtau_dq(Eigen::MatrixXd::Zero(model.nv,model.nv))
,dtau_dv(Eigen::MatrixXd::Zero(model.nv,model.nv))
,dtau_da(M)
,iMf((std::size_t)model.njoints)
,com((std::size_t)model.njoints)
,vcom((std::size_t)model.njoints)
......
......@@ -248,6 +248,16 @@ BOOST_AUTO_TEST_CASE(test_rnea_derivatives)
BOOST_CHECK(data.tau.isApprox(tau0));
BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(alpha)));
BOOST_CHECK(rnea_partial_dv.isApprox(rnea_partial_dv_fd,sqrt(alpha)));
Data data2(model);
computeRNEADerivatives(model,data2,q,v,a);
data2.M.triangularView<Eigen::StrictlyLower>()
= data2.M.transpose().triangularView<Eigen::StrictlyLower>();
BOOST_CHECK(rnea_partial_dq.isApprox(data2.dtau_dq));
BOOST_CHECK(rnea_partial_dv.isApprox(data2.dtau_dv));
BOOST_CHECK(rnea_partial_da.isApprox(data2.dtau_da));
}
BOOST_AUTO_TEST_SUITE_END()
Supports Markdown
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