contact-dynamics-derivatives.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019-2020 INRIA
3 //
4 
5 #include "pinocchio/algorithm/jacobian.hpp"
6 #include "pinocchio/algorithm/rnea.hpp"
7 #include "pinocchio/algorithm/rnea-derivatives.hpp"
8 #include "pinocchio/algorithm/kinematics-derivatives.hpp"
9 #include "pinocchio/algorithm/contact-dynamics.hpp"
10 #include "pinocchio/algorithm/joint-configuration.hpp"
11 #include "pinocchio/parsers/sample-models.hpp"
12 
13 #include <iostream>
14 
15 #include <boost/test/unit_test.hpp>
16 #include <boost/utility/binary.hpp>
17 
18 BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
19 
20 using namespace Eigen;
21 using namespace pinocchio;
22 
23 BOOST_AUTO_TEST_CASE ( test_FD_with_contact_cst_gamma )
24 {
25  pinocchio::Model model;
26  pinocchio::buildModels::humanoidRandom(model,true);
27  pinocchio::Data data(model), data_check(model);
28 
29  VectorXd q = VectorXd::Ones(model.nq);
30  normalize(model,q);
31 
32  computeJointJacobians(model, data, q);
33 
34  VectorXd v = VectorXd::Ones(model.nv);
35  VectorXd tau = VectorXd::Random(model.nv);
36 
37  const std::string RF = "rleg6_joint";
38  const Model::JointIndex RF_id = model.getJointId(RF);
39 // const std::string LF = "lleg6_joint";
40 // const Model::JointIndex LF_id = model.getJointId(LF);
41 
42  Data::Matrix6x J_RF(6,model.nv); J_RF.setZero();
43  getJointJacobian(model, data, RF_id, LOCAL, J_RF);
44  Motion::Vector6 gamma_RF; gamma_RF.setZero();
45  forwardKinematics(model,data,q,v,VectorXd::Zero(model.nv));
46  gamma_RF += data.a[RF_id].toVector(); // Jdot * qdot
47 
48  forwardDynamics(model, data, q, v, tau, J_RF, gamma_RF);
49  VectorXd ddq_ref = data.ddq;
50  Force::Vector6 contact_force_ref = data.lambda_c;
51 
52  PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext((size_t)model.njoints,Force::Zero());
53  fext[RF_id] = ForceRef<Force::Vector6>(contact_force_ref);
54 
55  // check call to RNEA
56  rnea(model,data_check,q,v,ddq_ref,fext);
57 
58  BOOST_CHECK(data_check.tau.isApprox(tau));
59  forwardKinematics(model,data_check,q,VectorXd::Zero(model.nv),ddq_ref);
60  BOOST_CHECK(data_check.a[RF_id].toVector().isApprox(-gamma_RF));
61 
62  Data data_fd(model);
63  VectorXd q_plus(model.nq);
64  VectorXd v_eps(model.nv); v_eps.setZero();
65  VectorXd v_plus(v);
66  VectorXd tau_plus(tau);
67  const double eps = 1e-8;
68 
69  // check: dddq_dtau and dlambda_dtau
70  MatrixXd dddq_dtau(model.nv,model.nv);
71  Data::Matrix6x dlambda_dtau(6,model.nv);
72 
73  for(int k = 0; k < model.nv; ++k)
74  {
75  tau_plus[k] += eps;
76  forwardDynamics(model, data_fd, q, v, tau_plus, J_RF, gamma_RF);
77 
78  const Data::TangentVectorType & ddq_plus = data_fd.ddq;
79  Force::Vector6 contact_force_plus = data_fd.lambda_c;
80 
81  dddq_dtau.col(k) = (ddq_plus - ddq_ref)/eps;
82  dlambda_dtau.col(k) = (contact_force_plus - contact_force_ref)/eps;
83 
84  tau_plus[k] -= eps;
85  }
86 
87  MatrixXd A(model.nv+6,model.nv+6);
88  data.M.transpose().triangularView<Eigen::Upper>() = data.M.triangularView<Eigen::Upper>();
89  A.topLeftCorner(model.nv,model.nv) = data.M;
90  A.bottomLeftCorner(6, model.nv) = J_RF;
91  A.topRightCorner(model.nv, 6) = J_RF.transpose();
92  A.bottomRightCorner(6,6).setZero();
93 
94  MatrixXd Ainv = A.inverse();
95  BOOST_CHECK(Ainv.topRows(model.nv).leftCols(model.nv).isApprox(dddq_dtau,std::sqrt(eps)));
96  BOOST_CHECK(Ainv.bottomRows(6).leftCols(model.nv).isApprox(-dlambda_dtau,std::sqrt(eps)));
97 
98  // check: dddq_dv and dlambda_dv
99  MatrixXd dddq_dv(model.nv,model.nv);
100  Data::Matrix6x dlambda_dv(6,model.nv);
101 
102  for(int k = 0; k < model.nv; ++k)
103  {
104  v_plus[k] += eps;
105  forwardDynamics(model, data_fd, q, v_plus, tau, J_RF, gamma_RF);
106 
107  const Data::TangentVectorType & ddq_plus = data_fd.ddq;
108  Force::Vector6 contact_force_plus = data_fd.lambda_c;
109 
110  dddq_dv.col(k) = (ddq_plus - ddq_ref)/eps;
111  dlambda_dv.col(k) = (contact_force_plus - contact_force_ref)/eps;
112 
113  v_plus[k] -= eps;
114  }
115 
116  computeRNEADerivatives(model,data_check,q,v,VectorXd::Zero(model.nv));
117  MatrixXd dddq_dv_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dv;
118  MatrixXd dlambda_dv_anal = -Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dv;
119 
120  BOOST_CHECK(dddq_dv_anal.isApprox(dddq_dv,std::sqrt(eps)));
121  BOOST_CHECK(dlambda_dv_anal.isApprox(-dlambda_dv,std::sqrt(eps)));
122 
123  MatrixXd dddq_dq(model.nv,model.nv);
124  Data::Matrix6x dlambda_dq(6,model.nv);
125 
126  for(int k = 0; k < model.nv; ++k)
127  {
128  v_eps[k] = eps;
129  q_plus = integrate(model,q,v_eps);
130  computeJointJacobians(model, data_fd, q_plus);
131  getJointJacobian(model, data_fd, RF_id, LOCAL, J_RF);
132  forwardDynamics(model, data_fd, q_plus, v, tau, J_RF, gamma_RF);
133 
134  const Data::TangentVectorType & ddq_plus = data_fd.ddq;
135  Force::Vector6 contact_force_plus = data_fd.lambda_c;
136 
137  dddq_dq.col(k) = (ddq_plus - ddq_ref)/eps;
138  dlambda_dq.col(k) = (contact_force_plus - contact_force_ref)/eps;
139 
140  v_eps[k] = 0.;
141  }
142 
143  computeRNEADerivatives(model,data_check,q,v,ddq_ref,fext);
144  Data::Matrix6x v_partial_dq(6,model.nv), a_partial_dq(6,model.nv), a_partial_dv(6,model.nv), a_partial_da(6,model.nv);
145  v_partial_dq.setZero(); a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero();
146  Data data_kin(model);
147  computeForwardKinematicsDerivatives(model,data_kin,q,VectorXd::Zero(model.nv),ddq_ref);
148  getJointAccelerationDerivatives(model,data_kin,RF_id,LOCAL,v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);
149 
150  MatrixXd dddq_dq_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dq;
151  dddq_dq_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dq;
152 
153  MatrixXd dlambda_dq_anal = Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dq;
154  dlambda_dq_anal += Ainv.bottomRows(6).rightCols(6) * a_partial_dq;
155 
156  BOOST_CHECK(dddq_dq_anal.isApprox(dddq_dq,std::sqrt(eps)));
157  BOOST_CHECK(dlambda_dq_anal.isApprox(dlambda_dq,std::sqrt(eps)));
158 
159 }
160 
161 template<typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
162 VectorXd contactDynamics(const Model & model, Data & data,
163  const Eigen::MatrixBase<ConfigVectorType> & q,
164  const Eigen::MatrixBase<TangentVectorType1> & v,
165  const Eigen::MatrixBase<TangentVectorType2> & tau,
166  const Model::JointIndex id)
167 {
168  computeJointJacobians(model, data, q);
169  Data::Matrix6x J(6,model.nv); J.setZero();
170 
171  getJointJacobian(model, data, id, LOCAL, J);
172  Motion::Vector6 gamma;
173  forwardKinematics(model, data, q, v, VectorXd::Zero(model.nv));
174  gamma = data.a[id].toVector();
175 
176  forwardDynamics(model, data, q, v, tau, J, gamma);
177  VectorXd res(VectorXd::Zero(model.nv+6));
178 
179  res.head(model.nv) = data.ddq;
180  res.tail(6) = data.lambda_c;
181 
182  return res;
183 }
184 
185 BOOST_AUTO_TEST_CASE ( test_FD_with_contact_varying_gamma )
186 {
187  pinocchio::Model model;
188  pinocchio::buildModels::humanoidRandom(model,true);
189  pinocchio::Data data(model), data_check(model);
190 
191  VectorXd q = VectorXd::Ones(model.nq);
192  normalize(model,q);
193 
194  VectorXd v = VectorXd::Random(model.nv);
195  VectorXd tau = VectorXd::Random(model.nv);
196 
197  const std::string RF = "rleg6_joint";
198  const Model::JointIndex RF_id = model.getJointId(RF);
199 
200  Data::Matrix6x J_RF(6,model.nv); J_RF.setZero();
201  computeJointJacobians(model, data, q);
202  getJointJacobian(model, data, RF_id, LOCAL, J_RF);
203  Motion::Vector6 gamma_RF; gamma_RF.setZero();
204 
205  VectorXd x_ref = contactDynamics(model,data,q,v,tau,RF_id);
206  VectorXd ddq_ref = x_ref.head(model.nv);
207  Force::Vector6 contact_force_ref = x_ref.tail(6);
208 
209  PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext((size_t)model.njoints,Force::Zero());
210  fext[RF_id] = ForceRef<Force::Vector6>(contact_force_ref);
211 
212  // check call to RNEA
213  rnea(model,data_check,q,v,ddq_ref,fext);
214 
215  BOOST_CHECK(data_check.tau.isApprox(tau));
216  forwardKinematics(model,data_check,q,v,ddq_ref);
217  BOOST_CHECK(data_check.a[RF_id].toVector().isZero());
218 
219  Data data_fd(model);
220  VectorXd q_plus(model.nq);
221  VectorXd v_eps(model.nv); v_eps.setZero();
222  VectorXd v_plus(v);
223  VectorXd tau_plus(tau);
224  VectorXd x_plus(model.nv + 6);
225  const double eps = 1e-8;
226 
227  // check: dddq_dtau and dlambda_dtau
228  MatrixXd dddq_dtau(model.nv,model.nv);
229  Data::Matrix6x dlambda_dtau(6,model.nv);
230 
231  for(int k = 0; k < model.nv; ++k)
232  {
233  tau_plus[k] += eps;
234  x_plus = contactDynamics(model,data,q,v,tau_plus,RF_id);
235 
236  const Data::TangentVectorType ddq_plus = x_plus.head(model.nv);
237  Force::Vector6 contact_force_plus = x_plus.tail(6);
238 
239  dddq_dtau.col(k) = (ddq_plus - ddq_ref)/eps;
240  dlambda_dtau.col(k) = (contact_force_plus - contact_force_ref)/eps;
241 
242  tau_plus[k] -= eps;
243  }
244 
245  MatrixXd A(model.nv+6,model.nv+6);
246  data.M.transpose().triangularView<Eigen::Upper>() = data.M.triangularView<Eigen::Upper>();
247  A.topLeftCorner(model.nv,model.nv) = data.M;
248  A.bottomLeftCorner(6, model.nv) = J_RF;
249  A.topRightCorner(model.nv, 6) = J_RF.transpose();
250  A.bottomRightCorner(6,6).setZero();
251 
252  MatrixXd Ainv = A.inverse();
253  BOOST_CHECK(Ainv.topRows(model.nv).leftCols(model.nv).isApprox(dddq_dtau,std::sqrt(eps)));
254  BOOST_CHECK(Ainv.bottomRows(6).leftCols(model.nv).isApprox(-dlambda_dtau,std::sqrt(eps)));
255 
256  // check: dddq_dv and dlambda_dv
257  MatrixXd dddq_dv(model.nv,model.nv);
258  Data::Matrix6x dlambda_dv(6,model.nv);
259 
260  for(int k = 0; k < model.nv; ++k)
261  {
262  v_plus[k] += eps;
263  x_plus = contactDynamics(model,data,q,v_plus,tau,RF_id);
264 
265  const Data::TangentVectorType ddq_plus = x_plus.head(model.nv);
266  Force::Vector6 contact_force_plus = x_plus.tail(6);
267 
268  dddq_dv.col(k) = (ddq_plus - ddq_ref)/eps;
269  dlambda_dv.col(k) = (contact_force_plus - contact_force_ref)/eps;
270 
271  v_plus[k] -= eps;
272  }
273 
274 
275  computeRNEADerivatives(model,data_check,q,v,VectorXd::Zero(model.nv));
276  Data::Matrix6x v_partial_dq(6,model.nv), a_partial_dq(6,model.nv), a_partial_dv(6,model.nv), a_partial_da(6,model.nv);
277  v_partial_dq.setZero(); a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero();
278  Data data_kin(model);
279  computeForwardKinematicsDerivatives(model,data_kin,q,v,VectorXd::Zero(model.nv));
280  getJointAccelerationDerivatives(model,data_kin,RF_id,LOCAL,v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);
281 
282  MatrixXd dddq_dv_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dv;
283  dddq_dv_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dv;
284  MatrixXd dlambda_dv_anal = -Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dv;
285  dlambda_dv_anal -= Ainv.bottomRows(6).rightCols(6) * a_partial_dv;
286 
287  BOOST_CHECK(dddq_dv_anal.isApprox(dddq_dv,std::sqrt(eps)));
288  BOOST_CHECK(dlambda_dv_anal.isApprox(-dlambda_dv,std::sqrt(eps)));
289 
290 
291  MatrixXd dddq_dq(model.nv,model.nv);
292  Data::Matrix6x dlambda_dq(6,model.nv);
293 
294  for(int k = 0; k < model.nv; ++k)
295  {
296  v_eps[k] = eps;
297  q_plus = integrate(model,q,v_eps);
298 
299  x_plus = contactDynamics(model,data,q_plus,v,tau,RF_id);
300 
301  const Data::TangentVectorType ddq_plus = x_plus.head(model.nv);
302  Force::Vector6 contact_force_plus = x_plus.tail(6);
303 
304  dddq_dq.col(k) = (ddq_plus - ddq_ref)/eps;
305  dlambda_dq.col(k) = (contact_force_plus - contact_force_ref)/eps;
306 
307  v_eps[k] = 0.;
308  }
309 
310  computeRNEADerivatives(model,data_check,q,v,ddq_ref,fext);
311  v_partial_dq.setZero(); a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero();
312  computeForwardKinematicsDerivatives(model,data_kin,q,v,ddq_ref);
313  getJointAccelerationDerivatives(model,data_kin,RF_id,LOCAL,v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);
314 
315  MatrixXd dddq_dq_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dq;
316  dddq_dq_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dq;
317 
318  BOOST_CHECK(dddq_dq_anal.isApprox(dddq_dq,std::sqrt(eps)));
319 
320  MatrixXd dlambda_dq_anal = Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dq;
321  dlambda_dq_anal += Ainv.bottomRows(6).rightCols(6) * a_partial_dq;
322 
323  BOOST_CHECK(dlambda_dq_anal.isApprox(dlambda_dq,std::sqrt(eps)));
324 
325 }
326 
327 BOOST_AUTO_TEST_SUITE_END ()
328 
pinocchio::DataTpl::tau
TangentVectorType tau
Vector of joint torques (dim model.nv).
Definition: src/multibody/data.hpp:128
q
q
pinocchio::DataTpl::dtau_dv
MatrixXs dtau_dv
Partial derivative of the joint torque vector with respect to the joint velocity. ...
Definition: src/multibody/data.hpp:296
pinocchio.deprecated.forwardDynamics
def forwardDynamics(model, data, args)
Definition: deprecated.py:42
pinocchio::rnea
void rnea(const int num_threads, ModelPoolTpl< Scalar, Options, JointCollectionTpl > &pool, const Eigen::MatrixBase< ConfigVectorPool > &q, const Eigen::MatrixBase< TangentVectorPool1 > &v, const Eigen::MatrixBase< TangentVectorPool2 > &a, const Eigen::MatrixBase< TangentVectorPool3 > &tau)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
Definition: parallel/rnea.hpp:32
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:54
dcrba.eps
int eps
Definition: dcrba.py:384
pinocchio::ModelTpl::njoints
int njoints
Number of joints.
Definition: src/multibody/model.hpp:82
pinocchio::id
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain...
v
v
pinocchio::DataTpl::dtau_dq
MatrixXs dtau_dq
Partial derivative of the joint torque vector with respect to the joint configuration.
Definition: src/multibody/data.hpp:293
pinocchio::getJointJacobian
void getJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointIndex jointId, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed either in the world (rf = WORLD) frame or i...
data
data
contactDynamics
VectorXd contactDynamics(const Model &model, Data &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau, const Model::JointIndex id)
Definition: contact-dynamics-derivatives.cpp:162
pinocchio::forwardKinematics
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
pinocchio::LOCAL
The LOCAL frame convention corresponds to the frame directly attached to the moving part (Joint...
Definition: src/multibody/fwd.hpp:47
pinocchio::python::computeRNEADerivatives
bp::tuple computeRNEADerivatives(const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Eigen::VectorXd &a)
Definition: expose-rnea-derivatives.cpp:36
PINOCCHIO_ALIGNED_STD_VECTOR
#define PINOCCHIO_ALIGNED_STD_VECTOR(Type)
Definition: container/aligned-vector.hpp:11
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: src/multibody/model.hpp:49
pinocchio::computeForwardKinematicsDerivatives
void computeForwardKinematicsDerivatives(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)
Computes all the terms required to compute the derivatives of the placement, spatial velocity and acc...
pinocchio::getJointAccelerationDerivatives
void getJointAccelerationDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Model::JointIndex jointId, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xOut1 > &v_partial_dq, const Eigen::MatrixBase< Matrix6xOut2 > &a_partial_dq, const Eigen::MatrixBase< Matrix6xOut3 > &a_partial_dv, const Eigen::MatrixBase< Matrix6xOut4 > &a_partial_da)
Computes the partial derivaties of the spatial acceleration of a given with respect to the joint conf...
pinocchio::DataTpl::ddq
TangentVectorType ddq
The joint accelerations computed from ABA.
Definition: src/multibody/data.hpp:204
pinocchio::DataTpl::lambda_c
VectorXs lambda_c
Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics.
Definition: src/multibody/data.hpp:338
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:30
res
res
pinocchio::computeJointJacobians
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeJointJacobians(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame...
pinocchio::ModelTpl::nv
int nv
Dimension of the velocity vector space.
Definition: src/multibody/model.hpp:79
pinocchio::ModelTpl::getJointId
JointIndex getJointId(const std::string &name) const
Return the index of a joint given by its name.
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:609
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_FD_with_contact_cst_gamma)
Definition: contact-dynamics-derivatives.cpp:23
A
A
pinocchio::buildModels::humanoidRandom
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
pinocchio::python::VectorXd
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: src/multibody/data.hpp:71
pinocchio::ModelTpl::nq
int nq
Dimension of the configuration vector representation.
Definition: src/multibody/model.hpp:76
pinocchio::DataTpl::TangentVectorType
VectorXs TangentVectorType
Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc). It also handles the notion of co-tangent vector (e.g. torque, etc).
Definition: src/multibody/data.hpp:68

pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:02