GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: unittest/contact-dynamics-derivatives.cpp Lines: 196 196 100.0 %
Date: 2024-01-23 21:41:47 Branches: 524 1036 50.6 %

Line Branch Exec Source
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
















4
BOOST_AUTO_TEST_CASE ( test_FD_with_contact_cst_gamma )
24
{
25
4
  pinocchio::Model model;
26
2
  pinocchio::buildModels::humanoidRandom(model,true);
27

4
  pinocchio::Data data(model), data_check(model);
28
29

4
  VectorXd q = VectorXd::Ones(model.nq);
30
2
  normalize(model,q);
31
32
2
  computeJointJacobians(model, data, q);
33
34

4
  VectorXd v = VectorXd::Ones(model.nv);
35

4
  VectorXd tau = VectorXd::Random(model.nv);
36
37
4
  const std::string RF = "rleg6_joint";
38
2
  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

4
  Data::Matrix6x J_RF(6,model.nv); J_RF.setZero();
43
2
  getJointJacobian(model, data, RF_id, LOCAL, J_RF);
44

2
  Motion::Vector6 gamma_RF; gamma_RF.setZero();
45

2
  forwardKinematics(model,data,q,v,VectorXd::Zero(model.nv));
46

2
  gamma_RF += data.a[RF_id].toVector(); // Jdot * qdot
47
48
2
  forwardDynamics(model, data, q, v, tau, J_RF, gamma_RF);
49
4
  VectorXd ddq_ref = data.ddq;
50
2
  Force::Vector6 contact_force_ref = data.lambda_c;
51
52

4
  PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext((size_t)model.njoints,Force::Zero());
53

2
  fext[RF_id] = ForceRef<Force::Vector6>(contact_force_ref);
54
55
  // check call to RNEA
56
2
  rnea(model,data_check,q,v,ddq_ref,fext);
57
58



2
  BOOST_CHECK(data_check.tau.isApprox(tau));
59

2
  forwardKinematics(model,data_check,q,VectorXd::Zero(model.nv),ddq_ref);
60




2
  BOOST_CHECK(data_check.a[RF_id].toVector().isApprox(-gamma_RF));
61
62
4
  Data data_fd(model);
63
4
  VectorXd q_plus(model.nq);
64

4
  VectorXd v_eps(model.nv); v_eps.setZero();
65
4
  VectorXd v_plus(v);
66
4
  VectorXd tau_plus(tau);
67
2
  const double eps = 1e-8;
68
69
  // check: dddq_dtau and dlambda_dtau
70
4
  MatrixXd dddq_dtau(model.nv,model.nv);
71
4
  Data::Matrix6x dlambda_dtau(6,model.nv);
72
73
66
  for(int k = 0; k < model.nv; ++k)
74
  {
75
64
    tau_plus[k] += eps;
76
64
    forwardDynamics(model, data_fd, q, v, tau_plus, J_RF, gamma_RF);
77
78
64
    const Data::TangentVectorType & ddq_plus = data_fd.ddq;
79
64
    Force::Vector6 contact_force_plus = data_fd.lambda_c;
80
81


64
    dddq_dtau.col(k) = (ddq_plus - ddq_ref)/eps;
82


64
    dlambda_dtau.col(k) = (contact_force_plus - contact_force_ref)/eps;
83
84
64
    tau_plus[k] -= eps;
85
  }
86
87
4
  MatrixXd A(model.nv+6,model.nv+6);
88


2
  data.M.transpose().triangularView<Eigen::Upper>() = data.M.triangularView<Eigen::Upper>();
89

2
  A.topLeftCorner(model.nv,model.nv) = data.M;
90

2
  A.bottomLeftCorner(6, model.nv) = J_RF;
91

2
  A.topRightCorner(model.nv, 6) = J_RF.transpose();
92

2
  A.bottomRightCorner(6,6).setZero();
93
94

4
  MatrixXd Ainv = A.inverse();
95




2
  BOOST_CHECK(Ainv.topRows(model.nv).leftCols(model.nv).isApprox(dddq_dtau,std::sqrt(eps)));
96





2
  BOOST_CHECK(Ainv.bottomRows(6).leftCols(model.nv).isApprox(-dlambda_dtau,std::sqrt(eps)));
97
98
  // check: dddq_dv and dlambda_dv
99
4
  MatrixXd dddq_dv(model.nv,model.nv);
100
4
  Data::Matrix6x dlambda_dv(6,model.nv);
101
102
66
  for(int k = 0; k < model.nv; ++k)
103
  {
104
64
    v_plus[k] += eps;
105
64
    forwardDynamics(model, data_fd, q, v_plus, tau, J_RF, gamma_RF);
106
107
64
    const Data::TangentVectorType & ddq_plus = data_fd.ddq;
108
64
    Force::Vector6 contact_force_plus = data_fd.lambda_c;
109
110


64
    dddq_dv.col(k) = (ddq_plus - ddq_ref)/eps;
111


64
    dlambda_dv.col(k) = (contact_force_plus - contact_force_ref)/eps;
112
113
64
    v_plus[k] -= eps;
114
  }
115
116

2
  computeRNEADerivatives(model,data_check,q,v,VectorXd::Zero(model.nv));
117


4
  MatrixXd dddq_dv_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dv;
118


4
  MatrixXd dlambda_dv_anal = -Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dv;
119
120



2
  BOOST_CHECK(dddq_dv_anal.isApprox(dddq_dv,std::sqrt(eps)));
121




2
  BOOST_CHECK(dlambda_dv_anal.isApprox(-dlambda_dv,std::sqrt(eps)));
122
123
4
  MatrixXd dddq_dq(model.nv,model.nv);
124
4
  Data::Matrix6x dlambda_dq(6,model.nv);
125
126
66
  for(int k = 0; k < model.nv; ++k)
127
  {
128
64
    v_eps[k] = eps;
129
64
    q_plus = integrate(model,q,v_eps);
130
64
    computeJointJacobians(model, data_fd, q_plus);
131
64
    getJointJacobian(model, data_fd, RF_id, LOCAL, J_RF);
132
64
    forwardDynamics(model, data_fd, q_plus, v, tau, J_RF, gamma_RF);
133
134
64
    const Data::TangentVectorType & ddq_plus = data_fd.ddq;
135
64
    Force::Vector6 contact_force_plus = data_fd.lambda_c;
136
137


64
    dddq_dq.col(k) = (ddq_plus - ddq_ref)/eps;
138


64
    dlambda_dq.col(k) = (contact_force_plus - contact_force_ref)/eps;
139
140
64
    v_eps[k] = 0.;
141
  }
142
143
2
  computeRNEADerivatives(model,data_check,q,v,ddq_ref,fext);
144


4
  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


2
  v_partial_dq.setZero(); a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero();
146
4
  Data data_kin(model);
147

2
  computeForwardKinematicsDerivatives(model,data_kin,q,VectorXd::Zero(model.nv),ddq_ref);
148
2
  getJointAccelerationDerivatives(model,data_kin,RF_id,LOCAL,v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);
149
150


4
  MatrixXd dddq_dq_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dq;
151


2
  dddq_dq_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dq;
152
153


4
  MatrixXd dlambda_dq_anal = Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dq;
154


2
  dlambda_dq_anal += Ainv.bottomRows(6).rightCols(6) * a_partial_dq;
155
156



2
  BOOST_CHECK(dddq_dq_anal.isApprox(dddq_dq,std::sqrt(eps)));
157



2
  BOOST_CHECK(dlambda_dq_anal.isApprox(dlambda_dq,std::sqrt(eps)));
158
159
2
}
160
161
template<typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
162
97
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
97
  computeJointJacobians(model, data, q);
