Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2017-2020 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include <iostream> |
6 |
|
|
|
7 |
|
|
#include "pinocchio/multibody/model.hpp" |
8 |
|
|
#include "pinocchio/multibody/data.hpp" |
9 |
|
|
#include "pinocchio/algorithm/jacobian.hpp" |
10 |
|
|
#include "pinocchio/algorithm/joint-configuration.hpp" |
11 |
|
|
#include "pinocchio/algorithm/kinematics.hpp" |
12 |
|
|
#include "pinocchio/algorithm/kinematics-derivatives.hpp" |
13 |
|
|
#include "pinocchio/multibody/sample-models.hpp" |
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_kinematics_derivatives_all) |
21 |
|
|
{ |
22 |
|
|
using namespace Eigen; |
23 |
|
|
using namespace pinocchio; |
24 |
|
|
|
25 |
|
✗ |
Model model; |
26 |
|
✗ |
buildModels::humanoidRandom(model); |
27 |
|
|
|
28 |
|
✗ |
Data data(model), data_ref(model); |
29 |
|
|
|
30 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
31 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
32 |
|
✗ |
VectorXd q = randomConfiguration(model); |
33 |
|
✗ |
VectorXd v(VectorXd::Random(model.nv)); |
34 |
|
✗ |
VectorXd a(VectorXd::Random(model.nv)); |
35 |
|
|
|
36 |
|
✗ |
forwardKinematics(model, data_ref, q, v, a); |
37 |
|
✗ |
computeForwardKinematicsDerivatives(model, data, q, v, a); |
38 |
|
|
|
39 |
|
✗ |
for (size_t i = 1; i < (size_t)model.njoints; ++i) |
40 |
|
|
{ |
41 |
|
✗ |
BOOST_CHECK(data.oMi[i].isApprox(data_ref.oMi[i])); |
42 |
|
✗ |
BOOST_CHECK(data.v[i].isApprox(data_ref.v[i])); |
43 |
|
✗ |
BOOST_CHECK(data.ov[i].isApprox(data_ref.oMi[i].act(data_ref.v[i]))); |
44 |
|
✗ |
BOOST_CHECK(data.a[i].isApprox(data_ref.a[i])); |
45 |
|
✗ |
BOOST_CHECK(data.oa[i].isApprox(data_ref.oMi[i].act(data_ref.a[i]))); |
46 |
|
|
} |
47 |
|
|
|
48 |
|
✗ |
computeJointJacobians(model, data_ref, q); |
49 |
|
✗ |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
50 |
|
|
|
51 |
|
✗ |
computeJointJacobiansTimeVariation(model, data_ref, q, v); |
52 |
|
✗ |
BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); |
53 |
|
|
} |
54 |
|
|
|
55 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_velocity) |
56 |
|
|
{ |
57 |
|
|
using namespace Eigen; |
58 |
|
|
using namespace pinocchio; |
59 |
|
|
|
60 |
|
✗ |
Model model; |
61 |
|
✗ |
buildModels::humanoidRandom(model); |
62 |
|
|
|
63 |
|
✗ |
Data data(model), data_ref(model); |
64 |
|
|
|
65 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
66 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
67 |
|
✗ |
VectorXd q = randomConfiguration(model); |
68 |
|
✗ |
VectorXd v(VectorXd::Random(model.nv)); |
69 |
|
✗ |
VectorXd a(VectorXd::Random(model.nv)); |
70 |
|
|
|
71 |
|
✗ |
computeForwardKinematicsDerivatives(model, data, q, v, a); |
72 |
|
|
|
73 |
|
✗ |
const Model::JointIndex jointId = model.existJointName("rarm2_joint") |
74 |
|
✗ |
? model.getJointId("rarm2_joint") |
75 |
|
✗ |
: (Model::Index)(model.njoints - 1); |
76 |
|
✗ |
Data::Matrix6x partial_dq(6, model.nv); |
77 |
|
✗ |
partial_dq.setZero(); |
78 |
|
✗ |
Data::Matrix6x partial_dq_local_world_aligned(6, model.nv); |
79 |
|
✗ |
partial_dq_local_world_aligned.setZero(); |
80 |
|
✗ |
Data::Matrix6x partial_dq_local(6, model.nv); |
81 |
|
✗ |
partial_dq_local.setZero(); |
82 |
|
✗ |
Data::Matrix6x partial_dv(6, model.nv); |
83 |
|
✗ |
partial_dv.setZero(); |
84 |
|
✗ |
Data::Matrix6x partial_dv_local_world_aligned(6, model.nv); |
85 |
|
✗ |
partial_dv_local_world_aligned.setZero(); |
86 |
|
✗ |
Data::Matrix6x partial_dv_local(6, model.nv); |
87 |
|
✗ |
partial_dv_local.setZero(); |
88 |
|
|
|
89 |
|
✗ |
getJointVelocityDerivatives(model, data, jointId, WORLD, partial_dq, partial_dv); |
90 |
|
|
|
91 |
|
✗ |
getJointVelocityDerivatives( |
92 |
|
|
model, data, jointId, LOCAL_WORLD_ALIGNED, partial_dq_local_world_aligned, |
93 |
|
|
partial_dv_local_world_aligned); |
94 |
|
|
|
95 |
|
✗ |
getJointVelocityDerivatives(model, data, jointId, LOCAL, partial_dq_local, partial_dv_local); |
96 |
|
|
|
97 |
|
✗ |
Data::Matrix6x J_ref(6, model.nv); |
98 |
|
✗ |
J_ref.setZero(); |
99 |
|
✗ |
Data::Matrix6x J_ref_local_world_aligned(6, model.nv); |
100 |
|
✗ |
J_ref_local_world_aligned.setZero(); |
101 |
|
✗ |
Data::Matrix6x J_ref_local(6, model.nv); |
102 |
|
✗ |
J_ref_local.setZero(); |
103 |
|
✗ |
computeJointJacobians(model, data_ref, q); |
104 |
|
✗ |
getJointJacobian(model, data_ref, jointId, WORLD, J_ref); |
105 |
|
✗ |
getJointJacobian(model, data_ref, jointId, LOCAL_WORLD_ALIGNED, J_ref_local_world_aligned); |
106 |
|
✗ |
getJointJacobian(model, data_ref, jointId, LOCAL, J_ref_local); |
107 |
|
|
|
108 |
|
✗ |
BOOST_CHECK(partial_dv.isApprox(J_ref)); |
109 |
|
✗ |
BOOST_CHECK(partial_dv_local_world_aligned.isApprox(J_ref_local_world_aligned)); |
110 |
|
✗ |
BOOST_CHECK(partial_dv_local.isApprox(J_ref_local)); |
111 |
|
|
|
112 |
|
|
// Check against finite differences |
113 |
|
✗ |
Data::Matrix6x partial_dq_fd(6, model.nv); |
114 |
|
✗ |
partial_dq_fd.setZero(); |
115 |
|
✗ |
Data::Matrix6x partial_dq_fd_local_world_aligned(6, model.nv); |
116 |
|
✗ |
partial_dq_fd_local_world_aligned.setZero(); |
117 |
|
✗ |
Data::Matrix6x partial_dq_fd_local(6, model.nv); |
118 |
|
✗ |
partial_dq_fd_local.setZero(); |
119 |
|
✗ |
Data::Matrix6x partial_dv_fd(6, model.nv); |
120 |
|
✗ |
partial_dv_fd.setZero(); |
121 |
|
✗ |
Data::Matrix6x partial_dv_fd_local_world_aligned(6, model.nv); |
122 |
|
✗ |
partial_dv_fd_local_world_aligned.setZero(); |
123 |
|
✗ |
Data::Matrix6x partial_dv_fd_local(6, model.nv); |
124 |
|
✗ |
partial_dv_fd_local.setZero(); |
125 |
|
✗ |
const double alpha = 1e-8; |
126 |
|
|
|
127 |
|
|
// dvel/dv |
128 |
|
✗ |
Eigen::VectorXd v_plus(v); |
129 |
|
✗ |
Data data_plus(model); |
130 |
|
✗ |
forwardKinematics(model, data_ref, q, v); |
131 |
|
✗ |
SE3 oMi_rot(SE3::Identity()); |
132 |
|
✗ |
oMi_rot.rotation() = data_ref.oMi[jointId].rotation(); |
133 |
|
✗ |
Motion v0(data_ref.oMi[jointId].act(data_ref.v[jointId])); |
134 |
|
✗ |
Motion v0_local_world_aligned(oMi_rot.act(data_ref.v[jointId])); |
135 |
|
✗ |
Motion v0_local(data_ref.v[jointId]); |
136 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
137 |
|
|
{ |
138 |
|
✗ |
v_plus[k] += alpha; |
139 |
|
✗ |
forwardKinematics(model, data_plus, q, v_plus); |
140 |
|
|
|
141 |
|
✗ |
partial_dv_fd.col(k) = |
142 |
|
✗ |
(data_plus.oMi[jointId].act(data_plus.v[jointId]) - v0).toVector() / alpha; |
143 |
|
✗ |
partial_dv_fd_local_world_aligned.col(k) = |
144 |
|
✗ |
(oMi_rot.act(data_plus.v[jointId]) - v0_local_world_aligned).toVector() / alpha; |
145 |
|
✗ |
partial_dv_fd_local.col(k) = (data_plus.v[jointId] - v0_local).toVector() / alpha; |
146 |
|
✗ |
v_plus[k] -= alpha; |
147 |
|
|
} |
148 |
|
|
|
149 |
|
✗ |
BOOST_CHECK(partial_dv.isApprox(partial_dv_fd, sqrt(alpha))); |
150 |
|
✗ |
BOOST_CHECK( |
151 |
|
|
partial_dv_local_world_aligned.isApprox(partial_dv_fd_local_world_aligned, sqrt(alpha))); |
152 |
|
✗ |
BOOST_CHECK(partial_dv_local.isApprox(partial_dv_fd_local, sqrt(alpha))); |
153 |
|
|
|
154 |
|
|
// dvel/dq |
155 |
|
✗ |
Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv)); |
156 |
|
✗ |
forwardKinematics(model, data_ref, q, v); |
157 |
|
✗ |
v0 = data_ref.oMi[jointId].act(data_ref.v[jointId]); |
158 |
|
|
|
159 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
160 |
|
|
{ |
161 |
|
✗ |
v_eps[k] += alpha; |
162 |
|
✗ |
q_plus = integrate(model, q, v_eps); |
163 |
|
✗ |
forwardKinematics(model, data_plus, q_plus, v); |
164 |
|
|
|
165 |
|
✗ |
SE3 oMi_plus_rot = data_plus.oMi[jointId]; |
166 |
|
✗ |
oMi_plus_rot.translation().setZero(); |
167 |
|
|
|
168 |
|
✗ |
Motion v_plus_local_world_aligned = oMi_plus_rot.act(data_plus.v[jointId]); |
169 |
|
✗ |
SE3::Vector3 trans = data_plus.oMi[jointId].translation() - data_ref.oMi[jointId].translation(); |
170 |
|
✗ |
v_plus_local_world_aligned.linear() -= v_plus_local_world_aligned.angular().cross(trans); |
171 |
|
✗ |
partial_dq_fd.col(k) = |
172 |
|
✗ |
(data_plus.oMi[jointId].act(data_plus.v[jointId]) - v0).toVector() / alpha; |
173 |
|
✗ |
partial_dq_fd_local_world_aligned.col(k) = |
174 |
|
✗ |
(v_plus_local_world_aligned - v0_local_world_aligned).toVector() / alpha; |
175 |
|
✗ |
partial_dq_fd_local.col(k) = (data_plus.v[jointId] - v0_local).toVector() / alpha; |
176 |
|
✗ |
v_eps[k] -= alpha; |
177 |
|
|
} |
178 |
|
|
|
179 |
|
✗ |
BOOST_CHECK(partial_dq.isApprox(partial_dq_fd, sqrt(alpha))); |
180 |
|
✗ |
BOOST_CHECK( |
181 |
|
|
partial_dq_local_world_aligned.isApprox(partial_dq_fd_local_world_aligned, sqrt(alpha))); |
182 |
|
✗ |
BOOST_CHECK(partial_dq_local.isApprox(partial_dq_fd_local, sqrt(alpha))); |
183 |
|
|
} |
184 |
|
|
|
185 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_acceleration) |
186 |
|
|
{ |
187 |
|
|
using namespace Eigen; |
188 |
|
|
using namespace pinocchio; |
189 |
|
|
|
190 |
|
✗ |
Model model; |
191 |
|
✗ |
buildModels::humanoidRandom(model); |
192 |
|
|
|
193 |
|
✗ |
Data data(model), data_ref(model); |
194 |
|
|
|
195 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
196 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
197 |
|
✗ |
VectorXd q = randomConfiguration(model); |
198 |
|
✗ |
VectorXd v(VectorXd::Random(model.nv)); |
199 |
|
✗ |
VectorXd a(VectorXd::Random(model.nv)); |
200 |
|
|
|
201 |
|
✗ |
computeForwardKinematicsDerivatives(model, data, q, v, a); |
202 |
|
|
|
203 |
|
✗ |
const Model::JointIndex jointId = model.existJointName("rarm2_joint") |
204 |
|
✗ |
? model.getJointId("rarm2_joint") |
205 |
|
✗ |
: (Model::Index)(model.njoints - 1); |
206 |
|
|
|
207 |
|
✗ |
Data::Matrix6x v_partial_dq(6, model.nv); |
208 |
|
✗ |
v_partial_dq.setZero(); |
209 |
|
✗ |
Data::Matrix6x v_partial_dq_local(6, model.nv); |
210 |
|
✗ |
v_partial_dq_local.setZero(); |
211 |
|
✗ |
Data::Matrix6x v_partial_dq_local_world_aligned(6, model.nv); |
212 |
|
✗ |
v_partial_dq_local_world_aligned.setZero(); |
213 |
|
✗ |
Data::Matrix6x a_partial_dq(6, model.nv); |
214 |
|
✗ |
a_partial_dq.setZero(); |
215 |
|
✗ |
Data::Matrix6x a_partial_dq_local_world_aligned(6, model.nv); |
216 |
|
✗ |
a_partial_dq_local_world_aligned.setZero(); |
217 |
|
✗ |
Data::Matrix6x a_partial_dq_local(6, model.nv); |
218 |
|
✗ |
a_partial_dq_local.setZero(); |
219 |
|
✗ |
Data::Matrix6x a_partial_dv(6, model.nv); |
220 |
|
✗ |
a_partial_dv.setZero(); |
221 |
|
✗ |
Data::Matrix6x a_partial_dv_local_world_aligned(6, model.nv); |
222 |
|
✗ |
a_partial_dv_local_world_aligned.setZero(); |
223 |
|
✗ |
Data::Matrix6x a_partial_dv_local(6, model.nv); |
224 |
|
✗ |
a_partial_dv_local.setZero(); |
225 |
|
✗ |
Data::Matrix6x a_partial_da(6, model.nv); |
226 |
|
✗ |
a_partial_da.setZero(); |
227 |
|
✗ |
Data::Matrix6x a_partial_da_local_world_aligned(6, model.nv); |
228 |
|
✗ |
a_partial_da_local_world_aligned.setZero(); |
229 |
|
✗ |
Data::Matrix6x a_partial_da_local(6, model.nv); |
230 |
|
✗ |
a_partial_da_local.setZero(); |
231 |
|
|
|
232 |
|
✗ |
getJointAccelerationDerivatives( |
233 |
|
|
model, data, jointId, WORLD, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da); |
234 |
|
|
|
235 |
|
✗ |
getJointAccelerationDerivatives( |
236 |
|
|
model, data, jointId, LOCAL_WORLD_ALIGNED, v_partial_dq_local_world_aligned, |
237 |
|
|
a_partial_dq_local_world_aligned, a_partial_dv_local_world_aligned, |
238 |
|
|
a_partial_da_local_world_aligned); |
239 |
|
|
|
240 |
|
✗ |
getJointAccelerationDerivatives( |
241 |
|
|
model, data, jointId, LOCAL, v_partial_dq_local, a_partial_dq_local, a_partial_dv_local, |
242 |
|
|
a_partial_da_local); |
243 |
|
|
|
244 |
|
|
// Check v_partial_dq against getJointVelocityDerivatives |
245 |
|
|
{ |
246 |
|
✗ |
Data data_v(model); |
247 |
|
✗ |
computeForwardKinematicsDerivatives(model, data_v, q, v, a); |
248 |
|
|
|
249 |
|
✗ |
Data::Matrix6x v_partial_dq_ref(6, model.nv); |
250 |
|
✗ |
v_partial_dq_ref.setZero(); |
251 |
|
✗ |
Data::Matrix6x v_partial_dq_ref_local_world_aligned(6, model.nv); |
252 |
|
✗ |
v_partial_dq_ref_local_world_aligned.setZero(); |
253 |
|
✗ |
Data::Matrix6x v_partial_dq_ref_local(6, model.nv); |
254 |
|
✗ |
v_partial_dq_ref_local.setZero(); |
255 |
|
✗ |
Data::Matrix6x v_partial_dv_ref(6, model.nv); |
256 |
|
✗ |
v_partial_dv_ref.setZero(); |
257 |
|
✗ |
Data::Matrix6x v_partial_dv_ref_local_world_aligned(6, model.nv); |
258 |
|
✗ |
v_partial_dv_ref_local_world_aligned.setZero(); |
259 |
|
✗ |
Data::Matrix6x v_partial_dv_ref_local(6, model.nv); |
260 |
|
✗ |
v_partial_dv_ref_local.setZero(); |
261 |
|
|
|
262 |
|
✗ |
getJointVelocityDerivatives(model, data_v, jointId, WORLD, v_partial_dq_ref, v_partial_dv_ref); |
263 |
|
|
|
264 |
|
✗ |
BOOST_CHECK(v_partial_dq.isApprox(v_partial_dq_ref)); |
265 |
|
✗ |
BOOST_CHECK(a_partial_da.isApprox(v_partial_dv_ref)); |
266 |
|
|
|
267 |
|
✗ |
getJointVelocityDerivatives( |
268 |
|
|
model, data_v, jointId, LOCAL_WORLD_ALIGNED, v_partial_dq_ref_local_world_aligned, |
269 |
|
|
v_partial_dv_ref_local_world_aligned); |
270 |
|
|
|
271 |
|
✗ |
BOOST_CHECK(v_partial_dq_local_world_aligned.isApprox(v_partial_dq_ref_local_world_aligned)); |
272 |
|
✗ |
BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(v_partial_dv_ref_local_world_aligned)); |
273 |
|
|
|
274 |
|
✗ |
getJointVelocityDerivatives( |
275 |
|
|
model, data_v, jointId, LOCAL, v_partial_dq_ref_local, v_partial_dv_ref_local); |
276 |
|
|
|
277 |
|
✗ |
BOOST_CHECK(v_partial_dq_local.isApprox(v_partial_dq_ref_local)); |
278 |
|
✗ |
BOOST_CHECK(a_partial_da_local.isApprox(v_partial_dv_ref_local)); |
279 |
|
|
} |
280 |
|
|
|
281 |
|
✗ |
Data::Matrix6x J_ref(6, model.nv); |
282 |
|
✗ |
J_ref.setZero(); |
283 |
|
✗ |
Data::Matrix6x J_ref_local(6, model.nv); |
284 |
|
✗ |
J_ref_local.setZero(); |
285 |
|
✗ |
Data::Matrix6x J_ref_local_world_aligned(6, model.nv); |
286 |
|
✗ |
J_ref_local_world_aligned.setZero(); |
287 |
|
✗ |
computeJointJacobians(model, data_ref, q); |
288 |
|
✗ |
getJointJacobian(model, data_ref, jointId, WORLD, J_ref); |
289 |
|
✗ |
getJointJacobian(model, data_ref, jointId, LOCAL_WORLD_ALIGNED, J_ref_local_world_aligned); |
290 |
|
✗ |
getJointJacobian(model, data_ref, jointId, LOCAL, J_ref_local); |
291 |
|
|
|
292 |
|
✗ |
BOOST_CHECK(a_partial_da.isApprox(J_ref)); |
293 |
|
✗ |
BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(J_ref_local_world_aligned)); |
294 |
|
✗ |
BOOST_CHECK(a_partial_da_local.isApprox(J_ref_local)); |
295 |
|
|
|
296 |
|
|
// Check against finite differences |
297 |
|
✗ |
Data::Matrix6x a_partial_da_fd(6, model.nv); |
298 |
|
✗ |
a_partial_da_fd.setZero(); |
299 |
|
✗ |
Data::Matrix6x a_partial_da_fd_local_world_aligned(6, model.nv); |
300 |
|
✗ |
a_partial_da_fd_local_world_aligned.setZero(); |
301 |
|
✗ |
Data::Matrix6x a_partial_da_fd_local(6, model.nv); |
302 |
|
✗ |
a_partial_da_fd_local.setZero(); |
303 |
|
✗ |
const double alpha = 1e-8; |
304 |
|
|
|
305 |
|
✗ |
Eigen::VectorXd v_plus(v), a_plus(a); |
306 |
|
✗ |
Data data_plus(model); |
307 |
|
✗ |
forwardKinematics(model, data_ref, q, v, a); |
308 |
|
✗ |
SE3 oMi_rot(SE3::Identity()); |
309 |
|
✗ |
oMi_rot.rotation() = data_ref.oMi[jointId].rotation(); |
310 |
|
|
|
311 |
|
|
// dacc/da |
312 |
|
✗ |
Motion a0(data_ref.oMi[jointId].act(data_ref.a[jointId])); |
313 |
|
✗ |
Motion a0_local_world_aligned(oMi_rot.act(data_ref.a[jointId])); |
314 |
|
✗ |
Motion a0_local(data_ref.a[jointId]); |
315 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
316 |
|
|
{ |
317 |
|
✗ |
a_plus[k] += alpha; |
318 |
|
✗ |
forwardKinematics(model, data_plus, q, v, a_plus); |
319 |
|
|
|
320 |
|
✗ |
a_partial_da_fd.col(k) = |
321 |
|
✗ |
(data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector() / alpha; |
322 |
|
✗ |
a_partial_da_fd_local_world_aligned.col(k) = |
323 |
|
✗ |
(oMi_rot.act(data_plus.a[jointId]) - a0_local_world_aligned).toVector() / alpha; |
324 |
|
✗ |
a_partial_da_fd_local.col(k) = (data_plus.a[jointId] - a0_local).toVector() / alpha; |
325 |
|
✗ |
a_plus[k] -= alpha; |
326 |
|
|
} |
327 |
|
✗ |
BOOST_CHECK(a_partial_da.isApprox(a_partial_da_fd, sqrt(alpha))); |
328 |
|
✗ |
BOOST_CHECK( |
329 |
|
|
a_partial_da_local_world_aligned.isApprox(a_partial_da_fd_local_world_aligned, sqrt(alpha))); |
330 |
|
✗ |
BOOST_CHECK(a_partial_da_local.isApprox(a_partial_da_fd_local, sqrt(alpha))); |
331 |
|
✗ |
motionSet::se3Action(data_ref.oMi[jointId].inverse(), a_partial_da, a_partial_da_local); |
332 |
|
✗ |
BOOST_CHECK(a_partial_da_local.isApprox(a_partial_da_fd_local, sqrt(alpha))); |
333 |
|
|
|
334 |
|
|
// dacc/dv |
335 |
|
✗ |
Data::Matrix6x a_partial_dv_fd(6, model.nv); |
336 |
|
✗ |
a_partial_dv_fd.setZero(); |
337 |
|
✗ |
Data::Matrix6x a_partial_dv_fd_local_world_aligned(6, model.nv); |
338 |
|
✗ |
a_partial_dv_fd_local_world_aligned.setZero(); |
339 |
|
✗ |
Data::Matrix6x a_partial_dv_fd_local(6, model.nv); |
340 |
|
✗ |
a_partial_dv_fd_local.setZero(); |
341 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
342 |
|
|
{ |
343 |
|
✗ |
v_plus[k] += alpha; |
344 |
|
✗ |
forwardKinematics(model, data_plus, q, v_plus, a); |
345 |
|
|
|
346 |
|
✗ |
a_partial_dv_fd.col(k) = |
347 |
|
✗ |
(data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector() / alpha; |
348 |
|
✗ |
a_partial_dv_fd_local_world_aligned.col(k) = |
349 |
|
✗ |
(oMi_rot.act(data_plus.a[jointId]) - a0_local_world_aligned).toVector() / alpha; |
350 |
|
✗ |
a_partial_dv_fd_local.col(k) = (data_plus.a[jointId] - a0_local).toVector() / alpha; |
351 |
|
✗ |
v_plus[k] -= alpha; |
352 |
|
|
} |
353 |
|
|
|
354 |
|
✗ |
BOOST_CHECK(a_partial_dv.isApprox(a_partial_dv_fd, sqrt(alpha))); |
355 |
|
✗ |
BOOST_CHECK( |
356 |
|
|
a_partial_dv_local_world_aligned.isApprox(a_partial_dv_fd_local_world_aligned, sqrt(alpha))); |
357 |
|
✗ |
BOOST_CHECK(a_partial_dv_local.isApprox(a_partial_dv_fd_local, sqrt(alpha))); |
358 |
|
✗ |
motionSet::se3Action(data_ref.oMi[jointId].inverse(), a_partial_dv, a_partial_dv_local); |
359 |
|
✗ |
BOOST_CHECK(a_partial_dv_local.isApprox(a_partial_dv_fd_local, sqrt(alpha))); |
360 |
|
|
|
361 |
|
|
// dacc/dq |
362 |
|
✗ |
a_partial_dq.setZero(); |
363 |
|
✗ |
a_partial_dv.setZero(); |
364 |
|
✗ |
a_partial_da.setZero(); |
365 |
|
|
|
366 |
|
✗ |
a_partial_dq_local_world_aligned.setZero(); |
367 |
|
✗ |
a_partial_dv_local_world_aligned.setZero(); |
368 |
|
✗ |
a_partial_da_local_world_aligned.setZero(); |
369 |
|
|
|
370 |
|
✗ |
a_partial_dq_local.setZero(); |
371 |
|
✗ |
a_partial_dv_local.setZero(); |
372 |
|
✗ |
a_partial_da_local.setZero(); |
373 |
|
|
|
374 |
|
✗ |
Data::Matrix6x a_partial_dq_fd(6, model.nv); |
375 |
|
✗ |
a_partial_dq_fd.setZero(); |
376 |
|
✗ |
Data::Matrix6x a_partial_dq_fd_local_world_aligned(6, model.nv); |
377 |
|
✗ |
a_partial_dq_fd_local_world_aligned.setZero(); |
378 |
|
✗ |
Data::Matrix6x a_partial_dq_fd_local(6, model.nv); |
379 |
|
✗ |
a_partial_dq_fd_local.setZero(); |
380 |
|
|
|
381 |
|
✗ |
computeForwardKinematicsDerivatives(model, data, q, v, a); |
382 |
|
✗ |
getJointAccelerationDerivatives( |
383 |
|
|
model, data, jointId, WORLD, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da); |
384 |
|
|
|
385 |
|
✗ |
getJointAccelerationDerivatives( |
386 |
|
|
model, data, jointId, LOCAL_WORLD_ALIGNED, v_partial_dq_local_world_aligned, |
387 |
|
|
a_partial_dq_local_world_aligned, a_partial_dv_local_world_aligned, |
388 |
|
|
a_partial_da_local_world_aligned); |
389 |
|
|
|
390 |
|
✗ |
getJointAccelerationDerivatives( |
391 |
|
|
model, data, jointId, LOCAL, v_partial_dq_local, a_partial_dq_local, a_partial_dv_local, |
392 |
|
|
a_partial_da_local); |
393 |
|
|
|
394 |
|
✗ |
Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv)); |
395 |
|
✗ |
forwardKinematics(model, data_ref, q, v, a); |
396 |
|
✗ |
a0 = data_ref.oMi[jointId].act(data_ref.a[jointId]); |
397 |
|
✗ |
oMi_rot.rotation() = data_ref.oMi[jointId].rotation(); |
398 |
|
✗ |
a0_local_world_aligned = oMi_rot.act(data_ref.a[jointId]); |
399 |
|
✗ |
a0_local = data_ref.a[jointId]; |
400 |
|
|
|
401 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
402 |
|
|
{ |
403 |
|
✗ |
v_eps[k] += alpha; |
404 |
|
✗ |
q_plus = integrate(model, q, v_eps); |
405 |
|
✗ |
forwardKinematics(model, data_plus, q_plus, v, a); |
406 |
|
|
|
407 |
|
✗ |
SE3 oMi_plus_rot = data_plus.oMi[jointId]; |
408 |
|
✗ |
oMi_plus_rot.translation().setZero(); |
409 |
|
|
|
410 |
|
✗ |
Motion a_plus_local_world_aligned = oMi_plus_rot.act(data_plus.a[jointId]); |
411 |
|
|
const SE3::Vector3 trans = |
412 |
|
✗ |
data_plus.oMi[jointId].translation() - data_ref.oMi[jointId].translation(); |
413 |
|
✗ |
a_plus_local_world_aligned.linear() -= a_plus_local_world_aligned.angular().cross(trans); |
414 |
|
✗ |
a_partial_dq_fd.col(k) = |
415 |
|
✗ |
(data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector() / alpha; |
416 |
|
✗ |
a_partial_dq_fd_local_world_aligned.col(k) = |
417 |
|
✗ |
(a_plus_local_world_aligned - a0_local_world_aligned).toVector() / alpha; |
418 |
|
✗ |
a_partial_dq_fd_local.col(k) = (data_plus.a[jointId] - a0_local).toVector() / alpha; |
419 |
|
✗ |
v_eps[k] -= alpha; |
420 |
|
|
} |
421 |
|
|
|
422 |
|
✗ |
BOOST_CHECK(a_partial_dq.isApprox(a_partial_dq_fd, sqrt(alpha))); |
423 |
|
✗ |
BOOST_CHECK( |
424 |
|
|
a_partial_dq_local_world_aligned.isApprox(a_partial_dq_fd_local_world_aligned, sqrt(alpha))); |
425 |
|
✗ |
BOOST_CHECK(a_partial_dq_local.isApprox(a_partial_dq_fd_local, sqrt(alpha))); |
426 |
|
|
} |
427 |
|
|
|
428 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_against_classic_formula) |
429 |
|
|
{ |
430 |
|
|
using namespace Eigen; |
431 |
|
|
using namespace pinocchio; |
432 |
|
|
|
433 |
|
✗ |
Model model; |
434 |
|
✗ |
buildModels::humanoidRandom(model, true); |
435 |
|
|
|
436 |
|
✗ |
Data data(model), data_ref(model); |
437 |
|
|
|
438 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
439 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
440 |
|
✗ |
VectorXd q = randomConfiguration(model); |
441 |
|
✗ |
VectorXd v(VectorXd::Random(model.nv)); |
442 |
|
✗ |
VectorXd a(VectorXd::Random(model.nv)); |
443 |
|
|
|
444 |
|
✗ |
const Model::JointIndex jointId = model.existJointName("rarm4_joint") |
445 |
|
✗ |
? model.getJointId("rarm4_joint") |
446 |
|
✗ |
: (Model::Index)(model.njoints - 1); |
447 |
|
✗ |
Data::Matrix6x v_partial_dq(6, model.nv); |
448 |
|
✗ |
v_partial_dq.setZero(); |
449 |
|
✗ |
Data::Matrix6x v_partial_dq_ref(6, model.nv); |
450 |
|
✗ |
v_partial_dq_ref.setZero(); |
451 |
|
✗ |
Data::Matrix6x v_partial_dv_ref(6, model.nv); |
452 |
|
✗ |
v_partial_dv_ref.setZero(); |
453 |
|
✗ |
Data::Matrix6x a_partial_dq(6, model.nv); |
454 |
|
✗ |
a_partial_dq.setZero(); |
455 |
|
✗ |
Data::Matrix6x a_partial_dv(6, model.nv); |
456 |
|
✗ |
a_partial_dv.setZero(); |
457 |
|
✗ |
Data::Matrix6x a_partial_da(6, model.nv); |
458 |
|
✗ |
a_partial_da.setZero(); |
459 |
|
|
|
460 |
|
|
// LOCAL: da/dv == dJ/dt + dv/dq |
461 |
|
|
// { |
462 |
|
|
// Data::Matrix6x rhs(6,model.nv); rhs.setZero(); |
463 |
|
|
// |
464 |
|
|
// v_partial_dq.setZero(); |
465 |
|
|
// a_partial_dq.setZero(); |
466 |
|
|
// a_partial_dv.setZero(); |
467 |
|
|
// a_partial_da.setZero(); |
468 |
|
|
// |
469 |
|
|
// computeForwardKinematicsDerivatives(model,data_ref,q,v,a); |
470 |
|
|
// computeForwardKinematicsDerivatives(model,data,q,v,a); |
471 |
|
|
// |
472 |
|
|
// getJointAccelerationDerivatives<LOCAL>(model,data,jointId, |
473 |
|
|
// v_partial_dq, |
474 |
|
|
// a_partial_dq,a_partial_dv,a_partial_da); |
475 |
|
|
// |
476 |
|
|
// getJointJacobianTimeVariation<LOCAL>(model,data_ref,jointId,rhs); |
477 |
|
|
// |
478 |
|
|
// v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero(); |
479 |
|
|
// getJointVelocityDerivatives<LOCAL>(model,data_ref,jointId, |
480 |
|
|
// v_partial_dq_ref,v_partial_dv_ref); |
481 |
|
|
// rhs += v_partial_dq_ref; |
482 |
|
|
// BOOST_CHECK(a_partial_dv.isApprox(rhs,1e-12)); |
483 |
|
|
// |
484 |
|
|
// std::cout << "a_partial_dv\n" << a_partial_dv << std::endl; |
485 |
|
|
// std::cout << "rhs\n" << rhs << std::endl; |
486 |
|
|
// } |
487 |
|
|
|
488 |
|
|
// WORLD: da/dv == dJ/dt + dv/dq |
489 |
|
|
{ |
490 |
|
✗ |
Data::Matrix6x rhs(6, model.nv); |
491 |
|
✗ |
rhs.setZero(); |
492 |
|
|
|
493 |
|
✗ |
v_partial_dq.setZero(); |
494 |
|
✗ |
a_partial_dq.setZero(); |
495 |
|
✗ |
a_partial_dv.setZero(); |
496 |
|
✗ |
a_partial_da.setZero(); |
497 |
|
|
|
498 |
|
✗ |
computeForwardKinematicsDerivatives(model, data_ref, q, v, a); |
499 |
|
✗ |
computeForwardKinematicsDerivatives(model, data, q, v, a); |
500 |
|
|
|
501 |
|
✗ |
getJointAccelerationDerivatives( |
502 |
|
|
model, data, jointId, WORLD, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da); |
503 |
|
|
|
504 |
|
✗ |
getJointJacobianTimeVariation(model, data_ref, jointId, WORLD, rhs); |
505 |
|
|
|
506 |
|
✗ |
v_partial_dq_ref.setZero(); |
507 |
|
✗ |
v_partial_dv_ref.setZero(); |
508 |
|
✗ |
getJointVelocityDerivatives( |
509 |
|
|
model, data_ref, jointId, WORLD, v_partial_dq_ref, v_partial_dv_ref); |
510 |
|
✗ |
rhs += v_partial_dq_ref; |
511 |
|
✗ |
BOOST_CHECK(a_partial_dv.isApprox(rhs, 1e-12)); |
512 |
|
|
|
513 |
|
|
// std::cout << "a_partial_dv\n" << a_partial_dv << std::endl; |
514 |
|
|
// std::cout << "rhs\n" << rhs << std::endl; |
515 |
|
|
} |
516 |
|
|
|
517 |
|
|
// // WORLD: da/dq == d/dt(dv/dq) |
518 |
|
|
// { |
519 |
|
|
// const double alpha = 1e-8; |
520 |
|
|
// Eigen::VectorXd q_plus(model.nq), v_plus(model.nv); |
521 |
|
|
// |
522 |
|
|
// Data data_plus(model); |
523 |
|
|
// v_plus = v + alpha * a; |
524 |
|
|
// q_plus = integrate(model,q,alpha*v); |
525 |
|
|
// |
526 |
|
|
// computeForwardKinematicsDerivatives(model,data_plus,q_plus,v_plus,a); |
527 |
|
|
// computeForwardKinematicsDerivatives(model,data_ref,q,v,a); |
528 |
|
|
// |
529 |
|
|
// Data::Matrix6x v_partial_dq_plus(6,model.nv); v_partial_dq_plus.setZero(); |
530 |
|
|
// Data::Matrix6x v_partial_dv_plus(6,model.nv); v_partial_dv_plus.setZero(); |
531 |
|
|
// |
532 |
|
|
// v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero(); |
533 |
|
|
// |
534 |
|
|
// v_partial_dq.setZero(); |
535 |
|
|
// a_partial_dq.setZero(); |
536 |
|
|
// a_partial_dv.setZero(); |
537 |
|
|
// a_partial_da.setZero(); |
538 |
|
|
// |
539 |
|
|
// getJointVelocityDerivatives<WORLD>(model,data_ref,jointId, |
540 |
|
|
// v_partial_dq_ref,v_partial_dv_ref); |
541 |
|
|
// getJointVelocityDerivatives<WORLD>(model,data_plus,jointId, |
542 |
|
|
// v_partial_dq_plus,v_partial_dv_plus); |
543 |
|
|
// |
544 |
|
|
// getJointAccelerationDerivatives<WORLD>(model,data_ref,jointId, |
545 |
|
|
// v_partial_dq, |
546 |
|
|
// a_partial_dq,a_partial_dv,a_partial_da); |
547 |
|
|
// |
548 |
|
|
// Data::Matrix6x a_partial_dq_fd(6,model.nv); a_partial_dq_fd.setZero(); |
549 |
|
|
// { |
550 |
|
|
// Data data_fd(model); |
551 |
|
|
// VectorXd q_fd(model.nq), v_eps(model.nv); v_eps.setZero(); |
552 |
|
|
// for(int k = 0; k < model.nv; ++k) |
553 |
|
|
// { |
554 |
|
|
// v_eps[k] += alpha; |
555 |
|
|
// q_fd = integrate(model,q,v_eps); |
556 |
|
|
// forwardKinematics(model,data_fd,q_fd,v,a); |
557 |
|
|
// a_partial_dq_fd.col(k) = (data_fd.oMi[jointId].act(data_fd.a[jointId]) - |
558 |
|
|
// data_ref.oa[jointId]).toVector()/alpha; v_eps[k] = 0.; |
559 |
|
|
// } |
560 |
|
|
// } |
561 |
|
|
// |
562 |
|
|
// Data::Matrix6x rhs = (v_partial_dq_plus - v_partial_dq_ref)/alpha; |
563 |
|
|
// BOOST_CHECK(a_partial_dq.isApprox(rhs,sqrt(alpha))); |
564 |
|
|
// |
565 |
|
|
// std::cout << "a_partial_dq\n" << a_partial_dq << std::endl; |
566 |
|
|
// std::cout << "a_partial_dq_fd\n" << a_partial_dq_fd << std::endl; |
567 |
|
|
// std::cout << "rhs\n" << rhs << std::endl; |
568 |
|
|
// } |
569 |
|
|
|
570 |
|
|
// LOCAL: da/dq == d/dt(dv/dq) |
571 |
|
|
{ |
572 |
|
✗ |
const double alpha = 1e-8; |
573 |
|
✗ |
Eigen::VectorXd q_plus(model.nq), v_plus(model.nv); |
574 |
|
|
|
575 |
|
✗ |
Data data_plus(model); |
576 |
|
✗ |
v_plus = v + alpha * a; |
577 |
|
✗ |
q_plus = integrate(model, q, alpha * v); |
578 |
|
|
|
579 |
|
✗ |
computeForwardKinematicsDerivatives(model, data_plus, q_plus, v_plus, a); |
580 |
|
✗ |
computeForwardKinematicsDerivatives(model, data_ref, q, v, a); |
581 |
|
|
|
582 |
|
✗ |
Data::Matrix6x v_partial_dq_plus(6, model.nv); |
583 |
|
✗ |
v_partial_dq_plus.setZero(); |
584 |
|
✗ |
Data::Matrix6x v_partial_dv_plus(6, model.nv); |
585 |
|
✗ |
v_partial_dv_plus.setZero(); |
586 |
|
|
|
587 |
|
✗ |
v_partial_dq_ref.setZero(); |
588 |
|
✗ |
v_partial_dv_ref.setZero(); |
589 |
|
|
|
590 |
|
✗ |
v_partial_dq.setZero(); |
591 |
|
✗ |
a_partial_dq.setZero(); |
592 |
|
✗ |
a_partial_dv.setZero(); |
593 |
|
✗ |
a_partial_da.setZero(); |
594 |
|
|
|
595 |
|
✗ |
getJointVelocityDerivatives( |
596 |
|
|
model, data_ref, jointId, LOCAL, v_partial_dq_ref, v_partial_dv_ref); |
597 |
|
✗ |
getJointVelocityDerivatives( |
598 |
|
|
model, data_plus, jointId, LOCAL, v_partial_dq_plus, v_partial_dv_plus); |
599 |
|
|
|
600 |
|
✗ |
getJointAccelerationDerivatives( |
601 |
|
|
model, data_ref, jointId, LOCAL, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da); |
602 |
|
|
|
603 |
|
✗ |
Data::Matrix6x rhs = (v_partial_dq_plus - v_partial_dq_ref) / alpha; |
604 |
|
✗ |
BOOST_CHECK(a_partial_dq.isApprox(rhs, sqrt(alpha))); |
605 |
|
|
|
606 |
|
|
// std::cout << "a_partial_dq\n" << a_partial_dq << std::endl; |
607 |
|
|
// std::cout << "rhs\n" << rhs << std::endl; |
608 |
|
|
} |
609 |
|
|
} |
610 |
|
|
|
611 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_classic_acceleration_derivatives) |
612 |
|
|
{ |
613 |
|
|
using namespace Eigen; |
614 |
|
|
using namespace pinocchio; |
615 |
|
|
|
616 |
|
✗ |
Model model; |
617 |
|
✗ |
buildModels::humanoidRandom(model, true); |
618 |
|
|
|
619 |
|
✗ |
Data data(model), data_ref(model), data_plus(model); |
620 |
|
|
|
621 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
622 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
623 |
|
✗ |
VectorXd q = randomConfiguration(model); |
624 |
|
✗ |
VectorXd v = VectorXd::Random(model.nv); |
625 |
|
✗ |
VectorXd a = VectorXd::Random(model.nv); |
626 |
|
|
|
627 |
|
✗ |
const Model::JointIndex joint_id = model.existJointName("rarm4_joint") |
628 |
|
✗ |
? model.getJointId("rarm4_joint") |
629 |
|
✗ |
: (Model::Index)(model.njoints - 1); |
630 |
|
✗ |
Data::Matrix3x v3_partial_dq(3, model.nv); |
631 |
|
✗ |
v3_partial_dq.setZero(); |
632 |
|
✗ |
Data::Matrix3x a3_partial_dq(3, model.nv); |
633 |
|
✗ |
a3_partial_dq.setZero(); |
634 |
|
✗ |
Data::Matrix3x a3_partial_dv(3, model.nv); |
635 |
|
✗ |
a3_partial_dv.setZero(); |
636 |
|
✗ |
Data::Matrix3x a3_partial_da(3, model.nv); |
637 |
|
✗ |
a3_partial_da.setZero(); |
638 |
|
|
|
639 |
|
✗ |
Data::Matrix6x v_partial_dq_ref(6, model.nv); |
640 |
|
✗ |
v_partial_dq_ref.setZero(); |
641 |
|
✗ |
Data::Matrix6x v_partial_dv_ref(6, model.nv); |
642 |
|
✗ |
v_partial_dv_ref.setZero(); |
643 |
|
✗ |
Data::Matrix6x a_partial_dq_ref(6, model.nv); |
644 |
|
✗ |
a_partial_dq_ref.setZero(); |
645 |
|
✗ |
Data::Matrix6x a_partial_dv_ref(6, model.nv); |
646 |
|
✗ |
a_partial_dv_ref.setZero(); |
647 |
|
✗ |
Data::Matrix6x a_partial_da_ref(6, model.nv); |
648 |
|
✗ |
a_partial_da_ref.setZero(); |
649 |
|
|
|
650 |
|
✗ |
computeForwardKinematicsDerivatives(model, data_ref, q, 0 * v, a); |
651 |
|
✗ |
computeForwardKinematicsDerivatives(model, data, q, 0 * v, a); |
652 |
|
|
|
653 |
|
|
// LOCAL |
654 |
|
✗ |
getJointAccelerationDerivatives( |
655 |
|
|
model, data_ref, joint_id, LOCAL, v_partial_dq_ref, v_partial_dv_ref, a_partial_dq_ref, |
656 |
|
|
a_partial_dv_ref, a_partial_da_ref); |
657 |
|
|
|
658 |
|
✗ |
getPointClassicAccelerationDerivatives( |
659 |
|
✗ |
model, data, joint_id, SE3::Identity(), LOCAL, v3_partial_dq, a3_partial_dq, a3_partial_dv, |
660 |
|
|
a3_partial_da); |
661 |
|
|
|
662 |
|
✗ |
BOOST_CHECK(v3_partial_dq.isApprox(v_partial_dq_ref.middleRows<3>(Motion::LINEAR))); |
663 |
|
✗ |
BOOST_CHECK(a3_partial_dq.isApprox(a_partial_dq_ref.middleRows<3>(Motion::LINEAR))); |
664 |
|
✗ |
BOOST_CHECK(a3_partial_dv.isApprox(a_partial_dv_ref.middleRows<3>(Motion::LINEAR))); |
665 |
|
✗ |
BOOST_CHECK(a3_partial_da.isApprox(a_partial_da_ref.middleRows<3>(Motion::LINEAR))); |
666 |
|
|
|
667 |
|
|
// LOCAL_WORLD_ALIGNED |
668 |
|
✗ |
v_partial_dq_ref.setZero(); |
669 |
|
✗ |
v_partial_dv_ref.setZero(); |
670 |
|
✗ |
a_partial_dq_ref.setZero(); |
671 |
|
✗ |
a_partial_dv_ref.setZero(); |
672 |
|
✗ |
a_partial_da_ref.setZero(); |
673 |
|
✗ |
getJointAccelerationDerivatives( |
674 |
|
|
model, data_ref, joint_id, LOCAL_WORLD_ALIGNED, v_partial_dq_ref, v_partial_dv_ref, |
675 |
|
|
a_partial_dq_ref, a_partial_dv_ref, a_partial_da_ref); |
676 |
|
|
|
677 |
|
✗ |
v3_partial_dq.setZero(); |
678 |
|
✗ |
a3_partial_dq.setZero(); |
679 |
|
✗ |
a3_partial_dv.setZero(); |
680 |
|
✗ |
a3_partial_da.setZero(); |
681 |
|
✗ |
getPointClassicAccelerationDerivatives( |
682 |
|
✗ |
model, data, joint_id, SE3::Identity(), LOCAL_WORLD_ALIGNED, v3_partial_dq, a3_partial_dq, |
683 |
|
|
a3_partial_dv, a3_partial_da); |
684 |
|
|
|
685 |
|
✗ |
BOOST_CHECK(v3_partial_dq.isApprox(v_partial_dq_ref.middleRows<3>(Motion::LINEAR))); |
686 |
|
|
// BOOST_CHECK(a3_partial_dq.isApprox(a_partial_dq_ref.middleRows<3>(Motion::LINEAR))); |
687 |
|
✗ |
BOOST_CHECK(a3_partial_dv.isApprox(a_partial_dv_ref.middleRows<3>(Motion::LINEAR))); |
688 |
|
✗ |
BOOST_CHECK(a3_partial_da.isApprox(a_partial_da_ref.middleRows<3>(Motion::LINEAR))); |
689 |
|
|
|
690 |
|
|
// std::cout << "a3_partial_dq:\n" << a3_partial_dq << std::endl; |
691 |
|
|
// std::cout << "a3_partial_dq_ref:\n" << a_partial_dq_ref.middleRows<3>(Motion::LINEAR) << |
692 |
|
|
// std::endl; |
693 |
|
|
|
694 |
|
✗ |
const SE3 iMpoint = SE3::Random(); |
695 |
|
✗ |
computeForwardKinematicsDerivatives(model, data, q, v, a); |
696 |
|
|
|
697 |
|
✗ |
v3_partial_dq.setZero(); |
698 |
|
✗ |
a3_partial_dq.setZero(); |
699 |
|
✗ |
a3_partial_dv.setZero(); |
700 |
|
✗ |
a3_partial_da.setZero(); |
701 |
|
✗ |
getPointClassicAccelerationDerivatives( |
702 |
|
|
model, data, joint_id, iMpoint, LOCAL, v3_partial_dq, a3_partial_dq, a3_partial_dv, |
703 |
|
|
a3_partial_da); |
704 |
|
|
|
705 |
|
✗ |
Data::Matrix3x v3_partial_dq_LWA(3, model.nv); |
706 |
|
✗ |
v3_partial_dq_LWA.setZero(); |
707 |
|
✗ |
Data::Matrix3x a3_partial_dq_LWA(3, model.nv); |
708 |
|
✗ |
a3_partial_dq_LWA.setZero(); |
709 |
|
✗ |
Data::Matrix3x a3_partial_LWA_dv(3, model.nv); |
710 |
|
✗ |
a3_partial_LWA_dv.setZero(); |
711 |
|
✗ |
Data::Matrix3x a3_partial_LWA_da(3, model.nv); |
712 |
|
✗ |
a3_partial_LWA_da.setZero(); |
713 |
|
|
|
714 |
|
✗ |
getPointClassicAccelerationDerivatives( |
715 |
|
|
model, data, joint_id, iMpoint, LOCAL_WORLD_ALIGNED, v3_partial_dq_LWA, a3_partial_dq_LWA, |
716 |
|
|
a3_partial_LWA_dv, a3_partial_LWA_da); |
717 |
|
|
|
718 |
|
✗ |
const double eps = 1e-8; |
719 |
|
✗ |
Eigen::VectorXd v_plus = Eigen::VectorXd::Zero(model.nv); |
720 |
|
|
|
721 |
|
✗ |
Data::Matrix3x v3_partial_dq_fd(3, model.nv); |
722 |
|
✗ |
Data::Matrix3x v3_partial_dv_fd(3, model.nv); |
723 |
|
✗ |
Data::Matrix3x a3_partial_dq_fd(3, model.nv); |
724 |
|
✗ |
Data::Matrix3x a3_partial_dv_fd(3, model.nv); |
725 |
|
✗ |
Data::Matrix3x a3_partial_da_fd(3, model.nv); |
726 |
|
|
|
727 |
|
✗ |
Data::Matrix3x v3_partial_dq_LWA_fd(3, model.nv); |
728 |
|
✗ |
Data::Matrix3x v3_partial_dv_LWA_fd(3, model.nv); |
729 |
|
✗ |
Data::Matrix3x a3_partial_dq_LWA_fd(3, model.nv); |
730 |
|
✗ |
Data::Matrix3x a3_partial_dv_LWA_fd(3, model.nv); |
731 |
|
✗ |
Data::Matrix3x a3_partial_da_LWA_fd(3, model.nv); |
732 |
|
|
|
733 |
|
✗ |
const SE3 oMpoint = data.oMi[joint_id] * iMpoint; |
734 |
|
✗ |
const Motion::Vector3 point_vec_L = iMpoint.actInv(data.v[joint_id]).linear(); // LOCAL |
735 |
|
|
const Motion::Vector3 point_acc_L = |
736 |
|
✗ |
classicAcceleration(data.v[joint_id], data.a[joint_id], iMpoint); // LOCAL |
737 |
|
✗ |
const Motion::Vector3 point_vec_LWA = oMpoint.rotation() * point_vec_L; // LOCAL_WORLD_ALIGNED |
738 |
|
✗ |
const Motion::Vector3 point_acc_LWA = oMpoint.rotation() * point_acc_L; // LOCAL_WORLD_ALIGNED |
739 |
|
|
|
740 |
|
|
// Derivatives w.r.t q |
741 |
|
✗ |
for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
742 |
|
|
{ |
743 |
|
✗ |
v_plus[k] = eps; |
744 |
|
✗ |
const VectorXd q_plus = integrate(model, q, v_plus); |
745 |
|
✗ |
forwardKinematics(model, data_plus, q_plus, v, a); |
746 |
|
|
|
747 |
|
✗ |
const SE3 oMpoint_plus = data_plus.oMi[joint_id] * iMpoint; |
748 |
|
✗ |
const Motion::Vector3 point_vec_L_plus = iMpoint.actInv(data_plus.v[joint_id]).linear(); |
749 |
|
|
const Motion::Vector3 point_acc_L_plus = |
750 |
|
✗ |
classicAcceleration(data_plus.v[joint_id], data_plus.a[joint_id], iMpoint); |
751 |
|
|
|
752 |
|
✗ |
const Motion::Vector3 point_vec_LWA_plus = oMpoint_plus.rotation() * point_vec_L_plus; |
753 |
|
✗ |
const Motion::Vector3 point_acc_LWA_plus = oMpoint_plus.rotation() * point_acc_L_plus; |
754 |
|
|
|
755 |
|
✗ |
v3_partial_dq_fd.col(k) = (point_vec_L_plus - point_vec_L) / eps; |
756 |
|
✗ |
a3_partial_dq_fd.col(k) = (point_acc_L_plus - point_acc_L) / eps; |
757 |
|
|
|
758 |
|
✗ |
v3_partial_dq_LWA_fd.col(k) = (point_vec_LWA_plus - point_vec_LWA) / eps; |
759 |
|
✗ |
a3_partial_dq_LWA_fd.col(k) = (point_acc_LWA_plus - point_acc_LWA) / eps; |
760 |
|
|
|
761 |
|
✗ |
v_plus[k] = 0.; |
762 |
|
|
} |
763 |
|
|
|
764 |
|
✗ |
BOOST_CHECK(v3_partial_dq_fd.isApprox(v3_partial_dq, sqrt(eps))); |
765 |
|
✗ |
BOOST_CHECK(a3_partial_dq_fd.isApprox(a3_partial_dq, sqrt(eps))); |
766 |
|
|
|
767 |
|
✗ |
BOOST_CHECK(v3_partial_dq_LWA_fd.isApprox(v3_partial_dq_LWA, sqrt(eps))); |
768 |
|
✗ |
BOOST_CHECK(a3_partial_dq_LWA_fd.isApprox(a3_partial_dq_LWA, sqrt(eps))); |
769 |
|
|
|
770 |
|
|
// Derivatives w.r.t v |
771 |
|
✗ |
for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
772 |
|
|
{ |
773 |
|
✗ |
v_plus = v; |
774 |
|
✗ |
v_plus[k] += eps; |
775 |
|
✗ |
forwardKinematics(model, data_plus, q, v_plus, a); |
776 |
|
|
|
777 |
|
✗ |
const SE3 oMpoint_plus = data_plus.oMi[joint_id] * iMpoint; |
778 |
|
✗ |
const Motion::Vector3 point_vec_L_plus = iMpoint.actInv(data_plus.v[joint_id]).linear(); |
779 |
|
|
const Motion::Vector3 point_acc_L_plus = |
780 |
|
✗ |
classicAcceleration(data_plus.v[joint_id], data_plus.a[joint_id], iMpoint); |
781 |
|
|
|
782 |
|
✗ |
const Motion::Vector3 point_vec_LWA_plus = oMpoint_plus.rotation() * point_vec_L_plus; |
783 |
|
✗ |
const Motion::Vector3 point_acc_LWA_plus = oMpoint_plus.rotation() * point_acc_L_plus; |
784 |
|
|
|
785 |
|
✗ |
v3_partial_dv_fd.col(k) = (point_vec_L_plus - point_vec_L) / eps; |
786 |
|
✗ |
a3_partial_dv_fd.col(k) = (point_acc_L_plus - point_acc_L) / eps; |
787 |
|
|
|
788 |
|
✗ |
v3_partial_dv_LWA_fd.col(k) = (point_vec_LWA_plus - point_vec_LWA) / eps; |
789 |
|
✗ |
a3_partial_dv_LWA_fd.col(k) = (point_acc_LWA_plus - point_acc_LWA) / eps; |
790 |
|
|
} |
791 |
|
|
|
792 |
|
✗ |
BOOST_CHECK(v3_partial_dv_fd.isApprox(a3_partial_da, sqrt(eps))); |
793 |
|
✗ |
BOOST_CHECK(a3_partial_dv_fd.isApprox(a3_partial_dv, sqrt(eps))); |
794 |
|
|
|
795 |
|
|
// Derivatives w.r.t v |
796 |
|
✗ |
Eigen::VectorXd a_plus = Eigen::VectorXd::Zero(model.nv); |
797 |
|
✗ |
for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
798 |
|
|
{ |
799 |
|
✗ |
a_plus = a; |
800 |
|
✗ |
a_plus[k] += eps; |
801 |
|
✗ |
forwardKinematics(model, data_plus, q, v, a_plus); |
802 |
|
|
|
803 |
|
✗ |
const SE3 oMpoint_plus = data_plus.oMi[joint_id] * iMpoint; |
804 |
|
|
const Motion::Vector3 point_acc_L_plus = |
805 |
|
✗ |
classicAcceleration(data_plus.v[joint_id], data_plus.a[joint_id], iMpoint); |
806 |
|
|
|
807 |
|
✗ |
const Motion::Vector3 point_acc_LWA_plus = oMpoint_plus.rotation() * point_acc_L_plus; |
808 |
|
|
|
809 |
|
✗ |
a3_partial_da_fd.col(k) = (point_acc_L_plus - point_acc_L) / eps; |
810 |
|
✗ |
a3_partial_da_LWA_fd.col(k) = (point_acc_LWA_plus - point_acc_LWA) / eps; |
811 |
|
|
} |
812 |
|
|
|
813 |
|
✗ |
BOOST_CHECK(a3_partial_da_fd.isApprox(a3_partial_da, sqrt(eps))); |
814 |
|
|
|
815 |
|
|
// Test other signature |
816 |
|
✗ |
Data data_other(model); |
817 |
|
✗ |
Data::Matrix3x v3_partial_dq_other(3, model.nv); |
818 |
|
✗ |
v3_partial_dq_other.setZero(); |
819 |
|
✗ |
Data::Matrix3x v3_partial_dv_other(3, model.nv); |
820 |
|
✗ |
v3_partial_dv_other.setZero(); |
821 |
|
✗ |
Data::Matrix3x a3_partial_dq_other(3, model.nv); |
822 |
|
✗ |
a3_partial_dq_other.setZero(); |
823 |
|
✗ |
Data::Matrix3x a3_partial_dv_other(3, model.nv); |
824 |
|
✗ |
a3_partial_dv_other.setZero(); |
825 |
|
✗ |
Data::Matrix3x a3_partial_da_other(3, model.nv); |
826 |
|
✗ |
a3_partial_da_other.setZero(); |
827 |
|
|
|
828 |
|
✗ |
computeForwardKinematicsDerivatives(model, data_other, q, v, a); |
829 |
|
✗ |
getPointClassicAccelerationDerivatives( |
830 |
|
|
model, data_other, joint_id, iMpoint, LOCAL, v3_partial_dq_other, v3_partial_dv_other, |
831 |
|
|
a3_partial_dq_other, a3_partial_dv_other, a3_partial_da_other); |
832 |
|
|
|
833 |
|
✗ |
BOOST_CHECK(v3_partial_dq_other.isApprox(v3_partial_dq)); |
834 |
|
✗ |
BOOST_CHECK(v3_partial_dv_other.isApprox(a3_partial_da)); |
835 |
|
✗ |
BOOST_CHECK(a3_partial_dq_other.isApprox(a3_partial_dq)); |
836 |
|
✗ |
BOOST_CHECK(a3_partial_dv_other.isApprox(a3_partial_dv)); |
837 |
|
✗ |
BOOST_CHECK(a3_partial_da_other.isApprox(a3_partial_da)); |
838 |
|
|
} |
839 |
|
|
|
840 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_classic_velocity_derivatives) |
841 |
|
|
{ |
842 |
|
|
using namespace Eigen; |
843 |
|
|
using namespace pinocchio; |
844 |
|
|
|
845 |
|
✗ |
Model model; |
846 |
|
✗ |
buildModels::humanoidRandom(model, true); |
847 |
|
|
|
848 |
|
✗ |
Data data(model), data_ref(model); |
849 |
|
|
|
850 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
851 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
852 |
|
✗ |
VectorXd q = randomConfiguration(model); |
853 |
|
✗ |
VectorXd v = VectorXd::Random(model.nv); |
854 |
|
✗ |
VectorXd a = VectorXd::Random(model.nv); |
855 |
|
|
|
856 |
|
✗ |
const SE3 iMpoint = SE3::Random(); |
857 |
|
✗ |
const Model::JointIndex joint_id = model.existJointName("rarm4_joint") |
858 |
|
✗ |
? model.getJointId("rarm4_joint") |
859 |
|
✗ |
: (Model::Index)(model.njoints - 1); |
860 |
|
✗ |
Data::Matrix3x v3_partial_dq_L(3, model.nv); |
861 |
|
✗ |
v3_partial_dq_L.setZero(); |
862 |
|
✗ |
Data::Matrix3x v3_partial_dv_L(3, model.nv); |
863 |
|
✗ |
v3_partial_dv_L.setZero(); |
864 |
|
|
|
865 |
|
✗ |
computeForwardKinematicsDerivatives(model, data, q, v, 0 * a); |
866 |
|
✗ |
getPointVelocityDerivatives( |
867 |
|
|
model, data, joint_id, iMpoint, LOCAL, v3_partial_dq_L, v3_partial_dv_L); |
868 |
|
|
|
869 |
|
✗ |
Data::Matrix3x v_partial_dq_ref_L(3, model.nv); |
870 |
|
✗ |
v_partial_dq_ref_L.setZero(); |
871 |
|
✗ |
Data::Matrix3x v_partial_dv_ref_L(3, model.nv); |
872 |
|
✗ |
v_partial_dv_ref_L.setZero(); |
873 |
|
✗ |
Data::Matrix3x a_partial_dq_ref_L(3, model.nv); |
874 |
|
✗ |
a_partial_dq_ref_L.setZero(); |
875 |
|
✗ |
Data::Matrix3x a_partial_dv_ref_L(3, model.nv); |
876 |
|
✗ |
a_partial_dv_ref_L.setZero(); |
877 |
|
✗ |
Data::Matrix3x a_partial_da_ref_L(3, model.nv); |
878 |
|
✗ |
a_partial_da_ref_L.setZero(); |
879 |
|
|
|
880 |
|
✗ |
computeForwardKinematicsDerivatives(model, data_ref, q, v, a); |
881 |
|
✗ |
getPointClassicAccelerationDerivatives( |
882 |
|
|
model, data_ref, joint_id, iMpoint, LOCAL, v_partial_dq_ref_L, v_partial_dv_ref_L, |
883 |
|
|
a_partial_dq_ref_L, a_partial_dv_ref_L, a_partial_da_ref_L); |
884 |
|
|
|
885 |
|
✗ |
BOOST_CHECK(v3_partial_dq_L.isApprox(v_partial_dq_ref_L)); |
886 |
|
✗ |
BOOST_CHECK(v3_partial_dv_L.isApprox(v_partial_dv_ref_L)); |
887 |
|
|
|
888 |
|
|
// LOCAL_WORLD_ALIGNED |
889 |
|
✗ |
Data::Matrix3x v3_partial_dq_LWA(3, model.nv); |
890 |
|
✗ |
v3_partial_dq_LWA.setZero(); |
891 |
|
✗ |
Data::Matrix3x v3_partial_dv_LWA(3, model.nv); |
892 |
|
✗ |
v3_partial_dv_LWA.setZero(); |
893 |
|
|
|
894 |
|
✗ |
getPointVelocityDerivatives( |
895 |
|
|
model, data, joint_id, iMpoint, LOCAL_WORLD_ALIGNED, v3_partial_dq_LWA, v3_partial_dv_LWA); |
896 |
|
|
|
897 |
|
✗ |
Data::Matrix3x v_partial_dq_ref_LWA(3, model.nv); |
898 |
|
✗ |
v_partial_dq_ref_LWA.setZero(); |
899 |
|
✗ |
Data::Matrix3x v_partial_dv_ref_LWA(3, model.nv); |
900 |
|
✗ |
v_partial_dv_ref_LWA.setZero(); |
901 |
|
✗ |
Data::Matrix3x a_partial_dq_ref_LWA(3, model.nv); |
902 |
|
✗ |
a_partial_dq_ref_LWA.setZero(); |
903 |
|
✗ |
Data::Matrix3x a_partial_dv_ref_LWA(3, model.nv); |
904 |
|
✗ |
a_partial_dv_ref_LWA.setZero(); |
905 |
|
✗ |
Data::Matrix3x a_partial_da_ref_LWA(3, model.nv); |
906 |
|
✗ |
a_partial_da_ref_LWA.setZero(); |
907 |
|
|
|
908 |
|
✗ |
getPointClassicAccelerationDerivatives( |
909 |
|
|
model, data_ref, joint_id, iMpoint, LOCAL_WORLD_ALIGNED, v_partial_dq_ref_LWA, |
910 |
|
|
v_partial_dv_ref_LWA, a_partial_dq_ref_LWA, a_partial_dv_ref_LWA, a_partial_da_ref_LWA); |
911 |
|
|
|
912 |
|
✗ |
BOOST_CHECK(v3_partial_dq_LWA.isApprox(v_partial_dq_ref_LWA)); |
913 |
|
✗ |
BOOST_CHECK(v3_partial_dv_LWA.isApprox(v_partial_dv_ref_LWA)); |
914 |
|
|
} |
915 |
|
|
|
916 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_kinematics_hessians) |
917 |
|
|
{ |
918 |
|
|
using namespace Eigen; |
919 |
|
|
using namespace pinocchio; |
920 |
|
|
|
921 |
|
✗ |
Model model; |
922 |
|
✗ |
buildModels::humanoidRandom(model, true); |
923 |
|
|
|
924 |
|
✗ |
Data data(model), data_ref(model), data_plus(model); |
925 |
|
|
|
926 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
927 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
928 |
|
✗ |
VectorXd q = randomConfiguration(model); |
929 |
|
|
|
930 |
|
✗ |
const Model::JointIndex joint_id = model.existJointName("rarm2_joint") |
931 |
|
✗ |
? model.getJointId("rarm2_joint") |
932 |
|
✗ |
: (Model::Index)(model.njoints - 1); |
933 |
|
|
|
934 |
|
✗ |
computeJointJacobians(model, data, q); |
935 |
|
✗ |
computeJointKinematicHessians(model, data); |
936 |
|
|
|
937 |
|
✗ |
Data data2(model); |
938 |
|
✗ |
computeJointKinematicHessians(model, data2, q); |
939 |
|
✗ |
BOOST_CHECK(data2.J.isApprox(data.J)); |
940 |
|
|
|
941 |
|
✗ |
const Eigen::DenseIndex matrix_offset = 6 * model.nv; |
942 |
|
|
|
943 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
944 |
|
|
{ |
945 |
|
✗ |
Eigen::Map<Data::Matrix6x> dJ(data.kinematic_hessians.data() + k * matrix_offset, 6, model.nv); |
946 |
|
|
Eigen::Map<Data::Matrix6x> dJ2( |
947 |
|
✗ |
data2.kinematic_hessians.data() + k * matrix_offset, 6, model.nv); |
948 |
|
|
|
949 |
|
✗ |
BOOST_CHECK(dJ2.isApprox(dJ)); |
950 |
|
|
} |
951 |
|
|
|
952 |
|
✗ |
for (int i = 0; i < model.nv; ++i) |
953 |
|
|
{ |
954 |
|
✗ |
for (int j = i; j < model.nv; ++j) |
955 |
|
|
{ |
956 |
|
✗ |
bool j_is_children_of_i = false; |
957 |
|
✗ |
for (int parent = j; parent >= 0; parent = data.parents_fromRow[(size_t)parent]) |
958 |
|
|
{ |
959 |
|
✗ |
if (parent == i) |
960 |
|
|
{ |
961 |
|
✗ |
j_is_children_of_i = true; |
962 |
|
✗ |
break; |
963 |
|
|
} |
964 |
|
|
} |
965 |
|
|
|
966 |
|
✗ |
if (j_is_children_of_i) |
967 |
|
|
{ |
968 |
|
✗ |
if (i == j) |
969 |
|
|
{ |
970 |
|
|
Eigen::Map<Data::Motion::Vector6> SixSi( |
971 |
|
✗ |
data.kinematic_hessians.data() + i * matrix_offset + i * 6); |
972 |
|
✗ |
BOOST_CHECK(SixSi.isZero()); |
973 |
|
|
} |
974 |
|
|
else |
975 |
|
|
{ |
976 |
|
|
Eigen::Map<Data::Motion::Vector6> SixSj( |
977 |
|
✗ |
data.kinematic_hessians.data() + i * matrix_offset + j * 6); |
978 |
|
|
|
979 |
|
|
Eigen::Map<Data::Motion::Vector6> SjxSi( |
980 |
|
✗ |
data.kinematic_hessians.data() + j * matrix_offset + i * 6); |
981 |
|
|
|
982 |
|
✗ |
BOOST_CHECK(SixSj.isApprox(-SjxSi)); |
983 |
|
|
} |
984 |
|
|
} |
985 |
|
|
else |
986 |
|
|
{ |
987 |
|
|
Eigen::Map<Data::Motion::Vector6> SixSj( |
988 |
|
✗ |
data.kinematic_hessians.data() + i * matrix_offset + j * 6); |
989 |
|
|
|
990 |
|
|
Eigen::Map<Data::Motion::Vector6> SjxSi( |
991 |
|
✗ |
data.kinematic_hessians.data() + j * matrix_offset + i * 6); |
992 |
|
|
|
993 |
|
✗ |
BOOST_CHECK(SixSj.isZero()); |
994 |
|
✗ |
BOOST_CHECK(SjxSi.isZero()); |
995 |
|
|
} |
996 |
|
|
} |
997 |
|
|
} |
998 |
|
|
|
999 |
|
✗ |
const double eps = 1e-8; |
1000 |
|
✗ |
Data::Matrix6x J_ref(6, model.nv), J_plus(6, model.nv); |
1001 |
|
✗ |
J_ref.setZero(); |
1002 |
|
✗ |
J_plus.setZero(); |
1003 |
|
|
|
1004 |
|
✗ |
computeJointJacobians(model, data_ref, q); |
1005 |
|
✗ |
VectorXd v_plus(VectorXd::Zero(model.nv)); |
1006 |
|
|
|
1007 |
|
✗ |
const Eigen::DenseIndex outer_offset = model.nv * 6; |
1008 |
|
|
|
1009 |
|
|
// WORLD |
1010 |
|
✗ |
getJointJacobian(model, data_ref, joint_id, WORLD, J_ref); |
1011 |
|
✗ |
Data::Tensor3x kinematic_hessian_world = getJointKinematicHessian(model, data, joint_id, WORLD); |
1012 |
|
✗ |
for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
1013 |
|
|
{ |
1014 |
|
✗ |
v_plus[k] = eps; |
1015 |
|
✗ |
const VectorXd q_plus = integrate(model, q, v_plus); |
1016 |
|
✗ |
computeJointJacobians(model, data_plus, q_plus); |
1017 |
|
✗ |
J_plus.setZero(); |
1018 |
|
✗ |
getJointJacobian(model, data_plus, joint_id, WORLD, J_plus); |
1019 |
|
|
|
1020 |
|
✗ |
Data::Matrix6x dJ_dq_ref = (J_plus - J_ref) / eps; |
1021 |
|
|
Eigen::Map<Data::Matrix6x> dJ_dq( |
1022 |
|
✗ |
kinematic_hessian_world.data() + k * outer_offset, 6, model.nv); |
1023 |
|
|
|
1024 |
|
|
// std::cout << "k: " << k << std::endl; |
1025 |
|
|
// std::cout << "dJ_dq:\n" << dJ_dq << std::endl; |
1026 |
|
|
// std::cout << "dJ_dq_ref:\n" << dJ_dq_ref << std::endl; |
1027 |
|
✗ |
BOOST_CHECK((dJ_dq_ref - dJ_dq).isZero(sqrt(eps))); |
1028 |
|
✗ |
v_plus[k] = 0.; |
1029 |
|
|
} |
1030 |
|
|
|
1031 |
|
|
// LOCAL_WORLD_ALIGNED |
1032 |
|
✗ |
computeJointJacobians(model, data_ref, q); |
1033 |
|
✗ |
getJointJacobian(model, data_ref, joint_id, LOCAL_WORLD_ALIGNED, J_ref); |
1034 |
|
|
Data::Tensor3x kinematic_hessian_local_world_aligned = |
1035 |
|
✗ |
getJointKinematicHessian(model, data, joint_id, LOCAL_WORLD_ALIGNED); |
1036 |
|
✗ |
Data::Matrix3x dt_last_fd(3, model.nv); |
1037 |
|
✗ |
for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
1038 |
|
|
{ |
1039 |
|
✗ |
v_plus[k] = eps; |
1040 |
|
✗ |
const VectorXd q_plus = integrate(model, q, v_plus); |
1041 |
|
✗ |
computeJointJacobians(model, data_plus, q_plus); |
1042 |
|
✗ |
J_plus.setZero(); |
1043 |
|
✗ |
getJointJacobian(model, data_plus, joint_id, LOCAL_WORLD_ALIGNED, J_plus); |
1044 |
|
|
|
1045 |
|
✗ |
SE3 tMt_plus = data_ref.oMi[joint_id].inverse() * data_plus.oMi[joint_id]; |
1046 |
|
✗ |
tMt_plus.rotation().setIdentity(); |
1047 |
|
|
|
1048 |
|
✗ |
dt_last_fd.col(k) = |
1049 |
|
✗ |
(data_plus.oMi[joint_id].translation() - data_ref.oMi[joint_id].translation()) / eps; |
1050 |
|
|
|
1051 |
|
✗ |
Data::Matrix6x dJ_dq_ref = (J_plus - J_ref) / eps; |
1052 |
|
|
Eigen::Map<Data::Matrix6x> dJ_dq( |
1053 |
|
✗ |
kinematic_hessian_local_world_aligned.data() + k * outer_offset, 6, model.nv); |
1054 |
|
|
|
1055 |
|
|
// std::cout << "k: " << k << std::endl; |
1056 |
|
|
// std::cout << "dJ_dq:\n" << dJ_dq << std::endl; |
1057 |
|
|
// std::cout << "dJ_dq_ref:\n" << dJ_dq_ref << std::endl; |
1058 |
|
✗ |
BOOST_CHECK((dJ_dq_ref - dJ_dq).isZero(sqrt(eps))); |
1059 |
|
✗ |
v_plus[k] = 0.; |
1060 |
|
|
} |
1061 |
|
|
|
1062 |
|
✗ |
Data::Matrix6x J_world(6, model.nv); |
1063 |
|
✗ |
J_world.setZero(); |
1064 |
|
✗ |
getJointJacobian(model, data_ref, joint_id, LOCAL_WORLD_ALIGNED, J_world); |
1065 |
|
|
|
1066 |
|
✗ |
BOOST_CHECK(dt_last_fd.isApprox(J_world.topRows<3>(), sqrt(eps))); |
1067 |
|
|
|
1068 |
|
|
// LOCAL |
1069 |
|
✗ |
computeJointJacobians(model, data_ref, q); |
1070 |
|
✗ |
getJointJacobian(model, data_ref, joint_id, LOCAL, J_ref); |
1071 |
|
✗ |
Data::Tensor3x kinematic_hessian_local = getJointKinematicHessian(model, data, joint_id, LOCAL); |
1072 |
|
✗ |
for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
1073 |
|
|
{ |
1074 |
|
✗ |
v_plus[k] = eps; |
1075 |
|
✗ |
const VectorXd q_plus = integrate(model, q, v_plus); |
1076 |
|
✗ |
computeJointJacobians(model, data_plus, q_plus); |
1077 |
|
✗ |
J_plus.setZero(); |
1078 |
|
✗ |
getJointJacobian(model, data_plus, joint_id, LOCAL, J_plus); |
1079 |
|
|
|
1080 |
|
|
// const SE3 tMt_plus = data_ref.oMi[joint_id].inverse() * data_plus.oMi[joint_id]; |
1081 |
|
|
|
1082 |
|
✗ |
Data::Matrix6x dJ_dq_ref = (J_plus - J_ref) / eps; |
1083 |
|
|
Eigen::Map<Data::Matrix6x> dJ_dq( |
1084 |
|
✗ |
kinematic_hessian_local.data() + k * outer_offset, 6, model.nv); |
1085 |
|
|
|
1086 |
|
|
// std::cout << "k: " << k << std::endl; |
1087 |
|
|
// std::cout << "dJ_dq:\n" << dJ_dq << std::endl; |
1088 |
|
|
// std::cout << "dJ_dq_ref:\n" << dJ_dq_ref << std::endl; |
1089 |
|
✗ |
BOOST_CHECK((dJ_dq_ref - dJ_dq).isZero(sqrt(eps))); |
1090 |
|
✗ |
v_plus[k] = 0.; |
1091 |
|
|
} |
1092 |
|
|
} |
1093 |
|
|
|
1094 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
1095 |
|
|
|