Go to the documentation of this file.
18 #include <boost/test/unit_test.hpp>
19 #include <boost/utility/binary.hpp>
21 template<
typename Jo
intModel>
25 const std::string & parent_name,
26 const std::string &
name,
28 bool setRandomLimits =
true)
31 typedef typename JointModel::ConfigVector_t CV;
32 typedef typename JointModel::TangentVector_t TV;
39 TV::Random() + TV::Constant(1), TV::Random() + TV::Constant(1),
40 CV::Random() - CV::Constant(1), CV::Random() + CV::Constant(1));
44 model.addJointFrame(idx);
50 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
56 const std::string parent_name =
model.names.back();
57 const std::string
joint_name =
"ee_spherical_joint";
62 model.lowerPositionLimit.head<7>().
fill(-1.);
63 model.upperPositionLimit.head<7>().
fill(1.);
66 Eigen::VectorXd
v = Eigen::VectorXd::Random(
model.nv);
67 Eigen::VectorXd
a = Eigen::VectorXd::Random(
model.nv);
70 dhdot_da(6,
model.nv);
72 model,
data,
q,
v,
a, dh_dq, dhdot_dq, dhdot_dv, dhdot_da);
75 for (
size_t k = 0; k < (size_t)
model.njoints; ++k)
83 for (
size_t k = 1; k < (size_t)
model.njoints; ++k)
103 const double eps = 1e-8;
110 Eigen::VectorXd q_plus(
model.nq, 1);
111 Eigen::VectorXd v_eps(
model.nv, 1);
116 for (Eigen::DenseIndex k = 0; k <
model.nv; ++k)
124 dhdot_dq_fd.col(k) = (dhg_plus - dhg).toVector() /
eps;
125 dh_dq_fd.col(k) = (hg_plus - hg).toVector() /
eps;
132 Eigen::VectorXd v_plus(
v);
135 for (Eigen::DenseIndex k = 0; k <
model.nv; ++k)
141 dhdot_dv_fd.col(k) = (dhg_plus - dhg).toVector() /
eps;
149 Eigen::VectorXd a_plus(
a);
152 for (Eigen::DenseIndex k = 0; k <
model.nv; ++k)
158 dhdot_da_fd.col(k) = (dhg_plus - dhg).toVector() /
eps;
182 model.lowerPositionLimit.head<7>().
fill(-1.);
183 model.upperPositionLimit.head<7>().
fill(1.);
186 Eigen::VectorXd
v = Eigen::VectorXd::Random(
model.nv);
187 Eigen::VectorXd
a = Eigen::VectorXd::Random(
model.nv);
190 dhdot_da(6,
model.nv);
192 dhdot_dv_ref(6,
model.nv), dhdot_da_ref(6,
model.nv);
195 model, data_ref,
q,
v,
a, dh_dq_ref, dhdot_dq_ref, dhdot_dv_ref, dhdot_da_ref);
231 model.lowerPositionLimit.head<7>().
fill(-1.);
232 model.upperPositionLimit.head<7>().
fill(1.);
235 Eigen::VectorXd
v = Eigen::VectorXd::Random(
model.nv);
236 Eigen::VectorXd
a = Eigen::VectorXd::Random(
model.nv);
239 dhdot_da(6,
model.nv);
241 dhdot_dv_ref(6,
model.nv), dhdot_da_ref(6,
model.nv);
244 model, data_ref,
q,
v,
a, dh_dq_ref, dhdot_dq_ref, dhdot_dv_ref, dhdot_da_ref);
283 BOOST_AUTO_TEST_SUITE_END()
Matrix6x dFdq
Variation of the forceset with respect to the joint configuration.
void computeRNEADerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a, const Eigen::MatrixBase< MatrixType1 > &rnea_partial_dq, const Eigen::MatrixBase< MatrixType2 > &rnea_partial_dv, const Eigen::MatrixBase< MatrixType3 > &rnea_partial_da)
Computes the partial derivatives of the Recursive Newton Euler Algorithms with respect to the joint c...
Matrix6x dAdv
Variation of the spatial acceleration set with respect to the joint velocity.
std::vector< Scalar > mass
Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint ....
Force dhg
Centroidal momentum time derivative.
static void addJointAndBody(pinocchio::Model &model, const pinocchio::JointModelBase< JointModel > &joint, const std::string &parent_name, const std::string &name, const pinocchio::SE3 placement=pinocchio::SE3::Random(), bool setRandomLimits=true)
Matrix6x dJ
Derivative of the Jacobian with respect to the time.
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
Matrix6x dFdv
Variation of the forceset with respect to the joint velocity.
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
Matrix6x dFda
Variation of the forceset with respect to the joint acceleration.
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
BOOST_AUTO_TEST_CASE(test_centroidal_derivatives)
pinocchio::JointIndex JointIndex
const MotionDense< Motion2 > const SE3Tpl< SE3Scalar, SE3Options > & placement
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeCentroidalMap(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the Centroidal Momentum Matrix.
void integrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< ReturnType > &qout)
Integrate a configuration vector for the specified model for a tangent vector during one unit time.
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & ccrba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the Centroidal Momentum Matrix, the Composite Ridig Body Inertia as well as the centroidal m...
static InertiaTpl Random()
const DataTpl< Scalar, Options, JointCollectionTpl >::Force & computeCentroidalMomentumTimeVariation(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Computes the Centroidal momemtum and its time derivatives, a.k.a. the total momenta of the system and...
Matrix6x Ag
Centroidal Momentum Matrix.
Matrix6x J
Jacobian of joint placements.
void getCentroidalDynamicsDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Matrix6xLike1 > &dh_dq, const Eigen::MatrixBase< Matrix6xLike1 > &dhdot_dq, const Eigen::MatrixBase< Matrix6xLike2 > &dhdot_dv, const Eigen::MatrixBase< Matrix6xLike3 > &dhdot_da)
Retrive the analytical derivatives of the centroidal dynamics from the RNEA derivatives....
void computeABADerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau, const Eigen::MatrixBase< MatrixType1 > &aba_partial_dq, const Eigen::MatrixBase< MatrixType2 > &aba_partial_dv, const Eigen::MatrixBase< MatrixType3 > &aba_partial_dtau)
The derivatives of the Articulated-Body algorithm.
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & rnea(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
Force hg
Centroidal momentum quantity.
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Matrix6x dVdq
Variation of the spatial velocity set with respect to the joint configuration.
void computeCentroidalDynamicsDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a, const Eigen::MatrixBase< Matrix6xLike0 > &dh_dq, const Eigen::MatrixBase< Matrix6xLike1 > &dhdot_dq, const Eigen::MatrixBase< Matrix6xLike2 > &dhdot_dv, const Eigen::MatrixBase< Matrix6xLike3 > &dhdot_da)
Computes the analytical derivatives of the centroidal dynamics with respect to the joint configuratio...
JointCollectionTpl & model
Main pinocchio namespace.
#define BOOST_CHECK(check)
Matrix6x dAdq
Variation of the spatial acceleration set with respect to the joint configuration.
pinocchio
Author(s):
autogenerated on Sat Jun 22 2024 02:41:43