unittest/contact-dynamics.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2016-2020 CNRS INRIA
3 //
4 
5 #include "pinocchio/spatial/se3.hpp"
6 #include "pinocchio/multibody/model.hpp"
7 #include "pinocchio/multibody/data.hpp"
8 #include "pinocchio/algorithm/jacobian.hpp"
9 #include "pinocchio/algorithm/contact-dynamics.hpp"
10 #include "pinocchio/algorithm/joint-configuration.hpp"
11 #include "pinocchio/multibody/sample-models.hpp"
12 #include "pinocchio/utils/timer.hpp"
13 
14 #include <iostream>
15 
16 #include <boost/test/unit_test.hpp>
17 #include <boost/utility/binary.hpp>
18 
19 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
20 
21 BOOST_AUTO_TEST_CASE(test_FD)
22 {
23  using namespace Eigen;
24  using namespace pinocchio;
25 
26  pinocchio::Model model;
27  pinocchio::buildModels::humanoidRandom(model, true);
28  pinocchio::Data data(model);
29 
30  VectorXd q = VectorXd::Ones(model.nq);
31  q.segment<4>(3).normalize();
32 
33  pinocchio::computeJointJacobians(model, data, q);
34 
35  VectorXd v = VectorXd::Ones(model.nv);
36  VectorXd tau = VectorXd::Zero(model.nv);
37 
38  const std::string RF = "rleg6_joint";
39  const std::string LF = "lleg6_joint";
40 
41  Data::Matrix6x J_RF(6, model.nv);
42  J_RF.setZero();
43  getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
44  Data::Matrix6x J_LF(6, model.nv);
45  J_LF.setZero();
46  getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);
47 
48  Eigen::MatrixXd J(12, model.nv);
49  J.setZero();
50  J.topRows<6>() = J_RF;
51  J.bottomRows<6>() = J_LF;
52 
53  Eigen::VectorXd gamma(VectorXd::Ones(12));
54 
55  Eigen::MatrixXd H(J.transpose());
56 
57  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
58  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
59  pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.);
60  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
61 
62  data.M.triangularView<Eigen::StrictlyLower>() =
63  data.M.transpose().triangularView<Eigen::StrictlyLower>();
64 
65  MatrixXd Minv(data.M.inverse());
66  MatrixXd JMinvJt(J * Minv * J.transpose());
67 
68  Eigen::MatrixXd G_ref(J.transpose());
69  cholesky::Uiv(model, data, G_ref);
70  for (int k = 0; k < model.nv; ++k)
71  G_ref.row(k) /= sqrt(data.D[k]);
72  Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
73  BOOST_CHECK(H_ref.isApprox(JMinvJt, 1e-12));
74 
75  VectorXd lambda_ref = -JMinvJt.inverse() * (J * Minv * (tau - data.nle) + gamma);
76  BOOST_CHECK(data.lambda_c.isApprox(lambda_ref, 1e-12));
77 
78  VectorXd a_ref = Minv * (tau - data.nle + J.transpose() * lambda_ref);
79 
80  Eigen::VectorXd dynamics_residual_ref(
81  data.M * a_ref + data.nle - tau - J.transpose() * lambda_ref);
82  BOOST_CHECK(
83  dynamics_residual_ref.norm()
84  <= 1e-11); // previously 1e-12, may be due to numerical approximations, i obtain 2.03e-12
85 
86  Eigen::VectorXd constraint_residual(J * data.ddq + gamma);
87  BOOST_CHECK(constraint_residual.norm() <= 1e-12);
88 
89  Eigen::VectorXd dynamics_residual(
90  data.M * data.ddq + data.nle - tau - J.transpose() * data.lambda_c);
91  BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
92 }
93 
94 BOOST_AUTO_TEST_CASE(test_computeKKTMatrix)
95 {
96  using namespace Eigen;
97  using namespace pinocchio;
98  pinocchio::Model model;
99  pinocchio::buildModels::humanoidRandom(model, true);
100  pinocchio::Data data(model), data_ref(model);
101 
102  VectorXd q = VectorXd::Ones(model.nq);
103  q.segment<4>(3).normalize();
104 
105  pinocchio::computeJointJacobians(model, data_ref, q);
106 
107  const std::string RF = "rleg6_joint";
108  const std::string LF = "lleg6_joint";
109 
110  Data::Matrix6x J_RF(6, model.nv);
111  J_RF.setZero();
112  getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_RF);
113  Data::Matrix6x J_LF(6, model.nv);
114  J_LF.setZero();
115  getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_LF);
116 
117  Eigen::MatrixXd J(12, model.nv);
118  J.setZero();
119  J.topRows<6>() = J_RF;
120  J.bottomRows<6>() = J_LF;
121 
122  // Check Forward Dynamics
123  pinocchio::crba(model, data_ref, q, pinocchio::Convention::WORLD);
124  data_ref.M.triangularView<Eigen::StrictlyLower>() =
125  data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
126 
127  Eigen::MatrixXd MJtJ(model.nv + 12, model.nv + 12);
128  MJtJ << data_ref.M, J.transpose(), J, Eigen::MatrixXd::Zero(12, 12);
129 
130  Eigen::MatrixXd KKTMatrix_inv(model.nv + 12, model.nv + 12);
131  computeKKTContactDynamicMatrixInverse(model, data, q, J, KKTMatrix_inv);
132 
133  BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));
134 }
135 
136 BOOST_AUTO_TEST_CASE(test_getKKTMatrix)
137 {
138  using namespace Eigen;
139  using namespace pinocchio;
140  pinocchio::Model model;
141  pinocchio::buildModels::humanoidRandom(model, true);
142  pinocchio::Data data(model);
143 
144  VectorXd q = VectorXd::Ones(model.nq);
145  q.segment<4>(3).normalize();
146 
147  pinocchio::computeJointJacobians(model, data, q);
148 
149  VectorXd v = VectorXd::Ones(model.nv);
150  VectorXd tau = VectorXd::Zero(model.nv);
151 
152  const std::string RF = "rleg6_joint";
153  const std::string LF = "lleg6_joint";
154 
155  Data::Matrix6x J_RF(6, model.nv);
156  J_RF.setZero();
157  getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
158  Data::Matrix6x J_LF(6, model.nv);
159  J_LF.setZero();
160  getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);
161 
162  Eigen::MatrixXd J(12, model.nv);
163  J.setZero();
164  J.topRows<6>() = J_RF;
165  J.bottomRows<6>() = J_LF;
166 
167  Eigen::VectorXd gamma(VectorXd::Ones(12));
168 
169  Eigen::MatrixXd H(J.transpose());
170 
171  // Check Forward Dynamics
172  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
173  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
174  pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.);
175  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
176 
177  data.M.triangularView<Eigen::StrictlyLower>() =
178  data.M.transpose().triangularView<Eigen::StrictlyLower>();
179 
180  Eigen::MatrixXd MJtJ(model.nv + 12, model.nv + 12);
181  MJtJ << data.M, J.transpose(), J, Eigen::MatrixXd::Zero(12, 12);
182 
183  Eigen::MatrixXd KKTMatrix_inv(model.nv + 12, model.nv + 12);
184 
185  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
186  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
187  getKKTContactDynamicMatrixInverse(model, data, J, KKTMatrix_inv);
188  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
189 
190  BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));
191 
192  // Check Impulse Dynamics
193  const double r_coeff = 1.;
194  VectorXd v_before = VectorXd::Ones(model.nv);
195 
196  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
197  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
198  pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, 0.);
199  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
200 
201  data.M.triangularView<Eigen::StrictlyLower>() =
202  data.M.transpose().triangularView<Eigen::StrictlyLower>();
203  MJtJ << data.M, J.transpose(), J, Eigen::MatrixXd::Zero(12, 12);
204 
205  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
206  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
207  getKKTContactDynamicMatrixInverse(model, data, J, KKTMatrix_inv);
208  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
209 
210  BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));
211 }
212 
213 BOOST_AUTO_TEST_CASE(test_FD_with_damping)
214 {
215  using namespace Eigen;
216  using namespace pinocchio;
217 
218  pinocchio::Model model;
219  pinocchio::buildModels::humanoidRandom(model, true);
220  pinocchio::Data data(model);
221 
222  VectorXd q = VectorXd::Ones(model.nq);
223  q.segment<4>(3).normalize();
224 
225  pinocchio::computeJointJacobians(model, data, q);
226 
227  VectorXd v = VectorXd::Ones(model.nv);
228  VectorXd tau = VectorXd::Zero(model.nv);
229 
230  const std::string RF = "rleg6_joint";
231 
232  Data::Matrix6x J_RF(6, model.nv);
233  J_RF.setZero();
234  getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
235 
236  Eigen::MatrixXd J(12, model.nv);
237  J.setZero();
238  J.topRows<6>() = J_RF;
239  J.bottomRows<6>() = J_RF;
240 
241  Eigen::VectorXd gamma(VectorXd::Ones(12));
242 
243  // Forward Dynamics with damping
244  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
245  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
246  pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 1e-12);
247  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
248 
249  // Matrix Definitions
250  Eigen::MatrixXd H(J.transpose());
251  data.M.triangularView<Eigen::StrictlyLower>() =
252  data.M.transpose().triangularView<Eigen::StrictlyLower>();
253 
254  MatrixXd Minv(data.M.inverse());
255  MatrixXd JMinvJt(J * Minv * J.transpose());
256 
257  // Check that JMinvJt is correctly formed
258  Eigen::MatrixXd G_ref(J.transpose());
259  cholesky::Uiv(model, data, G_ref);
260  for (int k = 0; k < model.nv; ++k)
261  G_ref.row(k) /= sqrt(data.D[k]);
262  Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
263  BOOST_CHECK(H_ref.isApprox(JMinvJt, 1e-12));
264 
265  // Actual Residuals
266  Eigen::VectorXd constraint_residual(J * data.ddq + gamma);
267  Eigen::VectorXd dynamics_residual(
268  data.M * data.ddq + data.nle - tau - J.transpose() * data.lambda_c);
269  BOOST_CHECK(constraint_residual.norm() <= 1e-9);
270  BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
271 }
272 
273 BOOST_AUTO_TEST_CASE(test_ID)
274 {
275  using namespace Eigen;
276  using namespace pinocchio;
277 
278  pinocchio::Model model;
279  pinocchio::buildModels::humanoidRandom(model, true);
280  pinocchio::Data data(model);
281 
282  VectorXd q = VectorXd::Ones(model.nq);
283  q.segment<4>(3).normalize();
284 
285  pinocchio::computeJointJacobians(model, data, q);
286 
287  VectorXd v_before = VectorXd::Ones(model.nv);
288 
289  const std::string RF = "rleg6_joint";
290  const std::string LF = "lleg6_joint";
291 
292  Data::Matrix6x J_RF(6, model.nv);
293  J_RF.setZero();
294  getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
295  Data::Matrix6x J_LF(6, model.nv);
296  J_LF.setZero();
297  getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);
298 
299  Eigen::MatrixXd J(12, model.nv);
300  J.setZero();
301  J.topRows<6>() = J_RF;
302  J.bottomRows<6>() = J_LF;
303 
304  const double r_coeff = 1.;
305 
306  Eigen::MatrixXd H(J.transpose());
307 
308  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
309  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
310  pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, 0.);
311  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
312 
313  data.M.triangularView<Eigen::StrictlyLower>() =
314  data.M.transpose().triangularView<Eigen::StrictlyLower>();
315 
316  MatrixXd Minv(data.M.inverse());
317  MatrixXd JMinvJt(J * Minv * J.transpose());
318 
319  Eigen::MatrixXd G_ref(J.transpose());
320  cholesky::Uiv(model, data, G_ref);
321  for (int k = 0; k < model.nv; ++k)
322  G_ref.row(k) /= sqrt(data.D[k]);
323  Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
324  BOOST_CHECK(H_ref.isApprox(JMinvJt, 1e-12));
325 
326  VectorXd lambda_ref = JMinvJt.inverse() * (-r_coeff * J * v_before - J * v_before);
327  BOOST_CHECK(data.impulse_c.isApprox(lambda_ref, 1e-12));
328 
329  VectorXd v_after_ref = Minv * (data.M * v_before + J.transpose() * lambda_ref);
330 
331  Eigen::VectorXd constraint_residual(J * data.dq_after + r_coeff * J * v_before);
332  BOOST_CHECK(constraint_residual.norm() <= 1e-12);
333 
334  Eigen::VectorXd dynamics_residual(
335  data.M * data.dq_after - data.M * v_before - J.transpose() * data.impulse_c);
336  BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
337 }
338 
339 BOOST_AUTO_TEST_CASE(timings_fd_llt)
340 {
341  using namespace Eigen;
342  using namespace pinocchio;
343 
344  pinocchio::Model model;
345  pinocchio::buildModels::humanoidRandom(model, true);
346  pinocchio::Data data(model);
347 
348 #ifdef NDEBUG
349  #ifdef _INTENSE_TESTING_
350  const size_t NBT = 1000 * 1000;
351  #else
352  const size_t NBT = 100;
353  #endif
354 
355 #else
356  const size_t NBT = 1;
357  std::cout << "(the time score in debug mode is not relevant) ";
358 #endif // ifndef NDEBUG
359 
360  VectorXd q = VectorXd::Ones(model.nq);
361  q.segment<4>(3).normalize();
362 
363  pinocchio::computeJointJacobians(model, data, q);
364 
365  VectorXd v = VectorXd::Ones(model.nv);
366  VectorXd tau = VectorXd::Zero(model.nv);
367 
368  const std::string RF = "rleg6_joint";
369  const std::string LF = "lleg6_joint";
370 
371  Data::Matrix6x J_RF(6, model.nv);
372  J_RF.setZero();
373  getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
374  Data::Matrix6x J_LF(6, model.nv);
375  J_LF.setZero();
376  getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);
377 
378  Eigen::MatrixXd J(12, model.nv);
379  J.topRows<6>() = J_RF;
380  J.bottomRows<6>() = J_LF;
381 
382  Eigen::VectorXd gamma(VectorXd::Ones(12));
383 
384  model.lowerPositionLimit.head<7>().fill(-1.);
385  model.upperPositionLimit.head<7>().fill(1.);
386 
387  q = pinocchio::randomConfiguration(model);
388 
389  PinocchioTicToc timer(PinocchioTicToc::US);
390  timer.tic();
391  SMOOTH(NBT)
392  {
393  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
394  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
395  pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.);
396  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
397  }
398  timer.toc(std::cout, NBT);
399 }
400 
401 BOOST_AUTO_TEST_SUITE_END()
pinocchio::DataTpl::M
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
Definition: multibody/data.hpp:199
PinocchioTicToc::toc
double toc()
Definition: timer.hpp:88
pinocchio::getKKTContactDynamicMatrixInverse
PINOCCHIO_DEPRECATED void getKKTContactDynamicMatrixInverse(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Eigen::MatrixBase< KKTMatrixType > &KKTMatrix_inv)
Computes the inverse of the KKT matrix for dynamics with contact constraints.
Eigen
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::Convention::WORLD
@ WORLD
model.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:130
inverse-kinematics-3d.J
J
Definition: inverse-kinematics-3d.py:28
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::crba
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & crba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Convention convention=Convention::LOCAL)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
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::computeKKTContactDynamicMatrixInverse
void computeKKTContactDynamicMatrixInverse(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Eigen::MatrixBase< KKTMatrixType > &KKTMatrix_inv, const Scalar &inv_damping=0.)
Computes the inverse of the KKT matrix for dynamics with contact constraints. It computes the followi...
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
bindings_dynamics.r_coeff
float r_coeff
Definition: bindings_dynamics.py:10
timer.hpp
anymal-simulation.model
model
Definition: anymal-simulation.py:8
forward-dynamics-derivatives.tau
tau
Definition: forward-dynamics-derivatives.py:25
SMOOTH
#define SMOOTH(s)
Definition: timer.hpp:38
joint-configuration.hpp
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...
se3.hpp
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...
cartpole.v
v
Definition: cartpole.py:153
data.hpp
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
#define PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
Definition: include/pinocchio/macros.hpp:131
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
#define PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
macros for pragma push/pop/ignore deprecated warnings
Definition: include/pinocchio/macros.hpp:129
pinocchio::cholesky::Uiv
Mat & Uiv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &v)
Perform the pivot inversion using the Cholesky decomposition stored in data and acting in place.
q
q
PinocchioTicToc::US
@ US
Definition: timer.hpp:53
dcrba.H
def H
Definition: dcrba.py:425
fill
fill
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: multibody/data.hpp:92
pinocchio::impulseDynamics
const PINOCCHIO_DEPRECATED DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & impulseDynamics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v_before, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Scalar r_coeff=0., const Scalar inv_damping=0.)
Compute the impulse dynamics with contact constraints. Internally, pinocchio::crba is called.
contact-dynamics.hpp
sample-models.hpp
jacobian.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
PinocchioTicToc
Definition: timer.hpp:47
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_FD)
Definition: unittest/contact-dynamics.cpp:21
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:887
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
PinocchioTicToc::tic
void tic()
Definition: timer.hpp:82
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

pinocchio
Author(s):
autogenerated on Thu Dec 19 2024 03:41:28