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/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 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_rnea_derivatives_SO) |
21 |
|
|
{ |
22 |
|
|
using namespace Eigen; |
23 |
|
|
using namespace pinocchio; |
24 |
|
|
|
25 |
|
✗ |
Model model; |
26 |
|
✗ |
buildModels::humanoidRandom(model); |
27 |
|
|
|
28 |
|
✗ |
Data data(model), data_fd(model); |
29 |
|
|
|
30 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
31 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
32 |
|
|
|
33 |
|
✗ |
VectorXd q = randomConfiguration(model); |
34 |
|
✗ |
VectorXd v(VectorXd::Random(model.nv)); |
35 |
|
✗ |
VectorXd a(VectorXd::Random(model.nv)); |
36 |
|
|
|
37 |
|
|
// check with only q non-zero |
38 |
|
✗ |
Data::Tensor3x dtau2_dq(model.nv, model.nv, model.nv); |
39 |
|
✗ |
Data::Tensor3x dtau2_dv(model.nv, model.nv, model.nv); |
40 |
|
✗ |
Data::Tensor3x dtau2_dqdv(model.nv, model.nv, model.nv); |
41 |
|
✗ |
Data::Tensor3x dtau2_dadq(model.nv, model.nv, model.nv); |
42 |
|
✗ |
dtau2_dq.setZero(); |
43 |
|
✗ |
dtau2_dv.setZero(); |
44 |
|
✗ |
dtau2_dqdv.setZero(); |
45 |
|
✗ |
dtau2_dadq.setZero(); |
46 |
|
|
|
47 |
|
✗ |
ComputeRNEASecondOrderDerivatives( |
48 |
|
✗ |
model, data, q, VectorXd::Zero(model.nv), VectorXd::Zero(model.nv), dtau2_dq, dtau2_dv, |
49 |
|
|
dtau2_dqdv, dtau2_dadq); |
50 |
|
|
|
51 |
|
✗ |
Data::Tensor3x dtau2_dq_fd(model.nv, model.nv, model.nv); |
52 |
|
✗ |
Data::Tensor3x dtau2_dv_fd(model.nv, model.nv, model.nv); |
53 |
|
✗ |
Data::Tensor3x dtau2_dqdv_fd(model.nv, model.nv, model.nv); |
54 |
|
✗ |
Data::Tensor3x dtau2_dadq_fd(model.nv, model.nv, model.nv); |
55 |
|
✗ |
dtau2_dq_fd.setZero(); |
56 |
|
✗ |
dtau2_dv_fd.setZero(); |
57 |
|
✗ |
dtau2_dqdv_fd.setZero(); |
58 |
|
✗ |
dtau2_dadq_fd.setZero(); |
59 |
|
|
|
60 |
|
✗ |
MatrixXd drnea_dq_plus(MatrixXd::Zero(model.nv, model.nv)); |
61 |
|
✗ |
MatrixXd drnea_dv_plus(MatrixXd::Zero(model.nv, model.nv)); |
62 |
|
✗ |
MatrixXd drnea_da_plus(MatrixXd::Zero(model.nv, model.nv)); |
63 |
|
|
|
64 |
|
✗ |
MatrixXd temp1(MatrixXd::Zero(model.nv, model.nv)); |
65 |
|
✗ |
MatrixXd temp2(MatrixXd::Zero(model.nv, model.nv)); |
66 |
|
|
|
67 |
|
✗ |
VectorXd v_eps(VectorXd::Zero(model.nv)); |
68 |
|
✗ |
VectorXd q_plus(model.nq); |
69 |
|
✗ |
VectorXd v_plus(model.nv); |
70 |
|
|
|
71 |
|
✗ |
MatrixXd rnea_partial_dq(MatrixXd::Zero(model.nv, model.nv)); |
72 |
|
✗ |
MatrixXd rnea_partial_dv(MatrixXd::Zero(model.nv, model.nv)); |
73 |
|
✗ |
MatrixXd rnea_partial_da(MatrixXd::Zero(model.nv, model.nv)); |
74 |
|
|
|
75 |
|
✗ |
computeRNEADerivatives( |
76 |
|
✗ |
model, data, q, VectorXd::Zero(model.nv), VectorXd::Zero(model.nv), rnea_partial_dq, |
77 |
|
|
rnea_partial_dv, rnea_partial_da); |
78 |
|
|
|
79 |
|
✗ |
const double alpha = 1e-7; |
80 |
|
|
|
81 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
82 |
|
|
{ |
83 |
|
✗ |
v_eps[k] += alpha; |
84 |
|
✗ |
q_plus = integrate(model, q, v_eps); |
85 |
|
✗ |
computeRNEADerivatives( |
86 |
|
✗ |
model, data_fd, q_plus, VectorXd::Zero(model.nv), VectorXd::Zero(model.nv), drnea_dq_plus, |
87 |
|
|
drnea_dv_plus, drnea_da_plus); |
88 |
|
✗ |
temp1 = (drnea_dq_plus - rnea_partial_dq) / alpha; |
89 |
|
✗ |
for (int ii = 0; ii < model.nv; ii++) |
90 |
|
|
{ |
91 |
|
✗ |
for (int jj = 0; jj < model.nv; jj++) |
92 |
|
|
{ |
93 |
|
✗ |
dtau2_dq_fd(jj, ii, k) = temp1(jj, ii); |
94 |
|
|
} |
95 |
|
|
} |
96 |
|
✗ |
v_eps[k] -= alpha; |
97 |
|
|
} |
98 |
|
|
|
99 |
|
✗ |
Map<VectorXd> mq(dtau2_dq.data(), dtau2_dq.size()); |
100 |
|
✗ |
Map<VectorXd> mq_fd(dtau2_dq_fd.data(), dtau2_dq_fd.size()); |
101 |
|
✗ |
BOOST_CHECK(mq.isApprox(mq_fd, sqrt(alpha))); |
102 |
|
|
|
103 |
|
|
// Check with q and a non zero |
104 |
|
✗ |
dtau2_dq.setZero(); |
105 |
|
✗ |
dtau2_dv.setZero(); |
106 |
|
✗ |
dtau2_dqdv.setZero(); |
107 |
|
✗ |
dtau2_dadq.setZero(); |
108 |
|
✗ |
ComputeRNEASecondOrderDerivatives( |
109 |
|
✗ |
model, data, q, VectorXd::Zero(model.nv), a, dtau2_dq, dtau2_dv, dtau2_dqdv, dtau2_dadq); |
110 |
|
|
|
111 |
|
✗ |
rnea_partial_dq.setZero(); |
112 |
|
✗ |
rnea_partial_dv.setZero(); |
113 |
|
✗ |
rnea_partial_da.setZero(); |
114 |
|
|
|
115 |
|
✗ |
dtau2_dq_fd.setZero(); |
116 |
|
✗ |
dtau2_dadq_fd.setZero(); |
117 |
|
✗ |
drnea_dq_plus.setZero(); |
118 |
|
✗ |
drnea_dv_plus.setZero(); |
119 |
|
✗ |
drnea_da_plus.setZero(); |
120 |
|
|
|
121 |
|
✗ |
computeRNEADerivatives( |
122 |
|
✗ |
model, data, q, VectorXd::Zero(model.nv), a, rnea_partial_dq, rnea_partial_dv, rnea_partial_da); |
123 |
|
|
|
124 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
125 |
|
|
{ |
126 |
|
✗ |
v_eps[k] += alpha; |
127 |
|
✗ |
q_plus = integrate(model, q, v_eps); |
128 |
|
✗ |
computeRNEADerivatives( |
129 |
|
✗ |
model, data_fd, q_plus, VectorXd::Zero(model.nv), a, drnea_dq_plus, drnea_dv_plus, |
130 |
|
|
drnea_da_plus); |
131 |
|
✗ |
temp1 = (drnea_dq_plus - rnea_partial_dq) / alpha; |
132 |
|
✗ |
temp2 = (drnea_da_plus - rnea_partial_da) / alpha; |
133 |
|
✗ |
temp2.triangularView<Eigen::StrictlyLower>() = |
134 |
|
✗ |
temp2.transpose().triangularView<Eigen::StrictlyLower>(); |
135 |
|
✗ |
for (int ii = 0; ii < model.nv; ii++) |
136 |
|
|
{ |
137 |
|
✗ |
for (int jj = 0; jj < model.nv; jj++) |
138 |
|
|
{ |
139 |
|
✗ |
dtau2_dq_fd(jj, ii, k) = temp1(jj, ii); |
140 |
|
✗ |
dtau2_dadq_fd(jj, ii, k) = temp2(jj, ii); |
141 |
|
|
} |
142 |
|
|
} |
143 |
|
✗ |
v_eps[k] -= alpha; |
144 |
|
|
} |
145 |
|
✗ |
Map<VectorXd> maq(dtau2_dadq.data(), dtau2_dadq.size()); |
146 |
|
✗ |
Map<VectorXd> maq_fd(dtau2_dadq_fd.data(), dtau2_dadq_fd.size()); |
147 |
|
|
|
148 |
|
✗ |
BOOST_CHECK(mq.isApprox(mq_fd, sqrt(alpha))); |
149 |
|
✗ |
BOOST_CHECK(maq.isApprox(maq_fd, sqrt(alpha))); |
150 |
|
|
|
151 |
|
|
// Check with q,v and a non zero |
152 |
|
✗ |
dtau2_dq.setZero(); |
153 |
|
✗ |
dtau2_dv.setZero(); |
154 |
|
✗ |
dtau2_dqdv.setZero(); |
155 |
|
✗ |
dtau2_dadq.setZero(); |
156 |
|
✗ |
ComputeRNEASecondOrderDerivatives( |
157 |
|
|
model, data, q, v, a, dtau2_dq, dtau2_dv, dtau2_dqdv, dtau2_dadq); |
158 |
|
|
|
159 |
|
✗ |
rnea_partial_dq.setZero(); |
160 |
|
✗ |
rnea_partial_dv.setZero(); |
161 |
|
✗ |
rnea_partial_da.setZero(); |
162 |
|
✗ |
computeRNEADerivatives(model, data, q, v, a, rnea_partial_dq, rnea_partial_dv, rnea_partial_da); |
163 |
|
|
|
164 |
|
✗ |
dtau2_dq_fd.setZero(); |
165 |
|
✗ |
dtau2_dadq_fd.setZero(); |
166 |
|
✗ |
drnea_dq_plus.setZero(); |
167 |
|
✗ |
drnea_dv_plus.setZero(); |
168 |
|
✗ |
drnea_da_plus.setZero(); |
169 |
|
|
|
170 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
171 |
|
|
{ |
172 |
|
✗ |
v_eps[k] += alpha; |
173 |
|
✗ |
q_plus = integrate(model, q, v_eps); |
174 |
|
✗ |
computeRNEADerivatives( |
175 |
|
|
model, data_fd, q_plus, v, a, drnea_dq_plus, drnea_dv_plus, drnea_da_plus); |
176 |
|
✗ |
temp1 = (drnea_dq_plus - rnea_partial_dq) / alpha; |
177 |
|
✗ |
temp2 = (drnea_da_plus - rnea_partial_da) / alpha; |
178 |
|
✗ |
temp2.triangularView<Eigen::StrictlyLower>() = |
179 |
|
✗ |
temp2.transpose().triangularView<Eigen::StrictlyLower>(); |
180 |
|
✗ |
for (int ii = 0; ii < model.nv; ii++) |
181 |
|
|
{ |
182 |
|
✗ |
for (int jj = 0; jj < model.nv; jj++) |
183 |
|
|
{ |
184 |
|
✗ |
dtau2_dq_fd(jj, ii, k) = temp1(jj, ii); |
185 |
|
✗ |
dtau2_dadq_fd(jj, ii, k) = temp2(jj, ii); |
186 |
|
|
} |
187 |
|
|
} |
188 |
|
✗ |
v_eps[k] -= alpha; |
189 |
|
|
} |
190 |
|
✗ |
dtau2_dv_fd.setZero(); |
191 |
|
✗ |
dtau2_dqdv_fd.setZero(); |
192 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
193 |
|
|
{ |
194 |
|
✗ |
v_eps[k] += alpha; |
195 |
|
✗ |
v_plus = v + v_eps; |
196 |
|
✗ |
computeRNEADerivatives( |
197 |
|
|
model, data_fd, q, v_plus, a, drnea_dq_plus, drnea_dv_plus, drnea_da_plus); |
198 |
|
✗ |
temp1 = (drnea_dv_plus - rnea_partial_dv) / alpha; |
199 |
|
✗ |
temp2 = (drnea_dq_plus - rnea_partial_dq) / alpha; |
200 |
|
✗ |
for (int ii = 0; ii < model.nv; ii++) |
201 |
|
|
{ |
202 |
|
✗ |
for (int jj = 0; jj < model.nv; jj++) |
203 |
|
|
{ |
204 |
|
✗ |
dtau2_dv_fd(jj, ii, k) = temp1(jj, ii); |
205 |
|
✗ |
dtau2_dqdv_fd(jj, ii, k) = temp2(jj, ii); |
206 |
|
|
} |
207 |
|
|
} |
208 |
|
✗ |
v_eps[k] -= alpha; |
209 |
|
|
} |
210 |
|
✗ |
Map<VectorXd> mv(dtau2_dv.data(), dtau2_dv.size()); |
211 |
|
✗ |
Map<VectorXd> mv_fd(dtau2_dv_fd.data(), dtau2_dv_fd.size()); |
212 |
|
|
|
213 |
|
✗ |
Map<VectorXd> mqv(dtau2_dqdv.data(), dtau2_dqdv.size()); |
214 |
|
✗ |
Map<VectorXd> mqv_fd(dtau2_dqdv_fd.data(), dtau2_dqdv_fd.size()); |
215 |
|
|
|
216 |
|
✗ |
BOOST_CHECK(mq.isApprox(mq_fd, sqrt(alpha))); |
217 |
|
✗ |
BOOST_CHECK(maq.isApprox(maq_fd, sqrt(alpha))); |
218 |
|
✗ |
BOOST_CHECK(mv.isApprox(mv_fd, sqrt(alpha))); |
219 |
|
✗ |
BOOST_CHECK(mqv.isApprox(mqv_fd, sqrt(alpha))); |
220 |
|
|
|
221 |
|
✗ |
Data data2(model); |
222 |
|
✗ |
ComputeRNEASecondOrderDerivatives(model, data2, q, v, a); |
223 |
|
|
|
224 |
|
✗ |
Map<VectorXd> mq2(data2.d2tau_dqdq.data(), (data2.d2tau_dqdq).size()); |
225 |
|
✗ |
Map<VectorXd> mv2(data2.d2tau_dvdv.data(), (data2.d2tau_dvdv).size()); |
226 |
|
✗ |
Map<VectorXd> mqv2(data2.d2tau_dqdv.data(), (data2.d2tau_dqdv).size()); |
227 |
|
✗ |
Map<VectorXd> maq2(data2.d2tau_dadq.data(), (data2.d2tau_dadq).size()); |
228 |
|
|
|
229 |
|
✗ |
BOOST_CHECK(mq.isApprox(mq2, sqrt(alpha))); |
230 |
|
✗ |
BOOST_CHECK(mv.isApprox(mv2, sqrt(alpha))); |
231 |
|
✗ |
BOOST_CHECK(mqv.isApprox(mqv2, sqrt(alpha))); |
232 |
|
✗ |
BOOST_CHECK(maq.isApprox(maq2, sqrt(alpha))); |
233 |
|
|
} |
234 |
|
|
|
235 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
236 |
|
|
|