GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: unittest/rnea-second-order-derivatives.cpp Lines: 144 144 100.0 %
Date: 2024-04-26 13:14:21 Branches: 334 644 51.9 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2017-2020 CNRS INRIA
3
//
4
5
#include "pinocchio/multibody/model.hpp"
6
#include "pinocchio/multibody/data.hpp"
7
#include "pinocchio/algorithm/jacobian.hpp"
8
#include "pinocchio/algorithm/joint-configuration.hpp"
9
#include "pinocchio/algorithm/rnea-second-order-derivatives.hpp"
10
#include "pinocchio/algorithm/rnea-derivatives.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
















4
BOOST_AUTO_TEST_CASE(test_rnea_derivatives_SO) {
21
  using namespace Eigen;
22
  using namespace pinocchio;
23
24
4
  Model model;
25
2
  buildModels::humanoidRandom(model);
26
27

4
  Data data(model), data_fd(model);
28
29

2
  model.lowerPositionLimit.head<3>().fill(-1.);
30

2
  model.upperPositionLimit.head<3>().fill(1.);
31
32
4
  VectorXd q = randomConfiguration(model);
33

4
  VectorXd v(VectorXd::Random(model.nv));
34

4
  VectorXd a(VectorXd::Random(model.nv));
35
36
  // check with only q non-zero
37
4
  Data::Tensor3x dtau2_dq(model.nv, model.nv, model.nv);
38
4
  Data::Tensor3x dtau2_dv(model.nv, model.nv, model.nv);
39
4
  Data::Tensor3x dtau2_dqdv(model.nv, model.nv, model.nv);
40
4
  Data::Tensor3x dtau2_dadq(model.nv, model.nv, model.nv);
41
2
  dtau2_dq.setZero();
42
2
  dtau2_dv.setZero();
43
2
  dtau2_dqdv.setZero();
44
2
  dtau2_dadq.setZero();
45
46

2
  ComputeRNEASecondOrderDerivatives(model, data, q, VectorXd::Zero(model.nv),
47
2
                           VectorXd::Zero(model.nv), dtau2_dq, dtau2_dv,
48
                           dtau2_dqdv, dtau2_dadq);
49
50
4
  Data::Tensor3x dtau2_dq_fd(model.nv, model.nv, model.nv);
51
4
  Data::Tensor3x dtau2_dv_fd(model.nv, model.nv, model.nv);
52
4
  Data::Tensor3x dtau2_dqdv_fd(model.nv, model.nv, model.nv);
53
4
  Data::Tensor3x dtau2_dadq_fd(model.nv, model.nv, model.nv);
54
2
  dtau2_dq_fd.setZero();
55
2
  dtau2_dv_fd.setZero();
56
2
  dtau2_dqdv_fd.setZero();
57
2
  dtau2_dadq_fd.setZero();
58
59

4
  MatrixXd drnea_dq_plus(MatrixXd::Zero(model.nv, model.nv));
60

4
  MatrixXd drnea_dv_plus(MatrixXd::Zero(model.nv, model.nv));
61

4
  MatrixXd drnea_da_plus(MatrixXd::Zero(model.nv, model.nv));
62
63

4
  MatrixXd temp1(MatrixXd::Zero(model.nv, model.nv));
64

4
  MatrixXd temp2(MatrixXd::Zero(model.nv, model.nv));
65
66

4
  VectorXd v_eps(VectorXd::Zero(model.nv));
67
4
  VectorXd q_plus(model.nq);
68
4
  VectorXd v_plus(model.nv);
69
70

4
  MatrixXd rnea_partial_dq(MatrixXd::Zero(model.nv, model.nv));
71

4
  MatrixXd rnea_partial_dv(MatrixXd::Zero(model.nv, model.nv));
72

4
  MatrixXd rnea_partial_da(MatrixXd::Zero(model.nv, model.nv));
73
74

2
  computeRNEADerivatives(model, data, q, VectorXd::Zero(model.nv),
75
2
                         VectorXd::Zero(model.nv), rnea_partial_dq,
76
                         rnea_partial_dv, rnea_partial_da);
77
78
2
  const double alpha = 1e-7;
79
80
66
  for (int k = 0; k < model.nv; ++k) {
81
64
    v_eps[k] += alpha;
82
64
    q_plus = integrate(model, q, v_eps);
83

64
    computeRNEADerivatives(model, data_fd, q_plus, VectorXd::Zero(model.nv),
84
64
                           VectorXd::Zero(model.nv), drnea_dq_plus,
85
                           drnea_dv_plus, drnea_da_plus);
86

64
    temp1 = (drnea_dq_plus - rnea_partial_dq) / alpha;
87
2112
    for (int ii = 0; ii < model.nv; ii++) {
88
67584
      for (int jj = 0; jj < model.nv; jj++) {
89

65536
        dtau2_dq_fd(jj, ii, k) = temp1(jj, ii);
90
      }
91
    }
92
64
    v_eps[k] -= alpha;
93
  }
94
95


2
  Map<VectorXd> mq(dtau2_dq.data(), dtau2_dq.size());
96


2
  Map<VectorXd> mq_fd(dtau2_dq_fd.data(), dtau2_dq_fd.size());
97



2
  BOOST_CHECK(mq.isApprox(mq_fd, sqrt(alpha)));
98
99
  // Check with q and a non zero
100
2
  dtau2_dq.setZero();
101
2
  dtau2_dv.setZero();
102
2
  dtau2_dqdv.setZero();
103
2
  dtau2_dadq.setZero();
104

2
  ComputeRNEASecondOrderDerivatives(model, data, q, VectorXd::Zero(model.nv), a,
105
                           dtau2_dq, dtau2_dv, dtau2_dqdv, dtau2_dadq);
106
107
2
  rnea_partial_dq.setZero();
108
2
  rnea_partial_dv.setZero();
109
2
  rnea_partial_da.setZero();
110
111
2
  dtau2_dq_fd.setZero();
112
2
  dtau2_dadq_fd.setZero();
113
2
  drnea_dq_plus.setZero();
114
2
  drnea_dv_plus.setZero();
115
2
  drnea_da_plus.setZero();
116
117

2
  computeRNEADerivatives(model, data, q, VectorXd::Zero(model.nv), a,
118
                         rnea_partial_dq, rnea_partial_dv, rnea_partial_da);
119
120
66
  for (int k = 0; k < model.nv; ++k) {
121
64
    v_eps[k] += alpha;
122
64
    q_plus = integrate(model, q, v_eps);
123

64
    computeRNEADerivatives(model, data_fd, q_plus, VectorXd::Zero(model.nv), a,
124
                           drnea_dq_plus, drnea_dv_plus, drnea_da_plus);
125

64
    temp1 = (drnea_dq_plus - rnea_partial_dq) / alpha;
126

64
    temp2 = (drnea_da_plus - rnea_partial_da) / alpha;
127
64
    temp2.triangularView<Eigen::StrictlyLower>() =
128

128
        temp2.transpose().triangularView<Eigen::StrictlyLower>();
129
2112
    for (int ii = 0; ii < model.nv; ii++) {
130
67584
      for (int jj = 0; jj < model.nv; jj++) {
131

65536
        dtau2_dq_fd(jj, ii, k) = temp1(jj, ii);
132

65536
        dtau2_dadq_fd(jj, ii, k) = temp2(jj, ii);
133
      }
134
    }
135
64
    v_eps[k] -= alpha;
136
  }
137


2
  Map<VectorXd> maq(dtau2_dadq.data(), dtau2_dadq.size());
138


2
  Map<VectorXd> maq_fd(dtau2_dadq_fd.data(), dtau2_dadq_fd.size());
139
140



2
  BOOST_CHECK(mq.isApprox(mq_fd, sqrt(alpha)));
141



2
  BOOST_CHECK(maq.isApprox(maq_fd, sqrt(alpha)));
142
143
  // Check with q,v and a non zero
144
2
  dtau2_dq.setZero();
145
2
  dtau2_dv.setZero();
146
2
  dtau2_dqdv.setZero();
147
2
  dtau2_dadq.setZero();
148
2
  ComputeRNEASecondOrderDerivatives(model, data, q, v, a, dtau2_dq, dtau2_dv, dtau2_dqdv,
149
                           dtau2_dadq);
150
151
2
  rnea_partial_dq.setZero();
152
2
  rnea_partial_dv.setZero();
153
2
  rnea_partial_da.setZero();
154
2
  computeRNEADerivatives(model, data, q, v, a, rnea_partial_dq, rnea_partial_dv,
155
                         rnea_partial_da);
156
157
2
  dtau2_dq_fd.setZero();
158
2
  dtau2_dadq_fd.setZero();
159
2
  drnea_dq_plus.setZero();
160
2
  drnea_dv_plus.setZero();
161
2
  drnea_da_plus.setZero();
162
163
66
  for (int k = 0; k < model.nv; ++k) {
164
64
    v_eps[k] += alpha;
165
64
    q_plus = integrate(model, q, v_eps);
166
64
    computeRNEADerivatives(model, data_fd, q_plus, v, a, drnea_dq_plus,
167
                           drnea_dv_plus, drnea_da_plus);
168

64
    temp1 = (drnea_dq_plus - rnea_partial_dq) / alpha;
169

64
    temp2 = (drnea_da_plus - rnea_partial_da) / alpha;
170
64
    temp2.triangularView<Eigen::StrictlyLower>() =
171

128
        temp2.transpose().triangularView<Eigen::StrictlyLower>();
172
2112
    for (int ii = 0; ii < model.nv; ii++) {
173
67584
      for (int jj = 0; jj < model.nv; jj++) {
174

65536
        dtau2_dq_fd(jj, ii, k) = temp1(jj, ii);
175

65536
        dtau2_dadq_fd(jj, ii, k) = temp2(jj, ii);
176
      }
177
    }
178
64
    v_eps[k] -= alpha;
179
  }
180
2
  dtau2_dv_fd.setZero();
181
2
  dtau2_dqdv_fd.setZero();
182
66
  for (int k = 0; k < model.nv; ++k) {
183
64
    v_eps[k] += alpha;
184

64
    v_plus = v + v_eps;
185
64
    computeRNEADerivatives(model, data_fd, q, v_plus, a, drnea_dq_plus,
186
                           drnea_dv_plus, drnea_da_plus);
187

64
    temp1 = (drnea_dv_plus - rnea_partial_dv) / alpha;
188

64
    temp2 = (drnea_dq_plus - rnea_partial_dq) / alpha;
189
2112
    for (int ii = 0; ii < model.nv; ii++) {
190
67584
      for (int jj = 0; jj < model.nv; jj++) {
191

65536
        dtau2_dv_fd(jj, ii, k) = temp1(jj, ii);
192

65536
        dtau2_dqdv_fd(jj, ii, k) = temp2(jj, ii);
193
      }
194
    }
195
64
    v_eps[k] -= alpha;
196
  }
197


2
  Map<VectorXd> mv(dtau2_dv.data(), dtau2_dv.size());
198


2
  Map<VectorXd> mv_fd(dtau2_dv_fd.data(), dtau2_dv_fd.size());
199
200


2
  Map<VectorXd> mqv(dtau2_dqdv.data(), dtau2_dqdv.size());
201


2
  Map<VectorXd> mqv_fd(dtau2_dqdv_fd.data(), dtau2_dqdv_fd.size());
202
203



2
  BOOST_CHECK(mq.isApprox(mq_fd, sqrt(alpha)));
204



2
  BOOST_CHECK(maq.isApprox(maq_fd, sqrt(alpha)));
205



2
  BOOST_CHECK(mv.isApprox(mv_fd, sqrt(alpha)));
206



2
  BOOST_CHECK(mqv.isApprox(mqv_fd, sqrt(alpha)));
207
208
4
  Data data2(model);
209
2
  ComputeRNEASecondOrderDerivatives(model, data2, q, v, a);
210
211


2
  Map<VectorXd> mq2(data2.d2tau_dqdq.data(), (data2.d2tau_dqdq).size());
212


2
  Map<VectorXd> mv2(data2.d2tau_dvdv.data(), (data2.d2tau_dvdv).size());
213


2
  Map<VectorXd> mqv2(data2.d2tau_dqdv.data(), (data2.d2tau_dqdv).size());
214


2
  Map<VectorXd> maq2(data2.d2tau_dadq.data(), (data2.d2tau_dadq).size());
215
216



2
  BOOST_CHECK(mq.isApprox(mq2, sqrt(alpha)));
217



2
  BOOST_CHECK(mv.isApprox(mv2, sqrt(alpha)));
218



2
  BOOST_CHECK(mqv.isApprox(mqv2, sqrt(alpha)));
219



2
  BOOST_CHECK(maq.isApprox(maq2, sqrt(alpha)));
220
2
}
221
222
BOOST_AUTO_TEST_SUITE_END()