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/parsers/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::forwardDynamics(model, data, q, v, tau, J, gamma, 0.);
58  data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
59 
60  MatrixXd Minv (data.M.inverse());
61  MatrixXd JMinvJt (J * Minv * J.transpose());
62 
63  Eigen::MatrixXd G_ref(J.transpose());
64  cholesky::Uiv(model, data, G_ref);
65  for(int k=0;k<model.nv;++k) G_ref.row(k) /= sqrt(data.D[k]);
66  Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
67  BOOST_CHECK(H_ref.isApprox(JMinvJt,1e-12));
68 
69  VectorXd lambda_ref = -JMinvJt.inverse() * (J*Minv*(tau - data.nle) + gamma);
70  BOOST_CHECK(data.lambda_c.isApprox(lambda_ref, 1e-12));
71 
72  VectorXd a_ref = Minv*(tau - data.nle + J.transpose()*lambda_ref);
73 
74  Eigen::VectorXd dynamics_residual_ref (data.M * a_ref + data.nle - tau - J.transpose()*lambda_ref);
75  BOOST_CHECK(dynamics_residual_ref.norm() <= 1e-11); // previously 1e-12, may be due to numerical approximations, i obtain 2.03e-12
76 
77  Eigen::VectorXd constraint_residual (J * data.ddq + gamma);
78  BOOST_CHECK(constraint_residual.norm() <= 1e-12);
79 
80  Eigen::VectorXd dynamics_residual (data.M * data.ddq + data.nle - tau - J.transpose()*data.lambda_c);
81  BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
82 
83 }
84 
85 BOOST_AUTO_TEST_CASE(test_computeKKTMatrix)
86 {
87  using namespace Eigen;
88  using namespace pinocchio;
89  pinocchio::Model model;
90  pinocchio::buildModels::humanoidRandom(model,true);
91  pinocchio::Data data(model), data_ref(model);
92 
93  VectorXd q = VectorXd::Ones(model.nq);
94  q.segment<4>(3).normalize();
95 
96  pinocchio::computeJointJacobians(model, data_ref, q);
97 
98  const std::string RF = "rleg6_joint";
99  const std::string LF = "lleg6_joint";
100 
101  Data::Matrix6x J_RF(6, model.nv);
102  J_RF.setZero();
103  getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_RF);
104  Data::Matrix6x J_LF(6, model.nv);
105  J_LF.setZero();
106  getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_LF);
107 
108  Eigen::MatrixXd J(12, model.nv);
109  J.setZero();
110  J.topRows<6>() = J_RF;
111  J.bottomRows<6>() = J_LF;
112 
113  //Check Forward Dynamics
114  pinocchio::crba(model,data_ref,q);
115  data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
116 
117  Eigen::MatrixXd MJtJ(model.nv+12, model.nv+12);
118  MJtJ << data_ref.M, J.transpose(),
119  J, Eigen::MatrixXd::Zero(12, 12);
120 
121  Eigen::MatrixXd KKTMatrix_inv(model.nv+12, model.nv+12);
122  computeKKTContactDynamicMatrixInverse(model, data, q, J, KKTMatrix_inv);
123 
124  BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));
125 }
126 
127 BOOST_AUTO_TEST_CASE(test_getKKTMatrix)
128 {
129  using namespace Eigen;
130  using namespace pinocchio;
131  pinocchio::Model model;
132  pinocchio::buildModels::humanoidRandom(model,true);
133  pinocchio::Data data(model);
134 
135  VectorXd q = VectorXd::Ones(model.nq);
136  q.segment <4> (3).normalize();
137 
138  pinocchio::computeJointJacobians(model, data, q);
139 
140  VectorXd v = VectorXd::Ones(model.nv);
141  VectorXd tau = VectorXd::Zero(model.nv);
142 
143  const std::string RF = "rleg6_joint";
144  const std::string LF = "lleg6_joint";
145 
146  Data::Matrix6x J_RF (6, model.nv);
147  J_RF.setZero();
148  getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
149  Data::Matrix6x J_LF (6, model.nv);
150  J_LF.setZero();
151  getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);
152 
153  Eigen::MatrixXd J (12, model.nv);
154  J.setZero();
155  J.topRows<6> () = J_RF;
156  J.bottomRows<6> () = J_LF;
157 
158  Eigen::VectorXd gamma (VectorXd::Ones(12));
159 
160  Eigen::MatrixXd H(J.transpose());
161 
162  //Check Forward Dynamics
163  pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.);
164  data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
165 
166  Eigen::MatrixXd MJtJ(model.nv+12, model.nv+12);
167  MJtJ << data.M, J.transpose(),
168  J, Eigen::MatrixXd::Zero(12, 12);
169 
170  Eigen::MatrixXd KKTMatrix_inv(model.nv+12, model.nv+12);
171  getKKTContactDynamicMatrixInverse(model, data, J, KKTMatrix_inv);
172 
173  BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));
174 
175  //Check Impulse Dynamics
176  const double r_coeff = 1.;
177  VectorXd v_before = VectorXd::Ones(model.nv);
178  pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, 0.);
179  data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
180  MJtJ << data.M, J.transpose(),
181  J, Eigen::MatrixXd::Zero(12, 12);
182 
183  getKKTContactDynamicMatrixInverse(model, data, J, KKTMatrix_inv);
184 
185  BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));
186 }
187 
188 BOOST_AUTO_TEST_CASE ( test_FD_with_damping )
189 {
190  using namespace Eigen;
191  using namespace pinocchio;
192 
193  pinocchio::Model model;
194  pinocchio::buildModels::humanoidRandom(model,true);
195  pinocchio::Data data(model);
196 
197  VectorXd q = VectorXd::Ones(model.nq);
198  q.segment <4> (3).normalize();
199 
200  pinocchio::computeJointJacobians(model, data, q);
201 
202  VectorXd v = VectorXd::Ones(model.nv);
203  VectorXd tau = VectorXd::Zero(model.nv);
204 
205  const std::string RF = "rleg6_joint";
206 
207  Data::Matrix6x J_RF (6, model.nv);
208  J_RF.setZero();
209  getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
210 
211  Eigen::MatrixXd J (12, model.nv);
212  J.setZero();
213  J.topRows<6> () = J_RF;
214  J.bottomRows<6> () = J_RF;
215 
216  Eigen::VectorXd gamma (VectorXd::Ones(12));
217 
218  // Forward Dynamics with damping
219  pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 1e-12);
220 
221  // Matrix Definitions
222  Eigen::MatrixXd H(J.transpose());
223  data.M.triangularView<Eigen::StrictlyLower>() =
224  data.M.transpose().triangularView<Eigen::StrictlyLower>();
225 
226  MatrixXd Minv (data.M.inverse());
227  MatrixXd JMinvJt (J * Minv * J.transpose());
228 
229  // Check that JMinvJt is correctly formed
230  Eigen::MatrixXd G_ref(J.transpose());
231  cholesky::Uiv(model, data, G_ref);
232  for(int k=0;k<model.nv;++k) G_ref.row(k) /= sqrt(data.D[k]);
233  Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
234  BOOST_CHECK(H_ref.isApprox(JMinvJt,1e-12));
235 
236  // Actual Residuals
237  Eigen::VectorXd constraint_residual (J * data.ddq + gamma);
238  Eigen::VectorXd dynamics_residual (data.M * data.ddq + data.nle - tau - J.transpose()*data.lambda_c);
239  BOOST_CHECK(constraint_residual.norm() <= 1e-9);
240  BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
241 }
242 
243 BOOST_AUTO_TEST_CASE ( test_ID )
244 {
245  using namespace Eigen;
246  using namespace pinocchio;
247 
248  pinocchio::Model model;
249  pinocchio::buildModels::humanoidRandom(model,true);
250  pinocchio::Data data(model);
251 
252  VectorXd q = VectorXd::Ones(model.nq);
253  q.segment <4> (3).normalize();
254 
255  pinocchio::computeJointJacobians(model, data, q);
256 
257  VectorXd v_before = VectorXd::Ones(model.nv);
258 
259  const std::string RF = "rleg6_joint";
260  const std::string LF = "lleg6_joint";
261 
262  Data::Matrix6x J_RF (6, model.nv);
263  J_RF.setZero();
264  getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
265  Data::Matrix6x J_LF (6, model.nv);
266  J_LF.setZero();
267  getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);
268 
269  Eigen::MatrixXd J (12, model.nv);
270  J.setZero();
271  J.topRows<6> () = J_RF;
272  J.bottomRows<6> () = J_LF;
273 
274  const double r_coeff = 1.;
275 
276  Eigen::MatrixXd H(J.transpose());
277 
278  pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, 0.);
279  data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
280 
281  MatrixXd Minv (data.M.inverse());
282  MatrixXd JMinvJt (J * Minv * J.transpose());
283 
284  Eigen::MatrixXd G_ref(J.transpose());
285  cholesky::Uiv(model, data, G_ref);
286  for(int k=0;k<model.nv;++k) G_ref.row(k) /= sqrt(data.D[k]);
287  Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
288  BOOST_CHECK(H_ref.isApprox(JMinvJt,1e-12));
289 
290  VectorXd lambda_ref = JMinvJt.inverse() * (-r_coeff * J * v_before - J * v_before);
291  BOOST_CHECK(data.impulse_c.isApprox(lambda_ref, 1e-12));
292 
293  VectorXd v_after_ref = Minv*(data.M * v_before + J.transpose()*lambda_ref);
294 
295  Eigen::VectorXd constraint_residual (J * data.dq_after + r_coeff * J * v_before);
296  BOOST_CHECK(constraint_residual.norm() <= 1e-12);
297 
298  Eigen::VectorXd dynamics_residual (data.M * data.dq_after - data.M * v_before - J.transpose()*data.impulse_c);
299  BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
300 
301 }
302 
303 BOOST_AUTO_TEST_CASE (timings_fd_llt)
304 {
305  using namespace Eigen;
306  using namespace pinocchio;
307 
308  pinocchio::Model model;
309  pinocchio::buildModels::humanoidRandom(model,true);
310  pinocchio::Data data(model);
311 
312 #ifdef NDEBUG
313 #ifdef _INTENSE_TESTING_
314  const size_t NBT = 1000*1000;
315 #else
316  const size_t NBT = 100;
317 #endif
318 
319 #else
320  const size_t NBT = 1;
321  std::cout << "(the time score in debug mode is not relevant) " ;
322 #endif // ifndef NDEBUG
323 
324  VectorXd q = VectorXd::Ones(model.nq);
325  q.segment <4> (3).normalize();
326 
327  pinocchio::computeJointJacobians(model, data, q);
328 
329  VectorXd v = VectorXd::Ones(model.nv);
330  VectorXd tau = VectorXd::Zero(model.nv);
331 
332  const std::string RF = "rleg6_joint";
333  const std::string LF = "lleg6_joint";
334 
335  Data::Matrix6x J_RF (6, model.nv);
336  J_RF.setZero();
337  getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
338  Data::Matrix6x J_LF (6, model.nv);
339  J_LF.setZero();
340  getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);
341 
342  Eigen::MatrixXd J (12, model.nv);
343  J.topRows<6> () = J_RF;
344  J.bottomRows<6> () = J_LF;
345 
346  Eigen::VectorXd gamma (VectorXd::Ones(12));
347 
348  model.lowerPositionLimit.head<7>().fill(-1.);
349  model.upperPositionLimit.head<7>().fill( 1.);
350 
351  q = pinocchio::randomConfiguration(model);
352 
353  PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
354  SMOOTH(NBT)
355  {
356  pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.);
357  }
358  timer.toc(std::cout,NBT);
359 
360 }
361 
362 BOOST_AUTO_TEST_SUITE_END ()
PinocchioTicToc::toc
double toc()
Definition: timer.hpp:68
pinocchio::getKKTContactDynamicMatrixInverse
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. It computes the followi...
pinocchio::impulseDynamics
const 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...
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)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
q
q
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::ModelTpl::lowerPositionLimit
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
Definition: src/multibody/model.hpp:139
v
v
bindings_dynamics.r_coeff
float r_coeff
Definition: bindings_dynamics.py:11
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:224
pinocchio::DataTpl::D
VectorXs D
Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition. ...
Definition: src/multibody/data.hpp:258
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
PinocchioTicToc::tic
void tic()
Definition: timer.hpp:63
pinocchio::DataTpl::nle
VectorXs nle
Vector of Non Linear Effects (dim model.nv). It corresponds to concatenation of the Coriolis...
Definition: src/multibody/data.hpp:133
pinocchio::LOCAL
The LOCAL frame convention corresponds to the frame directly attached to the moving part (Joint...
Definition: src/multibody/fwd.hpp:47
pinocchio::DataTpl::M
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
Definition: src/multibody/data.hpp:151
SMOOTH
#define SMOOTH(s)
Definition: timer.hpp:38
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
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::DataTpl::dq_after
TangentVectorType dq_after
Generalized velocity after impact.
Definition: src/multibody/data.hpp:347
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::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...
pinocchio::forwardDynamics
const 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::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:609
pinocchio::ModelTpl::upperPositionLimit
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
Definition: src/multibody/model.hpp:142
pinocchio::DataTpl::impulse_c
VectorXs impulse_c
Lagrange Multipliers corresponding to the contact impulses in pinocchio::impulseDynamics.
Definition: src/multibody/data.hpp:350
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_FD)
Definition: contact-dynamics.cpp:21
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
dcrba.H
H
Definition: dcrba.py:358
pinocchio::ModelTpl::nq
int nq
Dimension of the configuration vector representation.
Definition: src/multibody/model.hpp:76
fill
void fill(Eigen::Ref< MatType > mat, const typename MatType::Scalar &value)

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