Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2018-2020 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include "pinocchio/algorithm/jacobian.hpp" |
6 |
|
|
#include "pinocchio/algorithm/joint-configuration.hpp" |
7 |
|
|
#include "pinocchio/algorithm/kinematics.hpp" |
8 |
|
|
#include "pinocchio/algorithm/kinematics-derivatives.hpp" |
9 |
|
|
#include "pinocchio/algorithm/rnea.hpp" |
10 |
|
|
#include "pinocchio/algorithm/rnea-derivatives.hpp" |
11 |
|
|
#include "pinocchio/algorithm/aba.hpp" |
12 |
|
|
#include "pinocchio/algorithm/aba-derivatives.hpp" |
13 |
|
|
#include "pinocchio/algorithm/crba.hpp" |
14 |
|
|
#include "pinocchio/multibody/sample-models.hpp" |
15 |
|
|
|
16 |
|
|
#include <iostream> |
17 |
|
|
|
18 |
|
|
#include <boost/test/unit_test.hpp> |
19 |
|
|
#include <boost/utility/binary.hpp> |
20 |
|
|
|
21 |
|
|
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
22 |
|
|
|
23 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_aba_derivatives) |
24 |
|
|
{ |
25 |
|
|
using namespace Eigen; |
26 |
|
|
using namespace pinocchio; |
27 |
|
|
|
28 |
|
✗ |
Model model; |
29 |
|
✗ |
buildModels::humanoidRandom(model); |
30 |
|
|
|
31 |
|
✗ |
Data data(model), data_ref(model); |
32 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
33 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
34 |
|
✗ |
VectorXd q = randomConfiguration(model); |
35 |
|
✗ |
VectorXd v(VectorXd::Random(model.nv)); |
36 |
|
✗ |
VectorXd tau(VectorXd::Random(model.nv)); |
37 |
|
✗ |
VectorXd a(aba(model, data_ref, q, v, tau, Convention::LOCAL)); |
38 |
|
|
|
39 |
|
✗ |
VectorXd tau_from_a(rnea(model, data_ref, q, v, a)); |
40 |
|
✗ |
BOOST_CHECK(tau_from_a.isApprox(tau)); |
41 |
|
|
|
42 |
|
✗ |
MatrixXd aba_partial_dq(model.nv, model.nv); |
43 |
|
✗ |
aba_partial_dq.setZero(); |
44 |
|
✗ |
MatrixXd aba_partial_dv(model.nv, model.nv); |
45 |
|
✗ |
aba_partial_dv.setZero(); |
46 |
|
✗ |
Data::RowMatrixXs aba_partial_dtau(model.nv, model.nv); |
47 |
|
✗ |
aba_partial_dtau.setZero(); |
48 |
|
|
|
49 |
|
✗ |
const double prec = Eigen::NumTraits<double>::dummy_precision(); |
50 |
|
|
|
51 |
|
✗ |
computeABADerivatives(model, data, q, v, tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau); |
52 |
|
✗ |
computeRNEADerivatives(model, data_ref, q, v, a); |
53 |
|
✗ |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
54 |
|
✗ |
for (Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k) |
55 |
|
|
{ |
56 |
|
✗ |
BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k])); |
57 |
|
✗ |
BOOST_CHECK(data.ov[k].isApprox(data_ref.ov[k])); |
58 |
|
✗ |
BOOST_CHECK(data.oh[k].isApprox(data_ref.oh[k])); |
59 |
|
✗ |
BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oa[k] - model.gravity)); |
60 |
|
✗ |
BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oa_gf[k])); |
61 |
|
✗ |
BOOST_CHECK(data.of[k].isApprox(data_ref.of[k], 1e1 * prec)); |
62 |
|
✗ |
BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k])); |
63 |
|
✗ |
BOOST_CHECK(data.doYcrb[k].isApprox(data_ref.doYcrb[k])); |
64 |
|
|
} |
65 |
|
|
|
66 |
|
✗ |
aba(model, data_ref, q, v, tau, Convention::LOCAL); |
67 |
|
✗ |
for (Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k) |
68 |
|
|
{ |
69 |
|
✗ |
BOOST_CHECK(data.oYaba[k].isApprox( |
70 |
|
|
data_ref.oMi[k].toDualActionMatrix() * data_ref.Yaba[k] |
71 |
|
|
* data_ref.oMi[k].inverse().toActionMatrix())); |
72 |
|
|
// BOOST_CHECK(data.of[k].isApprox(data.oMi[k].act(data_ref.f[k]))); |
73 |
|
|
} |
74 |
|
✗ |
BOOST_CHECK(data.ddq.isApprox(data_ref.ddq)); |
75 |
|
|
|
76 |
|
✗ |
aba(model, data_ref, q, v, tau, Convention::WORLD); |
77 |
|
✗ |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
78 |
|
✗ |
BOOST_CHECK(data.u.isApprox(data_ref.u)); |
79 |
|
✗ |
for (Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k) |
80 |
|
|
{ |
81 |
|
✗ |
BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k])); |
82 |
|
✗ |
BOOST_CHECK(data.ov[k].isApprox(data_ref.ov[k])); |
83 |
|
✗ |
BOOST_CHECK(data.oYaba[k].isApprox(data_ref.oYaba[k])); |
84 |
|
|
// BOOST_CHECK(data.of[k].isApprox(data_ref.of[k])); |
85 |
|
✗ |
BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oa_gf[k])); |
86 |
|
✗ |
BOOST_CHECK(data.joints[k].U().isApprox(data_ref.joints[k].U())); |
87 |
|
✗ |
BOOST_CHECK(data.joints[k].StU().isApprox(data_ref.joints[k].StU())); |
88 |
|
✗ |
BOOST_CHECK(data.joints[k].Dinv().isApprox(data_ref.joints[k].Dinv())); |
89 |
|
✗ |
BOOST_CHECK(data.joints[k].UDinv().isApprox(data_ref.joints[k].UDinv())); |
90 |
|
|
} |
91 |
|
✗ |
BOOST_CHECK(data.ddq.isApprox(data_ref.ddq)); |
92 |
|
|
|
93 |
|
✗ |
computeJointJacobians(model, data_ref, q); |
94 |
|
✗ |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
95 |
|
|
|
96 |
|
✗ |
computeMinverse(model, data_ref, q); |
97 |
|
✗ |
data_ref.Minv.triangularView<Eigen::StrictlyLower>() = |
98 |
|
✗ |
data_ref.Minv.transpose().triangularView<Eigen::StrictlyLower>(); |
99 |
|
|
|
100 |
|
✗ |
BOOST_CHECK(aba_partial_dtau.isApprox(data_ref.Minv)); |
101 |
|
|
|
102 |
|
✗ |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
103 |
|
✗ |
BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); |
104 |
|
✗ |
BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq)); |
105 |
|
✗ |
BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq)); |
106 |
|
✗ |
BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv)); |
107 |
|
✗ |
BOOST_CHECK(data.dtau_dq.isApprox(data_ref.dtau_dq)); |
108 |
|
✗ |
BOOST_CHECK(data.dtau_dv.isApprox(data_ref.dtau_dv)); |
109 |
|
|
|
110 |
|
✗ |
MatrixXd aba_partial_dq_fd(model.nv, model.nv); |
111 |
|
✗ |
aba_partial_dq_fd.setZero(); |
112 |
|
✗ |
MatrixXd aba_partial_dv_fd(model.nv, model.nv); |
113 |
|
✗ |
aba_partial_dv_fd.setZero(); |
114 |
|
✗ |
MatrixXd aba_partial_dtau_fd(model.nv, model.nv); |
115 |
|
✗ |
aba_partial_dtau_fd.setZero(); |
116 |
|
|
|
117 |
|
✗ |
Data data_fd(model); |
118 |
|
✗ |
VectorXd a0 = aba(model, data_fd, q, v, tau, Convention::LOCAL); |
119 |
|
✗ |
VectorXd v_eps(VectorXd::Zero(model.nv)); |
120 |
|
✗ |
VectorXd q_plus(model.nq); |
121 |
|
✗ |
VectorXd a_plus(model.nv); |
122 |
|
✗ |
const double alpha = 1e-8; |
123 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
124 |
|
|
{ |
125 |
|
✗ |
v_eps[k] += alpha; |
126 |
|
✗ |
q_plus = integrate(model, q, v_eps); |
127 |
|
✗ |
a_plus = aba(model, data_fd, q_plus, v, tau, Convention::LOCAL); |
128 |
|
|
|
129 |
|
✗ |
aba_partial_dq_fd.col(k) = (a_plus - a0) / alpha; |
130 |
|
✗ |
v_eps[k] -= alpha; |
131 |
|
|
} |
132 |
|
✗ |
BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_fd, sqrt(alpha))); |
133 |
|
|
|
134 |
|
✗ |
VectorXd v_plus(v); |
135 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
136 |
|
|
{ |
137 |
|
✗ |
v_plus[k] += alpha; |
138 |
|
✗ |
a_plus = aba(model, data_fd, q, v_plus, tau, Convention::LOCAL); |
139 |
|
|
|
140 |
|
✗ |
aba_partial_dv_fd.col(k) = (a_plus - a0) / alpha; |
141 |
|
✗ |
v_plus[k] -= alpha; |
142 |
|
|
} |
143 |
|
✗ |
BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_fd, sqrt(alpha))); |
144 |
|
|
|
145 |
|
✗ |
VectorXd tau_plus(tau); |
146 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
147 |
|
|
{ |
148 |
|
✗ |
tau_plus[k] += alpha; |
149 |
|
✗ |
a_plus = aba(model, data_fd, q, v, tau_plus, Convention::LOCAL); |
150 |
|
|
|
151 |
|
✗ |
aba_partial_dtau_fd.col(k) = (a_plus - a0) / alpha; |
152 |
|
✗ |
tau_plus[k] -= alpha; |
153 |
|
|
} |
154 |
|
✗ |
BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_fd, sqrt(alpha))); |
155 |
|
|
} |
156 |
|
|
|
157 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_aba_minimal_argument) |
158 |
|
|
{ |
159 |
|
|
using namespace Eigen; |
160 |
|
|
using namespace pinocchio; |
161 |
|
|
|
162 |
|
✗ |
Model model; |
163 |
|
✗ |
buildModels::humanoidRandom(model); |
164 |
|
|
|
165 |
|
✗ |
Data data(model), data_ref(model); |
166 |
|
|
|
167 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
168 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
169 |
|
✗ |
VectorXd q = randomConfiguration(model); |
170 |
|
✗ |
VectorXd v(VectorXd::Random(model.nv)); |
171 |
|
✗ |
VectorXd tau(VectorXd::Random(model.nv)); |
172 |
|
✗ |
VectorXd a(aba(model, data_ref, q, v, tau, Convention::LOCAL)); |
173 |
|
|
|
174 |
|
✗ |
MatrixXd aba_partial_dq(model.nv, model.nv); |
175 |
|
✗ |
aba_partial_dq.setZero(); |
176 |
|
✗ |
MatrixXd aba_partial_dv(model.nv, model.nv); |
177 |
|
✗ |
aba_partial_dv.setZero(); |
178 |
|
✗ |
Data::RowMatrixXs aba_partial_dtau(model.nv, model.nv); |
179 |
|
✗ |
aba_partial_dtau.setZero(); |
180 |
|
|
|
181 |
|
✗ |
computeABADerivatives( |
182 |
|
|
model, data_ref, q, v, tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau); |
183 |
|
|
|
184 |
|
✗ |
computeABADerivatives(model, data, q, v, tau); |
185 |
|
|
|
186 |
|
✗ |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
187 |
|
✗ |
BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); |
188 |
|
✗ |
BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq)); |
189 |
|
✗ |
BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq)); |
190 |
|
✗ |
BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv)); |
191 |
|
✗ |
BOOST_CHECK(data.dtau_dq.isApprox(data_ref.dtau_dq)); |
192 |
|
✗ |
BOOST_CHECK(data.dtau_dv.isApprox(data_ref.dtau_dv)); |
193 |
|
✗ |
BOOST_CHECK(data.Minv.isApprox(aba_partial_dtau)); |
194 |
|
✗ |
BOOST_CHECK(data.ddq_dq.isApprox(aba_partial_dq)); |
195 |
|
✗ |
BOOST_CHECK(data.ddq_dv.isApprox(aba_partial_dv)); |
196 |
|
|
} |
197 |
|
|
|
198 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_aba_derivatives_fext) |
199 |
|
|
{ |
200 |
|
|
using namespace Eigen; |
201 |
|
|
using namespace pinocchio; |
202 |
|
|
|
203 |
|
✗ |
Model model; |
204 |
|
✗ |
buildModels::humanoidRandom(model); |
205 |
|
|
|
206 |
|
✗ |
Data data(model), data_ref(model); |
207 |
|
|
|
208 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
209 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
210 |
|
✗ |
VectorXd q = randomConfiguration(model); |
211 |
|
✗ |
VectorXd v(VectorXd::Random(model.nv)); |
212 |
|
✗ |
VectorXd tau(VectorXd::Random(model.nv)); |
213 |
|
✗ |
VectorXd a(aba(model, data_ref, q, v, tau, Convention::LOCAL)); |
214 |
|
|
|
215 |
|
|
typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector; |
216 |
|
✗ |
ForceVector fext((size_t)model.njoints); |
217 |
|
✗ |
for (ForceVector::iterator it = fext.begin(); it != fext.end(); ++it) |
218 |
|
✗ |
(*it).setRandom(); |
219 |
|
|
|
220 |
|
✗ |
MatrixXd aba_partial_dq(model.nv, model.nv); |
221 |
|
✗ |
aba_partial_dq.setZero(); |
222 |
|
✗ |
MatrixXd aba_partial_dv(model.nv, model.nv); |
223 |
|
✗ |
aba_partial_dv.setZero(); |
224 |
|
✗ |
Data::RowMatrixXs aba_partial_dtau(model.nv, model.nv); |
225 |
|
✗ |
aba_partial_dtau.setZero(); |
226 |
|
|
|
227 |
|
✗ |
computeABADerivatives( |
228 |
|
|
model, data, q, v, tau, fext, aba_partial_dq, aba_partial_dv, aba_partial_dtau); |
229 |
|
|
|
230 |
|
✗ |
aba(model, data_ref, q, v, tau, fext, Convention::LOCAL); |
231 |
|
|
// updateGlobalPlacements(model, data_ref); |
232 |
|
|
// for(size_t k =1; k < (size_t)model.njoints; ++k) |
233 |
|
|
// { |
234 |
|
|
// BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k])); |
235 |
|
|
// BOOST_CHECK(daita.of[k].isApprox(data_ref.oMi[k].act(data.f[k]))); |
236 |
|
|
// |
237 |
|
|
// } |
238 |
|
✗ |
BOOST_CHECK(data.ddq.isApprox(data_ref.ddq)); |
239 |
|
|
|
240 |
|
✗ |
computeABADerivatives(model, data_ref, q, v, tau); |
241 |
|
✗ |
BOOST_CHECK(aba_partial_dv.isApprox(data_ref.ddq_dv)); |
242 |
|
✗ |
BOOST_CHECK(aba_partial_dtau.isApprox(data_ref.Minv)); |
243 |
|
|
|
244 |
|
✗ |
MatrixXd aba_partial_dq_fd(model.nv, model.nv); |
245 |
|
✗ |
aba_partial_dq_fd.setZero(); |
246 |
|
✗ |
MatrixXd aba_partial_dv_fd(model.nv, model.nv); |
247 |
|
✗ |
aba_partial_dv_fd.setZero(); |
248 |
|
✗ |
MatrixXd aba_partial_dtau_fd(model.nv, model.nv); |
249 |
|
✗ |
aba_partial_dtau_fd.setZero(); |
250 |
|
|
|
251 |
|
✗ |
Data data_fd(model); |
252 |
|
✗ |
const VectorXd a0 = aba(model, data_fd, q, v, tau, fext, Convention::LOCAL); |
253 |
|
✗ |
VectorXd v_eps(VectorXd::Zero(model.nv)); |
254 |
|
✗ |
VectorXd q_plus(model.nq); |
255 |
|
✗ |
VectorXd a_plus(model.nv); |
256 |
|
✗ |
const double alpha = 1e-8; |
257 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
258 |
|
|
{ |
259 |
|
✗ |
v_eps[k] += alpha; |
260 |
|
✗ |
q_plus = integrate(model, q, v_eps); |
261 |
|
✗ |
a_plus = aba(model, data_fd, q_plus, v, tau, fext, Convention::LOCAL); |
262 |
|
|
|
263 |
|
✗ |
aba_partial_dq_fd.col(k) = (a_plus - a0) / alpha; |
264 |
|
✗ |
v_eps[k] -= alpha; |
265 |
|
|
} |
266 |
|
✗ |
BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_fd, sqrt(alpha))); |
267 |
|
|
|
268 |
|
✗ |
VectorXd v_plus(v); |
269 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
270 |
|
|
{ |
271 |
|
✗ |
v_plus[k] += alpha; |
272 |
|
✗ |
a_plus = aba(model, data_fd, q, v_plus, tau, fext, Convention::LOCAL); |
273 |
|
|
|
274 |
|
✗ |
aba_partial_dv_fd.col(k) = (a_plus - a0) / alpha; |
275 |
|
✗ |
v_plus[k] -= alpha; |
276 |
|
|
} |
277 |
|
✗ |
BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_fd, sqrt(alpha))); |
278 |
|
|
|
279 |
|
✗ |
VectorXd tau_plus(tau); |
280 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
281 |
|
|
{ |
282 |
|
✗ |
tau_plus[k] += alpha; |
283 |
|
✗ |
a_plus = aba(model, data_fd, q, v, tau_plus, fext, Convention::LOCAL); |
284 |
|
|
|
285 |
|
✗ |
aba_partial_dtau_fd.col(k) = (a_plus - a0) / alpha; |
286 |
|
✗ |
tau_plus[k] -= alpha; |
287 |
|
|
} |
288 |
|
✗ |
BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_fd, sqrt(alpha))); |
289 |
|
|
|
290 |
|
|
// test the shortcut |
291 |
|
✗ |
Data data_shortcut(model); |
292 |
|
✗ |
computeABADerivatives(model, data_shortcut, q, v, tau, fext); |
293 |
|
✗ |
BOOST_CHECK(data_shortcut.ddq_dq.isApprox(aba_partial_dq)); |
294 |
|
✗ |
BOOST_CHECK(data_shortcut.ddq_dv.isApprox(aba_partial_dv)); |
295 |
|
✗ |
BOOST_CHECK(data_shortcut.Minv.isApprox(aba_partial_dtau)); |
296 |
|
|
} |
297 |
|
|
|
298 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_multiple_calls) |
299 |
|
|
{ |
300 |
|
|
using namespace Eigen; |
301 |
|
|
using namespace pinocchio; |
302 |
|
|
|
303 |
|
✗ |
Model model; |
304 |
|
✗ |
buildModels::humanoidRandom(model); |
305 |
|
|
|
306 |
|
✗ |
Data data1(model), data2(model); |
307 |
|
|
|
308 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
309 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
310 |
|
✗ |
VectorXd q = randomConfiguration(model); |
311 |
|
✗ |
VectorXd v(VectorXd::Random(model.nv)); |
312 |
|
✗ |
VectorXd tau(VectorXd::Random(model.nv)); |
313 |
|
|
|
314 |
|
✗ |
computeABADerivatives(model, data1, q, v, tau); |
315 |
|
✗ |
data2 = data1; |
316 |
|
|
|
317 |
|
✗ |
for (int k = 0; k < 20; ++k) |
318 |
|
|
{ |
319 |
|
✗ |
computeABADerivatives(model, data1, q, v, tau); |
320 |
|
|
} |
321 |
|
|
|
322 |
|
✗ |
BOOST_CHECK(data1.J.isApprox(data2.J)); |
323 |
|
✗ |
BOOST_CHECK(data1.dJ.isApprox(data2.dJ)); |
324 |
|
✗ |
BOOST_CHECK(data1.dVdq.isApprox(data2.dVdq)); |
325 |
|
✗ |
BOOST_CHECK(data1.dAdq.isApprox(data2.dAdq)); |
326 |
|
✗ |
BOOST_CHECK(data1.dAdv.isApprox(data2.dAdv)); |
327 |
|
|
|
328 |
|
✗ |
BOOST_CHECK(data1.dFdq.isApprox(data2.dFdq)); |
329 |
|
✗ |
BOOST_CHECK(data1.dFdv.isApprox(data2.dFdv)); |
330 |
|
|
|
331 |
|
✗ |
BOOST_CHECK(data1.dtau_dq.isApprox(data2.dtau_dq)); |
332 |
|
✗ |
BOOST_CHECK(data1.dtau_dv.isApprox(data2.dtau_dv)); |
333 |
|
|
|
334 |
|
✗ |
BOOST_CHECK(data1.ddq_dq.isApprox(data2.ddq_dq)); |
335 |
|
✗ |
BOOST_CHECK(data1.ddq_dv.isApprox(data2.ddq_dv)); |
336 |
|
✗ |
BOOST_CHECK(data1.Minv.isApprox(data2.Minv)); |
337 |
|
|
} |
338 |
|
|
|
339 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_aba_derivatives_vs_kinematics_derivatives) |
340 |
|
|
{ |
341 |
|
|
using namespace Eigen; |
342 |
|
|
using namespace pinocchio; |
343 |
|
|
|
344 |
|
✗ |
Model model; |
345 |
|
✗ |
buildModels::humanoidRandom(model); |
346 |
|
|
|
347 |
|
✗ |
Data data(model), data_ref(model); |
348 |
|
|
|
349 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
350 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
351 |
|
✗ |
VectorXd q = randomConfiguration(model); |
352 |
|
✗ |
VectorXd v(VectorXd::Random(model.nv)); |
353 |
|
✗ |
VectorXd a(VectorXd::Random(model.nv)); |
354 |
|
|
|
355 |
|
✗ |
VectorXd tau = rnea(model, data_ref, q, v, a); |
356 |
|
|
|
357 |
|
✗ |
MatrixXd aba_partial_dq(model.nv, model.nv); |
358 |
|
✗ |
aba_partial_dq.setZero(); |
359 |
|
✗ |
MatrixXd aba_partial_dv(model.nv, model.nv); |
360 |
|
✗ |
aba_partial_dv.setZero(); |
361 |
|
✗ |
MatrixXd aba_partial_dtau(model.nv, model.nv); |
362 |
|
✗ |
aba_partial_dtau.setZero(); |
363 |
|
|
|
364 |
|
✗ |
computeABADerivatives(model, data, q, v, tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau); |
365 |
|
✗ |
computeForwardKinematicsDerivatives(model, data_ref, q, v, a); |
366 |
|
|
|
367 |
|
✗ |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
368 |
|
✗ |
BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); |
369 |
|
|
|
370 |
|
✗ |
for (size_t k = 1; k < (size_t)model.njoints; ++k) |
371 |
|
|
{ |
372 |
|
✗ |
BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k])); |
373 |
|
✗ |
BOOST_CHECK(data.ov[k].isApprox(data_ref.ov[k])); |
374 |
|
✗ |
BOOST_CHECK(data.oa[k].isApprox(data_ref.oa[k])); |
375 |
|
|
} |
376 |
|
|
} |
377 |
|
|
|
378 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_optimized_aba_derivatives) |
379 |
|
|
{ |
380 |
|
|
using namespace Eigen; |
381 |
|
|
using namespace pinocchio; |
382 |
|
|
|
383 |
|
✗ |
Model model; |
384 |
|
✗ |
buildModels::humanoidRandom(model); |
385 |
|
|
|
386 |
|
✗ |
Data data(model), data_ref(model); |
387 |
|
|
|
388 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
389 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
390 |
|
✗ |
VectorXd q = randomConfiguration(model); |
391 |
|
✗ |
VectorXd v(VectorXd::Random(model.nv)); |
392 |
|
✗ |
VectorXd tau(VectorXd::Random(model.nv)); |
393 |
|
|
|
394 |
|
✗ |
MatrixXd aba_partial_dq(model.nv, model.nv); |
395 |
|
✗ |
aba_partial_dq.setZero(); |
396 |
|
✗ |
MatrixXd aba_partial_dv(model.nv, model.nv); |
397 |
|
✗ |
aba_partial_dv.setZero(); |
398 |
|
✗ |
MatrixXd aba_partial_dtau(model.nv, model.nv); |
399 |
|
✗ |
aba_partial_dtau.setZero(); |
400 |
|
|
|
401 |
|
✗ |
aba(model, data, q, v, tau, Convention::WORLD); |
402 |
|
✗ |
computeABADerivatives(model, data, aba_partial_dq, aba_partial_dv, aba_partial_dtau); |
403 |
|
|
|
404 |
|
✗ |
MatrixXd aba_partial_dq_ref(model.nv, model.nv); |
405 |
|
✗ |
aba_partial_dq_ref.setZero(); |
406 |
|
✗ |
MatrixXd aba_partial_dv_ref(model.nv, model.nv); |
407 |
|
✗ |
aba_partial_dv_ref.setZero(); |
408 |
|
✗ |
MatrixXd aba_partial_dtau_ref(model.nv, model.nv); |
409 |
|
✗ |
aba_partial_dtau_ref.setZero(); |
410 |
|
|
|
411 |
|
✗ |
computeABADerivatives( |
412 |
|
|
model, data_ref, q, v, tau, aba_partial_dq_ref, aba_partial_dv_ref, aba_partial_dtau_ref); |
413 |
|
|
|
414 |
|
✗ |
BOOST_CHECK(data.ddq.isApprox(data_ref.ddq)); |
415 |
|
✗ |
BOOST_CHECK(data.Minv.isApprox(data_ref.Minv)); |
416 |
|
|
|
417 |
|
✗ |
BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_ref)); |
418 |
|
✗ |
BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_ref)); |
419 |
|
✗ |
BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_ref)); |
420 |
|
|
|
421 |
|
|
// Test multiple calls |
422 |
|
✗ |
const int num_calls = 20; |
423 |
|
✗ |
aba(model, data, q, v, tau, Convention::WORLD); |
424 |
|
✗ |
for (int it = 0; it < num_calls; ++it) |
425 |
|
|
{ |
426 |
|
✗ |
computeABADerivatives(model, data, aba_partial_dq, aba_partial_dv, aba_partial_dtau); |
427 |
|
|
|
428 |
|
✗ |
BOOST_CHECK(data.ddq.isApprox(data_ref.ddq)); |
429 |
|
✗ |
BOOST_CHECK(data.Minv.isApprox(data_ref.Minv)); |
430 |
|
|
|
431 |
|
✗ |
for (size_t joint_id = 1; joint_id < model.joints.size(); ++joint_id) |
432 |
|
|
{ |
433 |
|
✗ |
BOOST_CHECK(data.oMi[joint_id].isApprox(data_ref.oMi[joint_id])); |
434 |
|
✗ |
BOOST_CHECK(data.ov[joint_id].isApprox(data_ref.ov[joint_id])); |
435 |
|
✗ |
BOOST_CHECK(data.oa[joint_id].isApprox(data_ref.oa[joint_id])); |
436 |
|
✗ |
BOOST_CHECK(data.oa_gf[joint_id].isApprox(data_ref.oa_gf[joint_id])); |
437 |
|
✗ |
BOOST_CHECK(data.of[joint_id].isApprox(data_ref.of[joint_id])); |
438 |
|
✗ |
BOOST_CHECK(data.oh[joint_id].isApprox(data_ref.oh[joint_id])); |
439 |
|
✗ |
BOOST_CHECK(data.oYaba[joint_id].isApprox(data_ref.oYaba[joint_id])); |
440 |
|
✗ |
BOOST_CHECK(data.oYcrb[joint_id].isApprox(data_ref.oYcrb[joint_id])); |
441 |
|
|
} |
442 |
|
|
|
443 |
|
✗ |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
444 |
|
✗ |
BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); |
445 |
|
✗ |
BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq)); |
446 |
|
✗ |
BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq)); |
447 |
|
✗ |
BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv)); |
448 |
|
✗ |
BOOST_CHECK(data.dFdq.isApprox(data_ref.dFdq)); |
449 |
|
✗ |
BOOST_CHECK(data.dFdv.isApprox(data_ref.dFdv)); |
450 |
|
✗ |
BOOST_CHECK(data.dFda.isApprox(data_ref.dFda)); |
451 |
|
|
|
452 |
|
✗ |
BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_ref)); |
453 |
|
✗ |
BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_ref)); |
454 |
|
✗ |
BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_ref)); |
455 |
|
|
} |
456 |
|
|
} |
457 |
|
|
|
458 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_optimized_aba_derivatives_fext) |
459 |
|
|
{ |
460 |
|
|
using namespace Eigen; |
461 |
|
|
using namespace pinocchio; |
462 |
|
|
|
463 |
|
✗ |
Model model; |
464 |
|
✗ |
buildModels::humanoidRandom(model); |
465 |
|
|
|
466 |
|
✗ |
Data data(model), data_ref(model); |
467 |
|
|
|
468 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
469 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
470 |
|
✗ |
VectorXd q = randomConfiguration(model); |
471 |
|
✗ |
VectorXd v(VectorXd::Random(model.nv)); |
472 |
|
✗ |
VectorXd tau(VectorXd::Random(model.nv)); |
473 |
|
|
|
474 |
|
|
typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector; |
475 |
|
✗ |
ForceVector fext((size_t)model.njoints); |
476 |
|
✗ |
for (ForceVector::iterator it = fext.begin(); it != fext.end(); ++it) |
477 |
|
✗ |
(*it).setRandom(); |
478 |
|
|
|
479 |
|
✗ |
MatrixXd aba_partial_dq(model.nv, model.nv); |
480 |
|
✗ |
aba_partial_dq.setZero(); |
481 |
|
✗ |
MatrixXd aba_partial_dv(model.nv, model.nv); |
482 |
|
✗ |
aba_partial_dv.setZero(); |
483 |
|
✗ |
MatrixXd aba_partial_dtau(model.nv, model.nv); |
484 |
|
✗ |
aba_partial_dtau.setZero(); |
485 |
|
|
|
486 |
|
✗ |
aba(model, data, q, v, tau, fext, Convention::WORLD); |
487 |
|
✗ |
computeABADerivatives(model, data, fext, aba_partial_dq, aba_partial_dv, aba_partial_dtau); |
488 |
|
|
|
489 |
|
✗ |
MatrixXd aba_partial_dq_ref(model.nv, model.nv); |
490 |
|
✗ |
aba_partial_dq_ref.setZero(); |
491 |
|
✗ |
MatrixXd aba_partial_dv_ref(model.nv, model.nv); |
492 |
|
✗ |
aba_partial_dv_ref.setZero(); |
493 |
|
✗ |
MatrixXd aba_partial_dtau_ref(model.nv, model.nv); |
494 |
|
✗ |
aba_partial_dtau_ref.setZero(); |
495 |
|
|
|
496 |
|
✗ |
computeABADerivatives( |
497 |
|
|
model, data_ref, q, v, tau, fext, aba_partial_dq_ref, aba_partial_dv_ref, aba_partial_dtau_ref); |
498 |
|
|
|
499 |
|
✗ |
BOOST_CHECK(data.ddq.isApprox(data_ref.ddq)); |
500 |
|
✗ |
BOOST_CHECK(data.Minv.isApprox(data_ref.Minv)); |
501 |
|
|
|
502 |
|
✗ |
BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_ref)); |
503 |
|
✗ |
BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_ref)); |
504 |
|
✗ |
BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_ref)); |
505 |
|
|
|
506 |
|
|
// Test multiple calls |
507 |
|
✗ |
const int num_calls = 20; |
508 |
|
✗ |
aba(model, data, q, v, tau, fext, Convention::WORLD); |
509 |
|
✗ |
for (int it = 0; it < num_calls; ++it) |
510 |
|
|
{ |
511 |
|
✗ |
computeABADerivatives(model, data, fext, aba_partial_dq, aba_partial_dv, aba_partial_dtau); |
512 |
|
|
|
513 |
|
✗ |
BOOST_CHECK(data.ddq.isApprox(data_ref.ddq)); |
514 |
|
✗ |
BOOST_CHECK(data.Minv.isApprox(data_ref.Minv)); |
515 |
|
|
|
516 |
|
✗ |
for (size_t joint_id = 1; joint_id < model.joints.size(); ++joint_id) |
517 |
|
|
{ |
518 |
|
✗ |
BOOST_CHECK(data.oMi[joint_id].isApprox(data_ref.oMi[joint_id])); |
519 |
|
✗ |
BOOST_CHECK(data.ov[joint_id].isApprox(data_ref.ov[joint_id])); |
520 |
|
✗ |
BOOST_CHECK(data.oa[joint_id].isApprox(data_ref.oa[joint_id])); |
521 |
|
✗ |
BOOST_CHECK(data.oa_gf[joint_id].isApprox(data_ref.oa_gf[joint_id])); |
522 |
|
✗ |
BOOST_CHECK(data.of[joint_id].isApprox(data_ref.of[joint_id])); |
523 |
|
✗ |
BOOST_CHECK(data.oh[joint_id].isApprox(data_ref.oh[joint_id])); |
524 |
|
✗ |
BOOST_CHECK(data.oYaba[joint_id].isApprox(data_ref.oYaba[joint_id])); |
525 |
|
✗ |
BOOST_CHECK(data.oYcrb[joint_id].isApprox(data_ref.oYcrb[joint_id])); |
526 |
|
|
} |
527 |
|
|
|
528 |
|
✗ |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
529 |
|
✗ |
BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); |
530 |
|
✗ |
BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq)); |
531 |
|
✗ |
BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq)); |
532 |
|
✗ |
BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv)); |
533 |
|
✗ |
BOOST_CHECK(data.dFdq.isApprox(data_ref.dFdq)); |
534 |
|
✗ |
BOOST_CHECK(data.dFdv.isApprox(data_ref.dFdv)); |
535 |
|
✗ |
BOOST_CHECK(data.dFda.isApprox(data_ref.dFda)); |
536 |
|
|
|
537 |
|
✗ |
BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_ref)); |
538 |
|
✗ |
BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_ref)); |
539 |
|
✗ |
BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_ref)); |
540 |
|
|
} |
541 |
|
|
} |
542 |
|
|
|
543 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
544 |
|
|
|