unittest/centroidal-derivatives.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018-2021 INRIA
3 //
4 
5 #include "pinocchio/algorithm/crba.hpp"
6 #include "pinocchio/algorithm/rnea.hpp"
7 #include "pinocchio/algorithm/centroidal.hpp"
8 #include "pinocchio/algorithm/centroidal-derivatives.hpp"
9 #include "pinocchio/algorithm/rnea-derivatives.hpp"
10 #include "pinocchio/algorithm/aba-derivatives.hpp"
11 #include "pinocchio/algorithm/jacobian.hpp"
12 #include "pinocchio/algorithm/center-of-mass.hpp"
13 #include "pinocchio/algorithm/joint-configuration.hpp"
14 #include "pinocchio/multibody/sample-models.hpp"
15 
16 #include <iostream>
17 
18 #include <boost/test/unit_test.hpp>
19 #include <boost/utility/binary.hpp>
20 
21 template<typename JointModel>
22 static void addJointAndBody(
23  pinocchio::Model & model,
24  const pinocchio::JointModelBase<JointModel> & joint,
25  const std::string & parent_name,
26  const std::string & name,
27  const pinocchio::SE3 placement = pinocchio::SE3::Random(),
28  bool setRandomLimits = true)
29 {
30  using namespace pinocchio;
31  typedef typename JointModel::ConfigVector_t CV;
32  typedef typename JointModel::TangentVector_t TV;
33 
34  Model::JointIndex idx;
35 
36  if (setRandomLimits)
37  idx = model.addJoint(
38  model.getJointId(parent_name), joint, SE3::Random(), name + "_joint",
39  TV::Random() + TV::Constant(1), TV::Random() + TV::Constant(1),
40  CV::Random() - CV::Constant(1), CV::Random() + CV::Constant(1));
41  else
42  idx = model.addJoint(model.getJointId(parent_name), joint, placement, name + "_joint");
43 
44  model.addJointFrame(idx);
45 
46  model.appendBodyToJoint(idx, Inertia::Random(), SE3::Identity());
47  model.addBodyFrame(name + "_body", idx);
48 }
49 
50 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
51 
52 BOOST_AUTO_TEST_CASE(test_centroidal_derivatives)
53 {
54  pinocchio::Model model;
55  pinocchio::buildModels::humanoidRandom(model);
56  const std::string parent_name = model.names.back();
57  const std::string joint_name = "ee_spherical_joint";
58  addJointAndBody(model, pinocchio::JointModelSpherical(), parent_name, joint_name);
59 
60  pinocchio::Data data(model), data_ref(model);
61 
62  model.lowerPositionLimit.head<7>().fill(-1.);
63  model.upperPositionLimit.head<7>().fill(1.);
64 
65  Eigen::VectorXd q = pinocchio::randomConfiguration(model);
66  Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
67  Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv);
68 
69  pinocchio::Data::Matrix6x dh_dq(6, model.nv), dhdot_dq(6, model.nv), dhdot_dv(6, model.nv),
70  dhdot_da(6, model.nv);
71  pinocchio::computeCentroidalDynamicsDerivatives(
72  model, data, q, v, a, dh_dq, dhdot_dq, dhdot_dv, dhdot_da);
73  pinocchio::ccrba(model, data_ref, q, v);
74 
75  for (size_t k = 0; k < (size_t)model.njoints; ++k)
76  {
77  BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
78  BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k]));
79  }
80  BOOST_CHECK(dhdot_da.isApprox(data_ref.Ag));
81 
82  pinocchio::computeCentroidalMomentumTimeVariation(model, data_ref, q, v, a);
83  for (size_t k = 1; k < (size_t)model.njoints; ++k)
84  {
85  BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
86  BOOST_CHECK(data.ov[k].isApprox(data.oMi[k].act(data_ref.v[k])));
87  BOOST_CHECK(data.oa[k].isApprox(data.oMi[k].act(data_ref.a[k])));
88  BOOST_CHECK(data.oh[k].isApprox(data.oMi[k].act(data_ref.h[k])));
89  }
90 
91  BOOST_CHECK(data.mass[0] == data_ref.mass[0]);
92  BOOST_CHECK(data.com[0].isApprox(data_ref.com[0]));
93 
94  BOOST_CHECK(data.oh[0].isApprox(data_ref.h[0]));
95  BOOST_CHECK(data.of[0].isApprox(data_ref.f[0]));
96 
97  BOOST_CHECK(data.hg.isApprox(data_ref.hg));
98  BOOST_CHECK(data.dhg.isApprox(data_ref.dhg));
99  BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
100 
101  pinocchio::Data data_fd(model);
102 
103  const double eps = 1e-8;
104  const pinocchio::Force dhg =
105  pinocchio::computeCentroidalMomentumTimeVariation(model, data_fd, q, v, a);
106  const pinocchio::Force hg = data_fd.hg;
107  BOOST_CHECK(data.hg.isApprox(data_ref.hg));
108 
109  // Check dhdot_dq and dh_dq with finite differences
110  Eigen::VectorXd q_plus(model.nq, 1);
111  Eigen::VectorXd v_eps(model.nv, 1);
112  v_eps.setZero();
113  pinocchio::Data::Matrix6x dhdot_dq_fd(6, model.nv);
114  pinocchio::Data::Matrix6x dh_dq_fd(6, model.nv);
115 
116  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
117  {
118  v_eps[k] = eps;
119  q_plus = pinocchio::integrate(model, q, v_eps);
120 
121  const pinocchio::Force & dhg_plus =
122  pinocchio::computeCentroidalMomentumTimeVariation(model, data_fd, q_plus, v, a);
123  const pinocchio::Force hg_plus = data_fd.hg;
124  dhdot_dq_fd.col(k) = (dhg_plus - dhg).toVector() / eps;
125  dh_dq_fd.col(k) = (hg_plus - hg).toVector() / eps;
126  v_eps[k] = 0.;
127  }
128 
129  BOOST_CHECK(dhdot_dq.isApprox(dhdot_dq_fd, sqrt(eps)));
130  BOOST_CHECK(dh_dq.isApprox(dh_dq_fd, sqrt(eps)));
131  // Check dhdot_dv with finite differences
132  Eigen::VectorXd v_plus(v);
133  pinocchio::Data::Matrix6x dhdot_dv_fd(6, model.nv);
134 
135  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
136  {
137  v_plus[k] += eps;
138 
139  const pinocchio::Force & dhg_plus =
140  pinocchio::computeCentroidalMomentumTimeVariation(model, data_fd, q, v_plus, a);
141  dhdot_dv_fd.col(k) = (dhg_plus - dhg).toVector() / eps;
142 
143  v_plus[k] -= eps;
144  }
145 
146  BOOST_CHECK(dhdot_dv.isApprox(dhdot_dv_fd, sqrt(eps)));
147 
148  // Check dhdot_da with finite differences
149  Eigen::VectorXd a_plus(a);
150  pinocchio::Data::Matrix6x dhdot_da_fd(6, model.nv);
151 
152  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
153  {
154  a_plus[k] += eps;
155 
156  const pinocchio::Force & dhg_plus =
157  pinocchio::computeCentroidalMomentumTimeVariation(model, data_fd, q, v, a_plus);
158  dhdot_da_fd.col(k) = (dhg_plus - dhg).toVector() / eps;
159 
160  a_plus[k] -= eps;
161  }
162 
163  BOOST_CHECK(dhdot_da.isApprox(dhdot_da_fd, sqrt(eps)));
164 
165  pinocchio::computeRNEADerivatives(model, data_ref, q, v, a);
166  BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv));
167  BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq));
168  BOOST_CHECK(data.J.isApprox(data_ref.J));
169  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
170  BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq));
171 
172  pinocchio::computeCentroidalMap(model, data_ref, q);
173  BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
174 }
175 
176 BOOST_AUTO_TEST_CASE(test_retrieve_centroidal_derivatives_fromRNEA)
177 {
178  pinocchio::Model model;
179  pinocchio::buildModels::humanoidRandom(model);
180  pinocchio::Data data(model), data_ref(model);
181 
182  model.lowerPositionLimit.head<7>().fill(-1.);
183  model.upperPositionLimit.head<7>().fill(1.);
184 
185  Eigen::VectorXd q = pinocchio::randomConfiguration(model);
186  Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
187  Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv);
188 
189  pinocchio::Data::Matrix6x dh_dq(6, model.nv), dhdot_dq(6, model.nv), dhdot_dv(6, model.nv),
190  dhdot_da(6, model.nv);
191  pinocchio::Data::Matrix6x dh_dq_ref(6, model.nv), dhdot_dq_ref(6, model.nv),
192  dhdot_dv_ref(6, model.nv), dhdot_da_ref(6, model.nv);
193 
194  pinocchio::computeCentroidalDynamicsDerivatives(
195  model, data_ref, q, v, a, dh_dq_ref, dhdot_dq_ref, dhdot_dv_ref, dhdot_da_ref);
196 
197  pinocchio::computeRNEADerivatives(model, data, q, v, a);
198  pinocchio::getCentroidalDynamicsDerivatives(model, data, dh_dq, dhdot_dq, dhdot_dv, dhdot_da);
199 
200  BOOST_CHECK(data.J.isApprox(data_ref.J));
201 
202  for (pinocchio::Model::JointIndex k = 1; k < (pinocchio::Model::JointIndex)model.njoints; ++k)
203  {
204  BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k]));
205  pinocchio::Force force_ref = data_ref.of[k];
206  pinocchio::Force gravity_contribution = data.oYcrb[k] * (-model.gravity);
207  pinocchio::Force force = data.of[k] - gravity_contribution;
208  BOOST_CHECK(force.isApprox(force_ref));
209  }
210 
211  BOOST_CHECK(data.com[0].isApprox(data_ref.com[0]));
212 
213  BOOST_CHECK(data.hg.isApprox(data_ref.hg));
214  BOOST_CHECK(data.dhg.isApprox(data_ref.dhg));
215 
216  BOOST_CHECK(data.Fcrb[0].isApprox(data_ref.dFdq));
217  BOOST_CHECK(data.dFdv.isApprox(data_ref.dFdv));
218  BOOST_CHECK(data.dFda.isApprox(data_ref.dFda));
219  BOOST_CHECK(dh_dq.isApprox(dh_dq_ref));
220  BOOST_CHECK(dhdot_dq.isApprox(dhdot_dq_ref));
221  BOOST_CHECK(dhdot_dv.isApprox(dhdot_dv_ref));
222  BOOST_CHECK(dhdot_da.isApprox(dhdot_da_ref));
223 }
224 
225 BOOST_AUTO_TEST_CASE(test_retrieve_centroidal_derivatives_fromABA)
226 {
227  pinocchio::Model model;
228  pinocchio::buildModels::humanoidRandom(model);
229  pinocchio::Data data(model), data_ref(model), data_rnea(model);
230 
231  model.lowerPositionLimit.head<7>().fill(-1.);
232  model.upperPositionLimit.head<7>().fill(1.);
233 
234  Eigen::VectorXd q = pinocchio::randomConfiguration(model);
235  Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
236  Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv);
237 
238  pinocchio::Data::Matrix6x dh_dq(6, model.nv), dhdot_dq(6, model.nv), dhdot_dv(6, model.nv),
239  dhdot_da(6, model.nv);
240  pinocchio::Data::Matrix6x dh_dq_ref(6, model.nv), dhdot_dq_ref(6, model.nv),
241  dhdot_dv_ref(6, model.nv), dhdot_da_ref(6, model.nv);
242 
243  pinocchio::computeCentroidalDynamicsDerivatives(
244  model, data_ref, q, v, a, dh_dq_ref, dhdot_dq_ref, dhdot_dv_ref, dhdot_da_ref);
245 
246  Eigen::VectorXd tau = rnea(model, data_rnea, q, v, a);
247  pinocchio::computeABADerivatives(model, data, q, v, tau);
248  pinocchio::computeRNEADerivatives(model, data_rnea, q, v, a);
249  pinocchio::getCentroidalDynamicsDerivatives(model, data, dh_dq, dhdot_dq, dhdot_dv, dhdot_da);
250 
251  BOOST_CHECK(data.J.isApprox(data_ref.J));
252  BOOST_CHECK(data.dVdq.isApprox(data_rnea.dVdq));
253  BOOST_CHECK(data.dAdq.isApprox(data_rnea.dAdq));
254  BOOST_CHECK(data.dAdv.isApprox(data_rnea.dAdv));
255  BOOST_CHECK(data.dFdq.isApprox(data_rnea.dFdq));
256  BOOST_CHECK(data.dFdv.isApprox(data_rnea.dFdv));
257  BOOST_CHECK(data.dFda.isApprox(data_rnea.dFda));
258 
259  for (pinocchio::Model::JointIndex k = 1; k < (pinocchio::Model::JointIndex)model.njoints; ++k)
260  {
261  BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k]));
262  BOOST_CHECK(data.oh[k].isApprox(data_ref.oh[k]));
263  const pinocchio::Force & force_ref = data_ref.of[k];
264  const pinocchio::Force gravity_contribution = data.oYcrb[k] * (-model.gravity);
265  const pinocchio::Force force = data.of[k] - gravity_contribution;
266  BOOST_CHECK(force.isApprox(force_ref));
267  }
268 
269  BOOST_CHECK(data.com[0].isApprox(data_ref.com[0]));
270 
271  BOOST_CHECK(data.hg.isApprox(data_ref.hg));
272  BOOST_CHECK(data.dhg.isApprox(data_ref.dhg));
273 
274  BOOST_CHECK(data.Fcrb[0].isApprox(data_ref.dFdq));
275  BOOST_CHECK(data.dFdv.isApprox(data_ref.dFdv));
276  BOOST_CHECK(data.dFda.isApprox(data_ref.dFda));
277  BOOST_CHECK(dh_dq.isApprox(dh_dq_ref));
278  BOOST_CHECK(dhdot_dq.isApprox(dhdot_dq_ref));
279  BOOST_CHECK(dhdot_dv.isApprox(dhdot_dv_ref));
280  BOOST_CHECK(dhdot_da.isApprox(dhdot_da_ref));
281 }
282 
283 BOOST_AUTO_TEST_SUITE_END()
display-shapes-meshcat.placement
placement
Definition: display-shapes-meshcat.py:24
pinocchio::DataTpl::dFdq
Matrix6x dFdq
Variation of the forceset with respect to the joint configuration.
Definition: multibody/data.hpp:211
pinocchio::computeRNEADerivatives
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...
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::DataTpl::dAdv
Matrix6x dAdv
Variation of the spatial acceleration set with respect to the joint velocity.
Definition: multibody/data.hpp:397
pinocchio::DataTpl::mass
std::vector< Scalar > mass
Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint ....
Definition: multibody/data.hpp:458
pinocchio::DataTpl::dhg
Force dhg
Centroidal momentum time derivative.
Definition: multibody/data.hpp:305
addJointAndBody
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)
Definition: unittest/centroidal-derivatives.cpp:22
pinocchio::JointModelBase
Definition: joint-model-base.hpp:78
pinocchio::DataTpl::dJ
Matrix6x dJ
Derivative of the Jacobian with respect to the time.
Definition: multibody/data.hpp:377
pinocchio::SE3
context::SE3 SE3
Definition: spatial/fwd.hpp:59
pinocchio::name
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
pinocchio::DataTpl::dFdv
Matrix6x dFdv
Variation of the forceset with respect to the joint velocity.
Definition: multibody/data.hpp:214
rnea-derivatives.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::buildModels::humanoidRandom
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true, bool mimic=false)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
pinocchio::DataTpl::dFda
Matrix6x dFda
Variation of the forceset with respect to the joint acceleration.
Definition: multibody/data.hpp:217
pinocchio::randomConfiguration
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.
Definition: joint-configuration.hpp:315
rnea.hpp
aba-derivatives.hpp
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_centroidal_derivatives)
Definition: unittest/centroidal-derivatives.cpp:52
anymal-simulation.model
model
Definition: anymal-simulation.py:8
pinocchio::ModelTpl< Scalar >::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:67
forward-dynamics-derivatives.tau
tau
Definition: forward-dynamics-derivatives.py:25
center-of-mass.hpp
joint-configuration.hpp
pinocchio::Force
context::Force Force
Definition: spatial/fwd.hpp:63
pinocchio::placement
const MotionDense< Motion2 > const SE3Tpl< SE3Scalar, SE3Options > & placement
Definition: spatial/classic-acceleration.hpp:122
pinocchio::computeCentroidalMap
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.
pinocchio::integrate
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.
Definition: joint-configuration.hpp:70
cartpole.v
v
Definition: cartpole.py:153
pinocchio::ccrba
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...
pinocchio::JointModelSphericalTpl
Definition: multibody/joint/fwd.hpp:73
pinocchio::computeCentroidalMomentumTimeVariation
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...
pinocchio::DataTpl::Ag
Matrix6x Ag
Centroidal Momentum Matrix.
Definition: multibody/data.hpp:284
setup.name
name
Definition: cmake/cython/setup.in.py:179
q
q
a
Vec3f a
pinocchio::DataTpl::J
Matrix6x J
Jacobian of joint placements.
Definition: multibody/data.hpp:374
pinocchio::getCentroidalDynamicsDerivatives
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....
pinocchio::computeABADerivatives
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.
append-urdf-model-with-another-model.joint_name
string joint_name
Definition: append-urdf-model-with-another-model.py:33
fill
fill
centroidal.hpp
pinocchio::rnea
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...
pinocchio::DataTpl::hg
Force hg
Centroidal momentum quantity.
Definition: multibody/data.hpp:297
centroidal-derivatives.hpp
dcrba.eps
int eps
Definition: dcrba.py:458
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: multibody/data.hpp:92
sample-models.hpp
jacobian.hpp
pinocchio::ModelTpl< context::Scalar, context::Options >
crba.hpp
pinocchio::DataTpl::dVdq
Matrix6x dVdq
Variation of the spatial velocity set with respect to the joint configuration.
Definition: multibody/data.hpp:391
pinocchio::computeCentroidalDynamicsDerivatives
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...
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1082
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:33
pinocchio::DataTpl::dAdq
Matrix6x dAdq
Variation of the spatial acceleration set with respect to the joint configuration.
Definition: multibody/data.hpp:394

pinocchio
Author(s):
autogenerated on Wed Apr 16 2025 02:41:44