169

194
  Data::Matrix6x J(6,model.nv); J.setZero();
170
171
97
  getJointJacobian(model, data, id, LOCAL, J);
172
97
  Motion::Vector6 gamma;
173

97
  forwardKinematics(model, data, q, v, VectorXd::Zero(model.nv));
174

97
  gamma = data.a[id].toVector();
175
176
97
  forwardDynamics(model, data, q, v, tau, J, gamma);
177

97
  VectorXd res(VectorXd::Zero(model.nv+6));
178
179

97
  res.head(model.nv) = data.ddq;
180

97
  res.tail(6) = data.lambda_c;
181
182
194
  return res;
183
}
184
185
















4
BOOST_AUTO_TEST_CASE ( test_FD_with_contact_varying_gamma )
186
{
187
4
  pinocchio::Model model;
188
2
  pinocchio::buildModels::humanoidRandom(model,true);
189

4
  pinocchio::Data data(model), data_check(model);
190
191

4
  VectorXd q = VectorXd::Ones(model.nq);
192
2
  normalize(model,q);
193
194

4
  VectorXd v = VectorXd::Random(model.nv);
195

4
  VectorXd tau = VectorXd::Random(model.nv);
196
197
4
  const std::string RF = "rleg6_joint";
198
2
  const Model::JointIndex RF_id = model.getJointId(RF);
199
200

4
  Data::Matrix6x J_RF(6,model.nv); J_RF.setZero();
201
2
  computeJointJacobians(model, data, q);
202
2
  getJointJacobian(model, data, RF_id, LOCAL, J_RF);
203

2
  Motion::Vector6 gamma_RF; gamma_RF.setZero();
204
205
4
  VectorXd x_ref = contactDynamics(model,data,q,v,tau,RF_id);
206

4
  VectorXd ddq_ref = x_ref.head(model.nv);
207

2
  Force::Vector6 contact_force_ref = x_ref.tail(6);
208
209

4
  PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext((size_t)model.njoints,Force::Zero());
210

2
  fext[RF_id] = ForceRef<Force::Vector6>(contact_force_ref);
211
212
  // check call to RNEA
213
2
  rnea(model,data_check,q,v,ddq_ref,fext);
214
215



2
  BOOST_CHECK(data_check.tau.isApprox(tau));
216
2
  forwardKinematics(model,data_check,q,v,ddq_ref);
217




2
  BOOST_CHECK(data_check.a[RF_id].toVector().isZero());
218
219
4
  Data data_fd(model);
220
4
  VectorXd q_plus(model.nq);
221

4
  VectorXd v_eps(model.nv); v_eps.setZero();
222
4
  VectorXd v_plus(v);
223
4
  VectorXd tau_plus(tau);
224
4
  VectorXd x_plus(model.nv + 6);
225
2
  const double eps = 1e-8;
226
227
  // check: dddq_dtau and dlambda_dtau
228
4
  MatrixXd dddq_dtau(model.nv,model.nv);
229
4
  Data::Matrix6x dlambda_dtau(6,model.nv);
230
231
66
  for(int k = 0; k < model.nv; ++k)
232
  {
233
64
    tau_plus[k] += eps;
234
64
    x_plus = contactDynamics(model,data,q,v,tau_plus,RF_id);
235
236

64
    const Data::TangentVectorType ddq_plus = x_plus.head(model.nv);
237

64
    Force::Vector6 contact_force_plus = x_plus.tail(6);
238
239


64
    dddq_dtau.col(k) = (ddq_plus - ddq_ref)/eps;
240


64
    dlambda_dtau.col(k) = (contact_force_plus - contact_force_ref)/eps;
241
242
64
    tau_plus[k] -= eps;
243
  }
244
245
4
  MatrixXd A(model.nv+6,model.nv+6);
246


2
  data.M.transpose().triangularView<Eigen::Upper>() = data.M.triangularView<Eigen::Upper>();
247

2
  A.topLeftCorner(model.nv,model.nv) = data.M;
248

2
  A.bottomLeftCorner(6, model.nv) = J_RF;
249

2
  A.topRightCorner(model.nv, 6) = J_RF.transpose();
250

2
  A.bottomRightCorner(6,6).setZero();
251
252

4
  MatrixXd Ainv = A.inverse();
253




2
  BOOST_CHECK(Ainv.topRows(model.nv).leftCols(model.nv).isApprox(dddq_dtau,std::sqrt(eps)));
254





2
  BOOST_CHECK(Ainv.bottomRows(6).leftCols(model.nv).isApprox(-dlambda_dtau,std::sqrt(eps)));
255
256
  // check: dddq_dv and dlambda_dv
257
4
  MatrixXd dddq_dv(model.nv,model.nv);
258
4
  Data::Matrix6x dlambda_dv(6,model.nv);
259
260
66
  for(int k = 0; k < model.nv; ++k)
261
  {
262
64
    v_plus[k] += eps;
263
64
    x_plus = contactDynamics(model,data,q,v_plus,tau,RF_id);
264
265

64
    const Data::TangentVectorType ddq_plus = x_plus.head(model.nv);
266

64
    Force::Vector6 contact_force_plus = x_plus.tail(6);
267
268


64
    dddq_dv.col(k) = (ddq_plus - ddq_ref)/eps;
269


64
    dlambda_dv.col(k) = (contact_force_plus - contact_force_ref)/eps;
270
271
64
    v_plus[k] -= eps;
272
  }
273
274
275

2
  computeRNEADerivatives(model,data_check,q,v,VectorXd::Zero(model.nv));
276


4
  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


2
  v_partial_dq.setZero(); a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero();
278
4
  Data data_kin(model);
279

2
  computeForwardKinematicsDerivatives(model,data_kin,q,v,VectorXd::Zero(model.nv));
280
2
  getJointAccelerationDerivatives(model,data_kin,RF_id,LOCAL,v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);
281
282


4
  MatrixXd dddq_dv_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dv;
283


2
  dddq_dv_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dv;
284


4
  MatrixXd dlambda_dv_anal = -Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dv;
285


2
  dlambda_dv_anal -= Ainv.bottomRows(6).rightCols(6) * a_partial_dv;
286
287



2
  BOOST_CHECK(dddq_dv_anal.isApprox(dddq_dv,std::sqrt(eps)));
288




2
  BOOST_CHECK(dlambda_dv_anal.isApprox(-dlambda_dv,std::sqrt(eps)));
289
290
291
4
  MatrixXd dddq_dq(model.nv,model.nv);
292
4
  Data::Matrix6x dlambda_dq(6,model.nv);
293
294
66
  for(int k = 0; k < model.nv; ++k)
295
  {
296
64
    v_eps[k] = eps;
297
64
    q_plus = integrate(model,q,v_eps);
298
299
64
    x_plus = contactDynamics(model,data,q_plus,v,tau,RF_id);
300
301

64
    const Data::TangentVectorType ddq_plus = x_plus.head(model.nv);
302

64
    Force::Vector6 contact_force_plus = x_plus.tail(6);
303
304


64
    dddq_dq.col(k) = (ddq_plus - ddq_ref)/eps;
305


64
    dlambda_dq.col(k) = (contact_force_plus - contact_force_ref)/eps;
306
307
64
    v_eps[k] = 0.;
308
  }
309
310
2
  computeRNEADerivatives(model,data_check,q,v,ddq_ref,fext);
311


2
  v_partial_dq.setZero(); a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero();
312
2
  computeForwardKinematicsDerivatives(model,data_kin,q,v,ddq_ref);
313
2
  getJointAccelerationDerivatives(model,data_kin,RF_id,LOCAL,v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);
314
315


4
  MatrixXd dddq_dq_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dq;
316


2
  dddq_dq_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dq;
317
318



2
  BOOST_CHECK(dddq_dq_anal.isApprox(dddq_dq,std::sqrt(eps)));
319
320


4
  MatrixXd dlambda_dq_anal = Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dq;
321


2
  dlambda_dq_anal += Ainv.bottomRows(6).rightCols(6) * a_partial_dq;
322
323



2
  BOOST_CHECK(dlambda_dq_anal.isApprox(dlambda_dq,std::sqrt(eps)));
324
325
2
}
326
327
BOOST_AUTO_TEST_SUITE_END ()
328