Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2020 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/frames.hpp" |
10 |
|
|
#include "pinocchio/algorithm/frames-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_frames_derivatives_velocity) |
21 |
|
|
{ |
22 |
|
|
using namespace Eigen; |
23 |
|
|
using namespace pinocchio; |
24 |
|
|
|
25 |
|
✗ |
Model model; |
26 |
|
✗ |
buildModels::humanoidRandom(model); |
27 |
|
|
|
28 |
|
✗ |
const Model::JointIndex jointId = model.existJointName("rarm2_joint") |
29 |
|
✗ |
? model.getJointId("rarm2_joint") |
30 |
|
✗ |
: (Model::Index)(model.njoints - 1); |
31 |
|
✗ |
Frame frame("rand", jointId, 0, SE3::Random(), OP_FRAME); |
32 |
|
✗ |
FrameIndex frameId = model.addFrame(frame); |
33 |
|
|
|
34 |
|
✗ |
BOOST_CHECK(model.getFrameId("rand") == frameId); |
35 |
|
✗ |
BOOST_CHECK(model.frames[frameId].parentJoint == jointId); |
36 |
|
|
|
37 |
|
✗ |
Data data(model), data_ref(model); |
38 |
|
|
|
39 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
40 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
41 |
|
✗ |
VectorXd q = randomConfiguration(model); |
42 |
|
✗ |
VectorXd v(VectorXd::Random(model.nv)); |
43 |
|
✗ |
VectorXd a(VectorXd::Random(model.nv)); |
44 |
|
|
|
45 |
|
✗ |
computeForwardKinematicsDerivatives(model, data, q, v, a); |
46 |
|
|
|
47 |
|
✗ |
Data::Matrix6x partial_dq(6, model.nv); |
48 |
|
✗ |
partial_dq.setZero(); |
49 |
|
✗ |
Data::Matrix6x partial_dq_local_world_aligned(6, model.nv); |
50 |
|
✗ |
partial_dq_local_world_aligned.setZero(); |
51 |
|
✗ |
Data::Matrix6x partial_dq_local(6, model.nv); |
52 |
|
✗ |
partial_dq_local.setZero(); |
53 |
|
✗ |
Data::Matrix6x partial_dv(6, model.nv); |
54 |
|
✗ |
partial_dv.setZero(); |
55 |
|
✗ |
Data::Matrix6x partial_dv_local_world_aligned(6, model.nv); |
56 |
|
✗ |
partial_dv_local_world_aligned.setZero(); |
57 |
|
✗ |
Data::Matrix6x partial_dv_local(6, model.nv); |
58 |
|
✗ |
partial_dv_local.setZero(); |
59 |
|
|
|
60 |
|
✗ |
getFrameVelocityDerivatives(model, data, frameId, WORLD, partial_dq, partial_dv); |
61 |
|
|
|
62 |
|
✗ |
getFrameVelocityDerivatives( |
63 |
|
|
model, data, frameId, LOCAL_WORLD_ALIGNED, partial_dq_local_world_aligned, |
64 |
|
|
partial_dv_local_world_aligned); |
65 |
|
|
|
66 |
|
✗ |
getFrameVelocityDerivatives(model, data, frameId, LOCAL, partial_dq_local, partial_dv_local); |
67 |
|
|
|
68 |
|
✗ |
Data::Matrix6x J_ref(6, model.nv); |
69 |
|
✗ |
J_ref.setZero(); |
70 |
|
✗ |
Data::Matrix6x J_ref_local_world_aligned(6, model.nv); |
71 |
|
✗ |
J_ref_local_world_aligned.setZero(); |
72 |
|
✗ |
Data::Matrix6x J_ref_local(6, model.nv); |
73 |
|
✗ |
J_ref_local.setZero(); |
74 |
|
✗ |
computeJointJacobians(model, data_ref, q); |
75 |
|
✗ |
getFrameJacobian(model, data_ref, frameId, WORLD, J_ref); |
76 |
|
✗ |
getFrameJacobian(model, data_ref, frameId, LOCAL_WORLD_ALIGNED, J_ref_local_world_aligned); |
77 |
|
✗ |
getFrameJacobian(model, data_ref, frameId, LOCAL, J_ref_local); |
78 |
|
|
|
79 |
|
✗ |
BOOST_CHECK(data_ref.oMf[frameId].isApprox(data.oMf[frameId])); |
80 |
|
✗ |
BOOST_CHECK(partial_dv.isApprox(J_ref)); |
81 |
|
✗ |
BOOST_CHECK(partial_dv_local_world_aligned.isApprox(J_ref_local_world_aligned)); |
82 |
|
✗ |
BOOST_CHECK(partial_dv_local.isApprox(J_ref_local)); |
83 |
|
|
|
84 |
|
|
// Check against finite differences |
85 |
|
✗ |
Data::Matrix6x partial_dq_fd(6, model.nv); |
86 |
|
✗ |
partial_dq_fd.setZero(); |
87 |
|
✗ |
Data::Matrix6x partial_dq_fd_local_world_aligned(6, model.nv); |
88 |
|
✗ |
partial_dq_fd_local_world_aligned.setZero(); |
89 |
|
✗ |
Data::Matrix6x partial_dq_fd_local(6, model.nv); |
90 |
|
✗ |
partial_dq_fd_local.setZero(); |
91 |
|
✗ |
Data::Matrix6x partial_dv_fd(6, model.nv); |
92 |
|
✗ |
partial_dv_fd.setZero(); |
93 |
|
✗ |
Data::Matrix6x partial_dv_fd_local_world_aligned(6, model.nv); |
94 |
|
✗ |
partial_dv_fd_local_world_aligned.setZero(); |
95 |
|
✗ |
Data::Matrix6x partial_dv_fd_local(6, model.nv); |
96 |
|
✗ |
partial_dv_fd_local.setZero(); |
97 |
|
✗ |
const double alpha = 1e-8; |
98 |
|
|
|
99 |
|
|
// dvel/dv |
100 |
|
✗ |
Eigen::VectorXd v_plus(v); |
101 |
|
✗ |
Data data_plus(model); |
102 |
|
✗ |
forwardKinematics(model, data_ref, q, v); |
103 |
|
✗ |
Motion v0 = getFrameVelocity(model, data, frameId, WORLD); |
104 |
|
✗ |
Motion v0_local_world_aligned = getFrameVelocity(model, data, frameId, LOCAL_WORLD_ALIGNED); |
105 |
|
✗ |
Motion v0_local = getFrameVelocity(model, data, frameId, LOCAL); |
106 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
107 |
|
|
{ |
108 |
|
✗ |
v_plus[k] += alpha; |
109 |
|
✗ |
forwardKinematics(model, data_plus, q, v_plus); |
110 |
|
|
|
111 |
|
✗ |
partial_dv_fd.col(k) = |
112 |
|
✗ |
(getFrameVelocity(model, data_plus, frameId, WORLD) - v0).toVector() / alpha; |
113 |
|
✗ |
partial_dv_fd_local_world_aligned.col(k) = |
114 |
|
✗ |
(getFrameVelocity(model, data_plus, frameId, LOCAL_WORLD_ALIGNED) - v0_local_world_aligned) |
115 |
|
✗ |
.toVector() |
116 |
|
✗ |
/ alpha; |
117 |
|
✗ |
partial_dv_fd_local.col(k) = |
118 |
|
✗ |
(getFrameVelocity(model, data_plus, frameId, LOCAL) - v0_local).toVector() / alpha; |
119 |
|
✗ |
v_plus[k] -= alpha; |
120 |
|
|
} |
121 |
|
|
|
122 |
|
✗ |
BOOST_CHECK(partial_dv.isApprox(partial_dv_fd, sqrt(alpha))); |
123 |
|
✗ |
BOOST_CHECK( |
124 |
|
|
partial_dv_local_world_aligned.isApprox(partial_dv_fd_local_world_aligned, sqrt(alpha))); |
125 |
|
✗ |
BOOST_CHECK(partial_dv_local.isApprox(partial_dv_fd_local, sqrt(alpha))); |
126 |
|
|
|
127 |
|
|
// dvel/dq |
128 |
|
✗ |
Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv)); |
129 |
|
✗ |
forwardKinematics(model, data_ref, q, v); |
130 |
|
✗ |
updateFramePlacements(model, data_ref); |
131 |
|
|
|
132 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
133 |
|
|
{ |
134 |
|
✗ |
v_eps[k] += alpha; |
135 |
|
✗ |
q_plus = integrate(model, q, v_eps); |
136 |
|
✗ |
forwardKinematics(model, data_plus, q_plus, v); |
137 |
|
✗ |
updateFramePlacements(model, data_plus); |
138 |
|
|
|
139 |
|
|
Motion v_plus_local_world_aligned = |
140 |
|
✗ |
getFrameVelocity(model, data_plus, frameId, LOCAL_WORLD_ALIGNED); |
141 |
|
✗ |
SE3::Vector3 trans = data_plus.oMf[frameId].translation() - data_ref.oMf[frameId].translation(); |
142 |
|
✗ |
v_plus_local_world_aligned.linear() -= v_plus_local_world_aligned.angular().cross(trans); |
143 |
|
✗ |
partial_dq_fd.col(k) = |
144 |
|
✗ |
(getFrameVelocity(model, data_plus, frameId, WORLD) - v0).toVector() / alpha; |
145 |
|
✗ |
partial_dq_fd_local_world_aligned.col(k) = |
146 |
|
✗ |
(v_plus_local_world_aligned - v0_local_world_aligned).toVector() / alpha; |
147 |
|
✗ |
partial_dq_fd_local.col(k) = |
148 |
|
✗ |
(getFrameVelocity(model, data_plus, frameId, LOCAL) - v0_local).toVector() / alpha; |
149 |
|
✗ |
v_eps[k] -= alpha; |
150 |
|
|
} |
151 |
|
|
|
152 |
|
✗ |
BOOST_CHECK(partial_dq.isApprox(partial_dq_fd, sqrt(alpha))); |
153 |
|
✗ |
BOOST_CHECK( |
154 |
|
|
partial_dq_local_world_aligned.isApprox(partial_dq_fd_local_world_aligned, sqrt(alpha))); |
155 |
|
✗ |
BOOST_CHECK(partial_dq_local.isApprox(partial_dq_fd_local, sqrt(alpha))); |
156 |
|
|
} |
157 |
|
|
|
158 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_acceleration) |
159 |
|
|
{ |
160 |
|
|
using namespace Eigen; |
161 |
|
|
using namespace pinocchio; |
162 |
|
|
|
163 |
|
✗ |
Model model; |
164 |
|
✗ |
buildModels::humanoidRandom(model); |
165 |
|
|
|
166 |
|
✗ |
const Model::JointIndex jointId = model.existJointName("rarm2_joint") |
167 |
|
✗ |
? model.getJointId("rarm2_joint") |
168 |
|
✗ |
: (Model::Index)(model.njoints - 1); |
169 |
|
✗ |
Frame frame("rand", jointId, 0, SE3::Random(), OP_FRAME); |
170 |
|
✗ |
FrameIndex frameId = model.addFrame(frame); |
171 |
|
|
|
172 |
|
✗ |
BOOST_CHECK(model.getFrameId("rand") == frameId); |
173 |
|
✗ |
BOOST_CHECK(model.frames[frameId].parentJoint == jointId); |
174 |
|
|
|
175 |
|
✗ |
Data data(model), data_ref(model); |
176 |
|
|
|
177 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
178 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
179 |
|
✗ |
VectorXd q = randomConfiguration(model); |
180 |
|
✗ |
VectorXd v(VectorXd::Random(model.nv)); |
181 |
|
✗ |
VectorXd a(VectorXd::Random(model.nv)); |
182 |
|
|
|
183 |
|
✗ |
computeForwardKinematicsDerivatives(model, data, q, v, a); |
184 |
|
|
|
185 |
|
✗ |
Data::Matrix6x v_partial_dq(6, model.nv); |
186 |
|
✗ |
v_partial_dq.setZero(); |
187 |
|
✗ |
Data::Matrix6x v_partial_dq_local(6, model.nv); |
188 |
|
✗ |
v_partial_dq_local.setZero(); |
189 |
|
✗ |
Data::Matrix6x v_partial_dq_local_world_aligned(6, model.nv); |
190 |
|
✗ |
v_partial_dq_local_world_aligned.setZero(); |
191 |
|
✗ |
Data::Matrix6x a_partial_dq(6, model.nv); |
192 |
|
✗ |
a_partial_dq.setZero(); |
193 |
|
✗ |
Data::Matrix6x a_partial_dq_local_world_aligned(6, model.nv); |
194 |
|
✗ |
a_partial_dq_local_world_aligned.setZero(); |
195 |
|
✗ |
Data::Matrix6x a_partial_dq_local(6, model.nv); |
196 |
|
✗ |
a_partial_dq_local.setZero(); |
197 |
|
✗ |
Data::Matrix6x a_partial_dv(6, model.nv); |
198 |
|
✗ |
a_partial_dv.setZero(); |
199 |
|
✗ |
Data::Matrix6x a_partial_dv_local_world_aligned(6, model.nv); |
200 |
|
✗ |
a_partial_dv_local_world_aligned.setZero(); |
201 |
|
✗ |
Data::Matrix6x a_partial_dv_local(6, model.nv); |
202 |
|
✗ |
a_partial_dv_local.setZero(); |
203 |
|
✗ |
Data::Matrix6x a_partial_da(6, model.nv); |
204 |
|
✗ |
a_partial_da.setZero(); |
205 |
|
✗ |
Data::Matrix6x a_partial_da_local_world_aligned(6, model.nv); |
206 |
|
✗ |
a_partial_da_local_world_aligned.setZero(); |
207 |
|
✗ |
Data::Matrix6x a_partial_da_local(6, model.nv); |
208 |
|
✗ |
a_partial_da_local.setZero(); |
209 |
|
|
|
210 |
|
✗ |
getFrameAccelerationDerivatives( |
211 |
|
|
model, data, frameId, WORLD, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da); |
212 |
|
|
|
213 |
|
✗ |
getFrameAccelerationDerivatives( |
214 |
|
|
model, data, frameId, LOCAL_WORLD_ALIGNED, v_partial_dq_local_world_aligned, |
215 |
|
|
a_partial_dq_local_world_aligned, a_partial_dv_local_world_aligned, |
216 |
|
|
a_partial_da_local_world_aligned); |
217 |
|
|
|
218 |
|
✗ |
getFrameAccelerationDerivatives( |
219 |
|
|
model, data, frameId, LOCAL, v_partial_dq_local, a_partial_dq_local, a_partial_dv_local, |
220 |
|
|
a_partial_da_local); |
221 |
|
|
|
222 |
|
|
// Check v_partial_dq against getFrameVelocityDerivatives |
223 |
|
|
{ |
224 |
|
✗ |
Data data_v(model); |
225 |
|
✗ |
computeForwardKinematicsDerivatives(model, data_v, q, v, a); |
226 |
|
|
|
227 |
|
✗ |
Data::Matrix6x v_partial_dq_ref(6, model.nv); |
228 |
|
✗ |
v_partial_dq_ref.setZero(); |
229 |
|
✗ |
Data::Matrix6x v_partial_dq_ref_local_world_aligned(6, model.nv); |
230 |
|
✗ |
v_partial_dq_ref_local_world_aligned.setZero(); |
231 |
|
✗ |
Data::Matrix6x v_partial_dq_ref_local(6, model.nv); |
232 |
|
✗ |
v_partial_dq_ref_local.setZero(); |
233 |
|
✗ |
Data::Matrix6x v_partial_dv_ref(6, model.nv); |
234 |
|
✗ |
v_partial_dv_ref.setZero(); |
235 |
|
✗ |
Data::Matrix6x v_partial_dv_ref_local_world_aligned(6, model.nv); |
236 |
|
✗ |
v_partial_dv_ref_local_world_aligned.setZero(); |
237 |
|
✗ |
Data::Matrix6x v_partial_dv_ref_local(6, model.nv); |
238 |
|
✗ |
v_partial_dv_ref_local.setZero(); |
239 |
|
|
|
240 |
|
✗ |
getFrameVelocityDerivatives(model, data_v, frameId, WORLD, v_partial_dq_ref, v_partial_dv_ref); |
241 |
|
|
|
242 |
|
✗ |
BOOST_CHECK(v_partial_dq.isApprox(v_partial_dq_ref)); |
243 |
|
✗ |
BOOST_CHECK(a_partial_da.isApprox(v_partial_dv_ref)); |
244 |
|
|
|
245 |
|
✗ |
getFrameVelocityDerivatives( |
246 |
|
|
model, data_v, frameId, LOCAL_WORLD_ALIGNED, v_partial_dq_ref_local_world_aligned, |
247 |
|
|
v_partial_dv_ref_local_world_aligned); |
248 |
|
|
|
249 |
|
✗ |
BOOST_CHECK(v_partial_dq_local_world_aligned.isApprox(v_partial_dq_ref_local_world_aligned)); |
250 |
|
✗ |
BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(v_partial_dv_ref_local_world_aligned)); |
251 |
|
|
|
252 |
|
✗ |
getFrameVelocityDerivatives( |
253 |
|
|
model, data_v, frameId, LOCAL, v_partial_dq_ref_local, v_partial_dv_ref_local); |
254 |
|
|
|
255 |
|
✗ |
BOOST_CHECK(v_partial_dq_local.isApprox(v_partial_dq_ref_local)); |
256 |
|
✗ |
BOOST_CHECK(a_partial_da_local.isApprox(v_partial_dv_ref_local)); |
257 |
|
|
} |
258 |
|
|
|
259 |
|
✗ |
Data::Matrix6x J_ref(6, model.nv); |
260 |
|
✗ |
J_ref.setZero(); |
261 |
|
✗ |
Data::Matrix6x J_ref_local(6, model.nv); |
262 |
|
✗ |
J_ref_local.setZero(); |
263 |
|
✗ |
Data::Matrix6x J_ref_local_world_aligned(6, model.nv); |
264 |
|
✗ |
J_ref_local_world_aligned.setZero(); |
265 |
|
✗ |
computeJointJacobians(model, data_ref, q); |
266 |
|
✗ |
getFrameJacobian(model, data_ref, frameId, WORLD, J_ref); |
267 |
|
✗ |
getFrameJacobian(model, data_ref, frameId, LOCAL_WORLD_ALIGNED, J_ref_local_world_aligned); |
268 |
|
✗ |
getFrameJacobian(model, data_ref, frameId, LOCAL, J_ref_local); |
269 |
|
|
|
270 |
|
✗ |
BOOST_CHECK(a_partial_da.isApprox(J_ref)); |
271 |
|
✗ |
BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(J_ref_local_world_aligned)); |
272 |
|
✗ |
BOOST_CHECK(a_partial_da_local.isApprox(J_ref_local)); |
273 |
|
|
|
274 |
|
|
// Check against finite differences |
275 |
|
✗ |
Data::Matrix6x a_partial_da_fd(6, model.nv); |
276 |
|
✗ |
a_partial_da_fd.setZero(); |
277 |
|
✗ |
Data::Matrix6x a_partial_da_fd_local_world_aligned(6, model.nv); |
278 |
|
✗ |
a_partial_da_fd_local_world_aligned.setZero(); |
279 |
|
✗ |
Data::Matrix6x a_partial_da_fd_local(6, model.nv); |
280 |
|
✗ |
a_partial_da_fd_local.setZero(); |
281 |
|
✗ |
const double alpha = 1e-8; |
282 |
|
|
|
283 |
|
✗ |
Eigen::VectorXd v_plus(v), a_plus(a); |
284 |
|
✗ |
Data data_plus(model); |
285 |
|
✗ |
forwardKinematics(model, data_ref, q, v, a); |
286 |
|
|
|
287 |
|
|
// dacc/da |
288 |
|
✗ |
Motion a0 = getFrameAcceleration(model, data, frameId, WORLD); |
289 |
|
✗ |
Motion a0_local_world_aligned = getFrameAcceleration(model, data, frameId, LOCAL_WORLD_ALIGNED); |
290 |
|
✗ |
Motion a0_local = getFrameAcceleration(model, data, frameId, LOCAL); |
291 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
292 |
|
|
{ |
293 |
|
✗ |
a_plus[k] += alpha; |
294 |
|
✗ |
forwardKinematics(model, data_plus, q, v, a_plus); |
295 |
|
|
|
296 |
|
✗ |
a_partial_da_fd.col(k) = |
297 |
|
✗ |
(getFrameAcceleration(model, data_plus, frameId, WORLD) - a0).toVector() / alpha; |
298 |
|
✗ |
a_partial_da_fd_local_world_aligned.col(k) = |
299 |
|
✗ |
(getFrameAcceleration(model, data_plus, frameId, LOCAL_WORLD_ALIGNED) |
300 |
|
✗ |
- a0_local_world_aligned) |
301 |
|
✗ |
.toVector() |
302 |
|
✗ |
/ alpha; |
303 |
|
✗ |
a_partial_da_fd_local.col(k) = |
304 |
|
✗ |
(getFrameAcceleration(model, data_plus, frameId, LOCAL) - a0_local).toVector() / alpha; |
305 |
|
✗ |
a_plus[k] -= alpha; |
306 |
|
|
} |
307 |
|
✗ |
BOOST_CHECK(a_partial_da.isApprox(a_partial_da_fd, sqrt(alpha))); |
308 |
|
✗ |
BOOST_CHECK( |
309 |
|
|
a_partial_da_local_world_aligned.isApprox(a_partial_da_fd_local_world_aligned, sqrt(alpha))); |
310 |
|
✗ |
BOOST_CHECK(a_partial_da_local.isApprox(a_partial_da_fd_local, sqrt(alpha))); |
311 |
|
|
|
312 |
|
|
// dacc/dv |
313 |
|
✗ |
Data::Matrix6x a_partial_dv_fd(6, model.nv); |
314 |
|
✗ |
a_partial_dv_fd.setZero(); |
315 |
|
✗ |
Data::Matrix6x a_partial_dv_fd_local_world_aligned(6, model.nv); |
316 |
|
✗ |
a_partial_dv_fd_local_world_aligned.setZero(); |
317 |
|
✗ |
Data::Matrix6x a_partial_dv_fd_local(6, model.nv); |
318 |
|
✗ |
a_partial_dv_fd_local.setZero(); |
319 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
320 |
|
|
{ |
321 |
|
✗ |
v_plus[k] += alpha; |
322 |
|
✗ |
forwardKinematics(model, data_plus, q, v_plus, a); |
323 |
|
|
|
324 |
|
✗ |
a_partial_dv_fd.col(k) = |
325 |
|
✗ |
(getFrameAcceleration(model, data_plus, frameId, WORLD) - a0).toVector() / alpha; |
326 |
|
✗ |
a_partial_dv_fd_local_world_aligned.col(k) = |
327 |
|
✗ |
(getFrameAcceleration(model, data_plus, frameId, LOCAL_WORLD_ALIGNED) |
328 |
|
✗ |
- a0_local_world_aligned) |
329 |
|
✗ |
.toVector() |
330 |
|
✗ |
/ alpha; |
331 |
|
✗ |
a_partial_dv_fd_local.col(k) = |
332 |
|
✗ |
(getFrameAcceleration(model, data_plus, frameId, LOCAL) - a0_local).toVector() / alpha; |
333 |
|
✗ |
v_plus[k] -= alpha; |
334 |
|
|
} |
335 |
|
|
|
336 |
|
✗ |
BOOST_CHECK(a_partial_dv.isApprox(a_partial_dv_fd, sqrt(alpha))); |
337 |
|
✗ |
BOOST_CHECK( |
338 |
|
|
a_partial_dv_local_world_aligned.isApprox(a_partial_dv_fd_local_world_aligned, sqrt(alpha))); |
339 |
|
✗ |
BOOST_CHECK(a_partial_dv_local.isApprox(a_partial_dv_fd_local, sqrt(alpha))); |
340 |
|
|
|
341 |
|
|
// dacc/dq |
342 |
|
✗ |
a_partial_dq.setZero(); |
343 |
|
✗ |
a_partial_dv.setZero(); |
344 |
|
✗ |
a_partial_da.setZero(); |
345 |
|
|
|
346 |
|
✗ |
a_partial_dq_local_world_aligned.setZero(); |
347 |
|
✗ |
a_partial_dv_local_world_aligned.setZero(); |
348 |
|
✗ |
a_partial_da_local_world_aligned.setZero(); |
349 |
|
|
|
350 |
|
✗ |
a_partial_dq_local.setZero(); |
351 |
|
✗ |
a_partial_dv_local.setZero(); |
352 |
|
✗ |
a_partial_da_local.setZero(); |
353 |
|
|
|
354 |
|
✗ |
Data::Matrix6x a_partial_dq_fd(6, model.nv); |
355 |
|
✗ |
a_partial_dq_fd.setZero(); |
356 |
|
✗ |
Data::Matrix6x a_partial_dq_fd_local_world_aligned(6, model.nv); |
357 |
|
✗ |
a_partial_dq_fd_local_world_aligned.setZero(); |
358 |
|
✗ |
Data::Matrix6x a_partial_dq_fd_local(6, model.nv); |
359 |
|
✗ |
a_partial_dq_fd_local.setZero(); |
360 |
|
|
|
361 |
|
✗ |
computeForwardKinematicsDerivatives(model, data, q, v, a); |
362 |
|
✗ |
getFrameAccelerationDerivatives( |
363 |
|
|
model, data, frameId, WORLD, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da); |
364 |
|
|
|
365 |
|
✗ |
getFrameAccelerationDerivatives( |
366 |
|
|
model, data, frameId, LOCAL_WORLD_ALIGNED, v_partial_dq_local_world_aligned, |
367 |
|
|
a_partial_dq_local_world_aligned, a_partial_dv_local_world_aligned, |
368 |
|
|
a_partial_da_local_world_aligned); |
369 |
|
|
|
370 |
|
✗ |
getFrameAccelerationDerivatives( |
371 |
|
|
model, data, frameId, LOCAL, v_partial_dq_local, a_partial_dq_local, a_partial_dv_local, |
372 |
|
|
a_partial_da_local); |
373 |
|
|
|
374 |
|
✗ |
Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv)); |
375 |
|
✗ |
forwardKinematics(model, data_ref, q, v, a); |
376 |
|
✗ |
updateFramePlacements(model, data_ref); |
377 |
|
✗ |
a0 = getFrameAcceleration(model, data, frameId, WORLD); |
378 |
|
✗ |
a0_local_world_aligned = getFrameAcceleration(model, data, frameId, LOCAL_WORLD_ALIGNED); |
379 |
|
✗ |
a0_local = getFrameAcceleration(model, data, frameId, LOCAL); |
380 |
|
|
|
381 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
382 |
|
|
{ |
383 |
|
✗ |
v_eps[k] += alpha; |
384 |
|
✗ |
q_plus = integrate(model, q, v_eps); |
385 |
|
✗ |
forwardKinematics(model, data_plus, q_plus, v, a); |
386 |
|
✗ |
updateFramePlacements(model, data_plus); |
387 |
|
|
|
388 |
|
✗ |
a_partial_dq_fd.col(k) = |
389 |
|
✗ |
(getFrameAcceleration(model, data_plus, frameId, WORLD) - a0).toVector() / alpha; |
390 |
|
|
Motion a_plus_local_world_aligned = |
391 |
|
✗ |
getFrameAcceleration(model, data_plus, frameId, LOCAL_WORLD_ALIGNED); |
392 |
|
|
const SE3::Vector3 trans = |
393 |
|
✗ |
data_plus.oMf[frameId].translation() - data_ref.oMf[frameId].translation(); |
394 |
|
✗ |
a_plus_local_world_aligned.linear() -= a_plus_local_world_aligned.angular().cross(trans); |
395 |
|
✗ |
a_partial_dq_fd_local_world_aligned.col(k) = |
396 |
|
✗ |
(a_plus_local_world_aligned - a0_local_world_aligned).toVector() / alpha; |
397 |
|
✗ |
a_partial_dq_fd_local.col(k) = |
398 |
|
✗ |
(getFrameAcceleration(model, data_plus, frameId, LOCAL) - a0_local).toVector() / alpha; |
399 |
|
✗ |
v_eps[k] -= alpha; |
400 |
|
|
} |
401 |
|
|
|
402 |
|
✗ |
BOOST_CHECK(a_partial_dq.isApprox(a_partial_dq_fd, sqrt(alpha))); |
403 |
|
✗ |
BOOST_CHECK( |
404 |
|
|
a_partial_dq_local_world_aligned.isApprox(a_partial_dq_fd_local_world_aligned, sqrt(alpha))); |
405 |
|
✗ |
BOOST_CHECK(a_partial_dq_local.isApprox(a_partial_dq_fd_local, sqrt(alpha))); |
406 |
|
|
|
407 |
|
|
// Test other signatures |
408 |
|
✗ |
Data::Matrix6x v_partial_dq_other(6, model.nv); |
409 |
|
✗ |
v_partial_dq_other.setZero(); |
410 |
|
✗ |
Data::Matrix6x v_partial_dq_local_other(6, model.nv); |
411 |
|
✗ |
v_partial_dq_local_other.setZero(); |
412 |
|
✗ |
Data::Matrix6x v_partial_dq_local_world_aligned_other(6, model.nv); |
413 |
|
✗ |
v_partial_dq_local_world_aligned_other.setZero(); |
414 |
|
✗ |
Data::Matrix6x v_partial_dv_other(6, model.nv); |
415 |
|
✗ |
v_partial_dv_other.setZero(); |
416 |
|
✗ |
Data::Matrix6x v_partial_dv_local_other(6, model.nv); |
417 |
|
✗ |
v_partial_dv_local_other.setZero(); |
418 |
|
✗ |
Data::Matrix6x v_partial_dv_local_world_aligned_other(6, model.nv); |
419 |
|
✗ |
v_partial_dv_local_world_aligned_other.setZero(); |
420 |
|
✗ |
Data::Matrix6x a_partial_dq_other(6, model.nv); |
421 |
|
✗ |
a_partial_dq_other.setZero(); |
422 |
|
✗ |
Data::Matrix6x a_partial_dq_local_world_aligned_other(6, model.nv); |
423 |
|
✗ |
a_partial_dq_local_world_aligned_other.setZero(); |
424 |
|
✗ |
Data::Matrix6x a_partial_dq_local_other(6, model.nv); |
425 |
|
✗ |
a_partial_dq_local_other.setZero(); |
426 |
|
✗ |
Data::Matrix6x a_partial_dv_other(6, model.nv); |
427 |
|
✗ |
a_partial_dv_other.setZero(); |
428 |
|
✗ |
Data::Matrix6x a_partial_dv_local_world_aligned_other(6, model.nv); |
429 |
|
✗ |
a_partial_dv_local_world_aligned_other.setZero(); |
430 |
|
✗ |
Data::Matrix6x a_partial_dv_local_other(6, model.nv); |
431 |
|
✗ |
a_partial_dv_local_other.setZero(); |
432 |
|
✗ |
Data::Matrix6x a_partial_da_other(6, model.nv); |
433 |
|
✗ |
a_partial_da_other.setZero(); |
434 |
|
✗ |
Data::Matrix6x a_partial_da_local_world_aligned_other(6, model.nv); |
435 |
|
✗ |
a_partial_da_local_world_aligned_other.setZero(); |
436 |
|
✗ |
Data::Matrix6x a_partial_da_local_other(6, model.nv); |
437 |
|
✗ |
a_partial_da_local_other.setZero(); |
438 |
|
|
|
439 |
|
✗ |
getFrameAccelerationDerivatives( |
440 |
|
|
model, data, frameId, WORLD, v_partial_dq_other, v_partial_dv_other, a_partial_dq_other, |
441 |
|
|
a_partial_dv_other, a_partial_da_other); |
442 |
|
|
|
443 |
|
✗ |
BOOST_CHECK(v_partial_dq_other.isApprox(v_partial_dq)); |
444 |
|
✗ |
BOOST_CHECK(v_partial_dv_other.isApprox(a_partial_da)); |
445 |
|
✗ |
BOOST_CHECK(a_partial_dq_other.isApprox(a_partial_dq)); |
446 |
|
✗ |
BOOST_CHECK(a_partial_dv_other.isApprox(a_partial_dv)); |
447 |
|
✗ |
BOOST_CHECK(a_partial_da_other.isApprox(a_partial_da)); |
448 |
|
|
|
449 |
|
✗ |
getFrameAccelerationDerivatives( |
450 |
|
|
model, data, frameId, LOCAL_WORLD_ALIGNED, v_partial_dq_local_world_aligned_other, |
451 |
|
|
v_partial_dv_local_world_aligned_other, a_partial_dq_local_world_aligned_other, |
452 |
|
|
a_partial_dv_local_world_aligned_other, a_partial_da_local_world_aligned_other); |
453 |
|
|
|
454 |
|
✗ |
BOOST_CHECK(v_partial_dq_local_world_aligned_other.isApprox(v_partial_dq_local_world_aligned)); |
455 |
|
✗ |
BOOST_CHECK(v_partial_dv_local_world_aligned_other.isApprox(a_partial_da_local_world_aligned)); |
456 |
|
✗ |
BOOST_CHECK(a_partial_dq_local_world_aligned_other.isApprox(a_partial_dq_local_world_aligned)); |
457 |
|
✗ |
BOOST_CHECK(a_partial_dv_local_world_aligned_other.isApprox(a_partial_dv_local_world_aligned)); |
458 |
|
✗ |
BOOST_CHECK(a_partial_da_local_world_aligned_other.isApprox(a_partial_da_local_world_aligned)); |
459 |
|
|
|
460 |
|
✗ |
getFrameAccelerationDerivatives( |
461 |
|
|
model, data, frameId, LOCAL, v_partial_dq_local_other, v_partial_dv_local_other, |
462 |
|
|
a_partial_dq_local_other, a_partial_dv_local_other, a_partial_da_local_other); |
463 |
|
|
|
464 |
|
✗ |
BOOST_CHECK(v_partial_dq_local_other.isApprox(v_partial_dq_local)); |
465 |
|
✗ |
BOOST_CHECK(v_partial_dv_local_other.isApprox(a_partial_da_local)); |
466 |
|
✗ |
BOOST_CHECK(a_partial_dq_local_other.isApprox(a_partial_dq_local)); |
467 |
|
✗ |
BOOST_CHECK(a_partial_dv_local_other.isApprox(a_partial_dv_local)); |
468 |
|
✗ |
BOOST_CHECK(a_partial_da_local_other.isApprox(a_partial_da_local)); |
469 |
|
|
} |
470 |
|
|
|
471 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
472 |
|
|
|