unittest/constrained-dynamics-derivatives.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019 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/multibody/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);
43  J_RF.setZero();
44  getJointJacobian(model, data, RF_id, LOCAL, J_RF);
45  Motion::Vector6 gamma_RF;
46  gamma_RF.setZero();
47  forwardKinematics(model, data, q, v, VectorXd::Zero(model.nv));
48  gamma_RF += data.a[RF_id].toVector(); // Jdot * qdot
49 
50  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
51  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
52  forwardDynamics(model, data, q, v, tau, J_RF, gamma_RF);
53  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
54 
55  VectorXd ddq_ref = data.ddq;
56  Force::Vector6 contact_force_ref = data.lambda_c;
57 
58  container::aligned_vector<Force> fext((size_t)model.njoints, Force::Zero());
59  fext[RF_id] = ForceRef<Force::Vector6>(contact_force_ref);
60 
61  // check call to RNEA
62  rnea(model, data_check, q, v, ddq_ref, fext);
63 
64  BOOST_CHECK(data_check.tau.isApprox(tau));
65  forwardKinematics(model, data_check, q, VectorXd::Zero(model.nv), ddq_ref);
66  BOOST_CHECK(data_check.a[RF_id].toVector().isApprox(-gamma_RF));
67 
68  Data data_fd(model);
69  VectorXd q_plus(model.nq);
70  VectorXd v_eps(model.nv);
71  v_eps.setZero();
72  VectorXd v_plus(v);
73  VectorXd tau_plus(tau);
74  const double eps = 1e-8;
75 
76  // check: dddq_dtau and dlambda_dtau
77  MatrixXd dddq_dtau(model.nv, model.nv);
78  Data::Matrix6x dlambda_dtau(6, model.nv);
79 
80  for (int k = 0; k < model.nv; ++k)
81  {
82  tau_plus[k] += eps;
83 
84  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
85  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
86  forwardDynamics(model, data_fd, q, v, tau_plus, J_RF, gamma_RF);
87  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
88 
89  const Data::TangentVectorType & ddq_plus = data_fd.ddq;
90  Force::Vector6 contact_force_plus = data_fd.lambda_c;
91 
92  dddq_dtau.col(k) = (ddq_plus - ddq_ref) / eps;
93  dlambda_dtau.col(k) = (contact_force_plus - contact_force_ref) / eps;
94 
95  tau_plus[k] -= eps;
96  }
97 
98  MatrixXd A(model.nv + 6, model.nv + 6);
99  data.M.transpose().triangularView<Eigen::Upper>() = data.M.triangularView<Eigen::Upper>();
100  A.topLeftCorner(model.nv, model.nv) = data.M;
101  A.bottomLeftCorner(6, model.nv) = J_RF;
102  A.topRightCorner(model.nv, 6) = J_RF.transpose();
103  A.bottomRightCorner(6, 6).setZero();
104 
105  MatrixXd Ainv = A.inverse();
106  BOOST_CHECK(Ainv.topRows(model.nv).leftCols(model.nv).isApprox(dddq_dtau, std::sqrt(eps)));
107  BOOST_CHECK(Ainv.bottomRows(6).leftCols(model.nv).isApprox(-dlambda_dtau, std::sqrt(eps)));
108 
109  // check: dddq_dv and dlambda_dv
110  MatrixXd dddq_dv(model.nv, model.nv);
111  Data::Matrix6x dlambda_dv(6, model.nv);
112 
113  for (int k = 0; k < model.nv; ++k)
114  {
115  v_plus[k] += eps;
116 
117  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
118  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
119  forwardDynamics(model, data_fd, q, v_plus, tau, J_RF, gamma_RF);
120  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
121 
122  const Data::TangentVectorType & ddq_plus = data_fd.ddq;
123  Force::Vector6 contact_force_plus = data_fd.lambda_c;
124 
125  dddq_dv.col(k) = (ddq_plus - ddq_ref) / eps;
126  dlambda_dv.col(k) = (contact_force_plus - contact_force_ref) / eps;
127 
128  v_plus[k] -= eps;
129  }
130 
131  computeRNEADerivatives(model, data_check, q, v, VectorXd::Zero(model.nv));
132  MatrixXd dddq_dv_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dv;
133  MatrixXd dlambda_dv_anal = -Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dv;
134 
135  BOOST_CHECK(dddq_dv_anal.isApprox(dddq_dv, std::sqrt(eps)));
136  BOOST_CHECK(dlambda_dv_anal.isApprox(-dlambda_dv, std::sqrt(eps)));
137 
138  MatrixXd dddq_dq(model.nv, model.nv);
139  Data::Matrix6x dlambda_dq(6, model.nv);
140 
141  for (int k = 0; k < model.nv; ++k)
142  {
143  v_eps[k] = eps;
144  q_plus = integrate(model, q, v_eps);
145  computeJointJacobians(model, data_fd, q_plus);
146  getJointJacobian(model, data_fd, RF_id, LOCAL, J_RF);
147 
148  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
149  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
150  forwardDynamics(model, data_fd, q_plus, v, tau, J_RF, gamma_RF);
151  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
152 
153  const Data::TangentVectorType & ddq_plus = data_fd.ddq;
154  Force::Vector6 contact_force_plus = data_fd.lambda_c;
155 
156  dddq_dq.col(k) = (ddq_plus - ddq_ref) / eps;
157  dlambda_dq.col(k) = (contact_force_plus - contact_force_ref) / eps;
158 
159  v_eps[k] = 0.;
160  }
161 
162  computeRNEADerivatives(model, data_check, q, v, ddq_ref, fext);
163  Data::Matrix6x v_partial_dq(6, model.nv), a_partial_dq(6, model.nv), a_partial_dv(6, model.nv),
164  a_partial_da(6, model.nv);
165  v_partial_dq.setZero();
166  a_partial_dq.setZero();
167  a_partial_dv.setZero();
168  a_partial_da.setZero();
169  Data data_kin(model);
170  computeForwardKinematicsDerivatives(model, data_kin, q, VectorXd::Zero(model.nv), ddq_ref);
171  getJointAccelerationDerivatives(
172  model, data_kin, RF_id, LOCAL, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
173 
174  MatrixXd dddq_dq_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dq;
175  dddq_dq_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dq;
176 
177  MatrixXd dlambda_dq_anal = Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dq;
178  dlambda_dq_anal += Ainv.bottomRows(6).rightCols(6) * a_partial_dq;
179 
180  BOOST_CHECK(dddq_dq_anal.isApprox(dddq_dq, std::sqrt(eps)));
181  BOOST_CHECK(dlambda_dq_anal.isApprox(dlambda_dq, std::sqrt(eps)));
182 }
183 
184 template<typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
185 VectorXd constraintDynamics(
186  const Model & model,
187  Data & data,
188  const Eigen::MatrixBase<ConfigVectorType> & q,
189  const Eigen::MatrixBase<TangentVectorType1> & v,
190  const Eigen::MatrixBase<TangentVectorType2> & tau,
191  const Model::JointIndex id)
192 {
193  computeJointJacobians(model, data, q);
194  Data::Matrix6x J(6, model.nv);
195  J.setZero();
196 
197  getJointJacobian(model, data, id, LOCAL, J);
198  Motion::Vector6 gamma;
199  forwardKinematics(model, data, q, v, VectorXd::Zero(model.nv));
200  gamma = data.a[id].toVector();
201 
202  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
203  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
204  forwardDynamics(model, data, q, v, tau, J, gamma);
205  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
206 
207  VectorXd res(VectorXd::Zero(model.nv + 6));
208 
209  res.head(model.nv) = data.ddq;
210  res.tail(6) = data.lambda_c;
211 
212  return res;
213 }
214 
215 BOOST_AUTO_TEST_CASE(test_FD_with_contact_varying_gamma)
216 {
217  pinocchio::Model model;
218  pinocchio::buildModels::humanoidRandom(model, true);
219  pinocchio::Data data(model), data_check(model);
220 
221  VectorXd q = VectorXd::Ones(model.nq);
222  normalize(model, q);
223 
224  VectorXd v = VectorXd::Random(model.nv);
225  VectorXd tau = VectorXd::Random(model.nv);
226 
227  const std::string RF = "rleg6_joint";
228  const Model::JointIndex RF_id = model.getJointId(RF);
229 
230  Data::Matrix6x J_RF(6, model.nv);
231  J_RF.setZero();
232  computeJointJacobians(model, data, q);
233  getJointJacobian(model, data, RF_id, LOCAL, J_RF);
234  Motion::Vector6 gamma_RF;
235  gamma_RF.setZero();
236 
237  VectorXd x_ref = constraintDynamics(model, data, q, v, tau, RF_id);
238  VectorXd ddq_ref = x_ref.head(model.nv);
239  Force::Vector6 contact_force_ref = x_ref.tail(6);
240 
241  container::aligned_vector<Force> fext((size_t)model.njoints, Force::Zero());
242  fext[RF_id] = ForceRef<Force::Vector6>(contact_force_ref);
243 
244  // check call to RNEA
245  rnea(model, data_check, q, v, ddq_ref, fext);
246 
247  BOOST_CHECK(data_check.tau.isApprox(tau));
248  forwardKinematics(model, data_check, q, v, ddq_ref);
249  BOOST_CHECK(data_check.a[RF_id].toVector().isZero());
250 
251  Data data_fd(model);
252  VectorXd q_plus(model.nq);
253  VectorXd v_eps(model.nv);
254  v_eps.setZero();
255  VectorXd v_plus(v);
256  VectorXd tau_plus(tau);
257  VectorXd x_plus(model.nv + 6);
258  const double eps = 1e-8;
259 
260  // check: dddq_dtau and dlambda_dtau
261  MatrixXd dddq_dtau(model.nv, model.nv);
262  Data::Matrix6x dlambda_dtau(6, model.nv);
263 
264  for (int k = 0; k < model.nv; ++k)
265  {
266  tau_plus[k] += eps;
267  x_plus = constraintDynamics(model, data, q, v, tau_plus, RF_id);
268 
269  const Data::TangentVectorType ddq_plus = x_plus.head(model.nv);
270  Force::Vector6 contact_force_plus = x_plus.tail(6);
271 
272  dddq_dtau.col(k) = (ddq_plus - ddq_ref) / eps;
273  dlambda_dtau.col(k) = (contact_force_plus - contact_force_ref) / eps;
274 
275  tau_plus[k] -= eps;
276  }
277 
278  MatrixXd A(model.nv + 6, model.nv + 6);
279  data.M.transpose().triangularView<Eigen::Upper>() = data.M.triangularView<Eigen::Upper>();
280  A.topLeftCorner(model.nv, model.nv) = data.M;
281  A.bottomLeftCorner(6, model.nv) = J_RF;
282  A.topRightCorner(model.nv, 6) = J_RF.transpose();
283  A.bottomRightCorner(6, 6).setZero();
284 
285  MatrixXd Ainv = A.inverse();
286  BOOST_CHECK(Ainv.topRows(model.nv).leftCols(model.nv).isApprox(dddq_dtau, std::sqrt(eps)));
287  BOOST_CHECK(Ainv.bottomRows(6).leftCols(model.nv).isApprox(-dlambda_dtau, std::sqrt(eps)));
288 
289  // check: dddq_dv and dlambda_dv
290  MatrixXd dddq_dv(model.nv, model.nv);
291  Data::Matrix6x dlambda_dv(6, model.nv);
292 
293  for (int k = 0; k < model.nv; ++k)
294  {
295  v_plus[k] += eps;
296  x_plus = constraintDynamics(model, data, q, v_plus, tau, RF_id);
297 
298  const Data::TangentVectorType ddq_plus = x_plus.head(model.nv);
299  Force::Vector6 contact_force_plus = x_plus.tail(6);
300 
301  dddq_dv.col(k) = (ddq_plus - ddq_ref) / eps;
302  dlambda_dv.col(k) = (contact_force_plus - contact_force_ref) / eps;
303 
304  v_plus[k] -= eps;
305  }
306 
307  computeRNEADerivatives(model, data_check, q, v, VectorXd::Zero(model.nv));
308  Data::Matrix6x v_partial_dq(6, model.nv), a_partial_dq(6, model.nv), a_partial_dv(6, model.nv),
309  a_partial_da(6, model.nv);
310  v_partial_dq.setZero();
311  a_partial_dq.setZero();
312  a_partial_dv.setZero();
313  a_partial_da.setZero();
314  Data data_kin(model);
315  computeForwardKinematicsDerivatives(model, data_kin, q, v, VectorXd::Zero(model.nv));
316  getJointAccelerationDerivatives(
317  model, data_kin, RF_id, LOCAL, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
318 
319  MatrixXd dddq_dv_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dv;
320  dddq_dv_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dv;
321  MatrixXd dlambda_dv_anal = -Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dv;
322  dlambda_dv_anal -= Ainv.bottomRows(6).rightCols(6) * a_partial_dv;
323 
324  BOOST_CHECK(dddq_dv_anal.isApprox(dddq_dv, std::sqrt(eps)));
325  BOOST_CHECK(dlambda_dv_anal.isApprox(-dlambda_dv, std::sqrt(eps)));
326 
327  MatrixXd dddq_dq(model.nv, model.nv);
328  Data::Matrix6x dlambda_dq(6, model.nv);
329 
330  for (int k = 0; k < model.nv; ++k)
331  {
332  v_eps[k] = eps;
333  q_plus = integrate(model, q, v_eps);
334 
335  x_plus = constraintDynamics(model, data, q_plus, v, tau, RF_id);
336 
337  const Data::TangentVectorType ddq_plus = x_plus.head(model.nv);
338  Force::Vector6 contact_force_plus = x_plus.tail(6);
339 
340  dddq_dq.col(k) = (ddq_plus - ddq_ref) / eps;
341  dlambda_dq.col(k) = (contact_force_plus - contact_force_ref) / eps;
342 
343  v_eps[k] = 0.;
344  }
345 
346  computeRNEADerivatives(model, data_check, q, v, ddq_ref, fext);
347  v_partial_dq.setZero();
348  a_partial_dq.setZero();
349  a_partial_dv.setZero();
350  a_partial_da.setZero();
351  computeForwardKinematicsDerivatives(model, data_kin, q, v, ddq_ref);
352  getJointAccelerationDerivatives(
353  model, data_kin, RF_id, LOCAL, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
354 
355  MatrixXd dddq_dq_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dq;
356  dddq_dq_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dq;
357 
358  BOOST_CHECK(dddq_dq_anal.isApprox(dddq_dq, std::sqrt(eps)));
359 
360  MatrixXd dlambda_dq_anal = Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dq;
361  dlambda_dq_anal += Ainv.bottomRows(6).rightCols(6) * a_partial_dq;
362 
363  BOOST_CHECK(dlambda_dq_anal.isApprox(dlambda_dq, std::sqrt(eps)));
364 }
365 
366 BOOST_AUTO_TEST_SUITE_END()
pinocchio::DataTpl::TangentVectorType
VectorXs TangentVectorType
Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc)....
Definition: multibody/data.hpp:89
pinocchio::DataTpl::ddq
TangentVectorType ddq
The joint accelerations computed from ABA.
Definition: multibody/data.hpp:256
Eigen
pinocchio::DataTpl
Definition: context/generic.hpp:25
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::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.
inverse-kinematics.J
J
Definition: inverse-kinematics.py:31
rnea-derivatives.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
PINOCCHIO_COMPILER_DIAGNOSTIC_POP
#define PINOCCHIO_COMPILER_DIAGNOSTIC_POP
Definition: include/pinocchio/macros.hpp:131
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::DataTpl::lambda_c
VectorXs lambda_c
Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics.
Definition: multibody/data.hpp:471
pinocchio::python::computeRNEADerivatives
bp::tuple computeRNEADerivatives(const context::Model &model, context::Data &data, const context::VectorXs &q, const context::VectorXs &v, const context::VectorXs &a)
Definition: expose-rnea-derivatives.cpp:38
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....
rnea.hpp
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...
anymal-simulation.model
model
Definition: anymal-simulation.py:12
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:68
forward-dynamics-derivatives.tau
tau
Definition: forward-dynamics-derivatives.py:23
res
res
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...
joint-configuration.hpp
pinocchio::DataTpl::dtau_dq
RowMatrixXs dtau_dq
Partial derivative of the joint torque vector with respect to the joint configuration.
Definition: multibody/data.hpp:387
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:72
autodiff-rnea.v
v
Definition: autodiff-rnea.py:15
pinocchio::getJointJacobian
void getJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed in one of the pinocchio::ReferenceFrame opt...
pinocchio::DataTpl::tau
TangentVectorType tau
Vector of joint torques (dim model.nv).
Definition: multibody/data.hpp:173
pinocchio::forwardDynamics
const PINOCCHIO_DEPRECATED DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & forwardDynamics(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< ConstraintMatrixType > &J, const Eigen::MatrixBase< DriftVectorType > &gamma, const Scalar inv_damping=0.)
Compute the forward dynamics with contact constraints. Internally, pinocchio::computeAllTerms is call...
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
#define PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
Definition: include/pinocchio/macros.hpp:132
simulation-contact-dynamics.A
A
Definition: simulation-contact-dynamics.py:115
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
#define PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
macros for pragma push/pop/ignore deprecated warnings
Definition: include/pinocchio/macros.hpp:130
q
q
constraintDynamics
VectorXd constraintDynamics(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: unittest/constrained-dynamics-derivatives.cpp:185
pinocchio::container::aligned_vector
std::vector< T, Eigen::aligned_allocator< T > > aligned_vector
Definition: container/aligned-vector.hpp:21
pinocchio::ForceRef
Definition: force-ref.hpp:53
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...
kinematics-derivatives.hpp
dcrba.eps
int eps
Definition: dcrba.py:439
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: multibody/data.hpp:92
contact-dynamics.hpp
pinocchio::DataTpl::dtau_dv
RowMatrixXs dtau_dv
Partial derivative of the joint torque vector with respect to the joint velocity.
Definition: multibody/data.hpp:390
sample-models.hpp
jacobian.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:914
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_FD_with_contact_cst_gamma)
Definition: unittest/constrained-dynamics-derivatives.cpp:23
BOOST_CHECK
#define BOOST_CHECK(check)
Definition: overview-urdf.cpp:34
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

pinocchio
Author(s):
autogenerated on Sat Jun 22 2024 02:41:45