Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2016-2020 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include "pinocchio/multibody/model.hpp" |
6 |
|
|
#include "pinocchio/multibody/data.hpp" |
7 |
|
|
#include "pinocchio/algorithm/model.hpp" |
8 |
|
|
#include "pinocchio/algorithm/jacobian.hpp" |
9 |
|
|
#include "pinocchio/algorithm/frames.hpp" |
10 |
|
|
#include "pinocchio/algorithm/rnea.hpp" |
11 |
|
|
#include "pinocchio/algorithm/crba.hpp" |
12 |
|
|
#include "pinocchio/spatial/act-on-set.hpp" |
13 |
|
|
#include "pinocchio/multibody/sample-models.hpp" |
14 |
|
|
#include "pinocchio/utils/timer.hpp" |
15 |
|
|
#include "pinocchio/algorithm/joint-configuration.hpp" |
16 |
|
|
|
17 |
|
|
#include <iostream> |
18 |
|
|
|
19 |
|
|
#include <boost/test/unit_test.hpp> |
20 |
|
|
#include <boost/utility/binary.hpp> |
21 |
|
|
|
22 |
|
|
template<typename Derived> |
23 |
|
✗ |
inline bool isFinite(const Eigen::MatrixBase<Derived> & x) |
24 |
|
|
{ |
25 |
|
✗ |
return ((x - x).array() == (x - x).array()).all(); |
26 |
|
|
} |
27 |
|
|
|
28 |
|
|
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
29 |
|
|
|
30 |
|
✗ |
BOOST_AUTO_TEST_CASE(frame_basic) |
31 |
|
|
{ |
32 |
|
|
using namespace pinocchio; |
33 |
|
✗ |
Model model; |
34 |
|
✗ |
buildModels::humanoidRandom(model); |
35 |
|
|
|
36 |
|
✗ |
BOOST_CHECK(model.frames.size() >= size_t(model.njoints)); |
37 |
|
✗ |
for (Model::FrameVector::const_iterator it = model.frames.begin(); it != model.frames.end(); ++it) |
38 |
|
|
{ |
39 |
|
✗ |
const Frame & frame = *it; |
40 |
|
✗ |
BOOST_CHECK(frame == frame); |
41 |
|
✗ |
Frame frame_copy(frame); |
42 |
|
✗ |
BOOST_CHECK(frame_copy == frame); |
43 |
|
|
} |
44 |
|
|
|
45 |
|
✗ |
Frame frame1("toto", 0, 0, SE3::Random(), OP_FRAME); |
46 |
|
✗ |
std::ostringstream os; |
47 |
|
✗ |
os << frame1 << std::endl; |
48 |
|
✗ |
BOOST_CHECK(!os.str().empty()); |
49 |
|
|
|
50 |
|
|
// Check other signature |
51 |
|
✗ |
Frame frame2("toto", 0, frame1.placement, OP_FRAME); |
52 |
|
✗ |
BOOST_CHECK(frame1 == frame2); |
53 |
|
|
} |
54 |
|
|
|
55 |
|
✗ |
BOOST_AUTO_TEST_CASE(cast) |
56 |
|
|
{ |
57 |
|
|
using namespace pinocchio; |
58 |
|
✗ |
Frame frame("toto", 0, 0, SE3::Random(), OP_FRAME); |
59 |
|
|
|
60 |
|
✗ |
BOOST_CHECK(frame.cast<double>() == frame); |
61 |
|
✗ |
BOOST_CHECK(frame.cast<double>().cast<long double>() == frame.cast<long double>()); |
62 |
|
|
|
63 |
|
|
typedef FrameTpl<long double> Frameld; |
64 |
|
✗ |
Frameld frame2(frame); |
65 |
|
|
|
66 |
|
✗ |
BOOST_CHECK(frame2 == frame.cast<long double>()); |
67 |
|
|
} |
68 |
|
|
|
69 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_kinematics) |
70 |
|
|
{ |
71 |
|
|
using namespace Eigen; |
72 |
|
|
using namespace pinocchio; |
73 |
|
|
|
74 |
|
✗ |
pinocchio::Model model; |
75 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model); |
76 |
|
✗ |
Model::Index parent_idx = model.existJointName("rarm2_joint") ? model.getJointId("rarm2_joint") |
77 |
|
✗ |
: (Model::Index)(model.njoints - 1); |
78 |
|
✗ |
const std::string & frame_name = std::string(model.names[parent_idx] + "_frame"); |
79 |
|
✗ |
const SE3 & framePlacement = SE3::Random(); |
80 |
|
✗ |
model.addFrame(Frame(frame_name, parent_idx, 0, framePlacement, OP_FRAME)); |
81 |
|
✗ |
pinocchio::Data data(model); |
82 |
|
|
|
83 |
|
✗ |
VectorXd q = VectorXd::Ones(model.nq); |
84 |
|
✗ |
q.middleRows<4>(3).normalize(); |
85 |
|
✗ |
framesForwardKinematics(model, data, q); |
86 |
|
|
|
87 |
|
✗ |
BOOST_CHECK( |
88 |
|
|
data.oMf[model.getFrameId(frame_name)].isApprox(data.oMi[parent_idx] * framePlacement)); |
89 |
|
|
} |
90 |
|
|
|
91 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_update_placements) |
92 |
|
|
{ |
93 |
|
|
using namespace Eigen; |
94 |
|
|
using namespace pinocchio; |
95 |
|
|
|
96 |
|
✗ |
pinocchio::Model model; |
97 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model); |
98 |
|
✗ |
Model::Index parent_idx = model.existJointName("rarm2_joint") ? model.getJointId("rarm2_joint") |
99 |
|
✗ |
: (Model::Index)(model.njoints - 1); |
100 |
|
✗ |
const std::string & frame_name = std::string(model.names[parent_idx] + "_frame"); |
101 |
|
✗ |
const SE3 & framePlacement = SE3::Random(); |
102 |
|
|
Model::FrameIndex frame_idx = |
103 |
|
✗ |
model.addFrame(Frame(frame_name, parent_idx, 0, framePlacement, OP_FRAME)); |
104 |
|
✗ |
pinocchio::Data data(model); |
105 |
|
✗ |
pinocchio::Data data_ref(model); |
106 |
|
|
|
107 |
|
✗ |
VectorXd q = VectorXd::Ones(model.nq); |
108 |
|
✗ |
q.middleRows<4>(3).normalize(); |
109 |
|
|
|
110 |
|
✗ |
forwardKinematics(model, data, q); |
111 |
|
✗ |
updateFramePlacements(model, data); |
112 |
|
|
|
113 |
|
✗ |
framesForwardKinematics(model, data_ref, q); |
114 |
|
|
|
115 |
|
✗ |
BOOST_CHECK(data.oMf[frame_idx].isApprox(data_ref.oMf[frame_idx])); |
116 |
|
|
} |
117 |
|
|
|
118 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_update_single_placement) |
119 |
|
|
{ |
120 |
|
|
using namespace Eigen; |
121 |
|
|
using namespace pinocchio; |
122 |
|
|
|
123 |
|
✗ |
pinocchio::Model model; |
124 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model); |
125 |
|
✗ |
Model::Index parent_idx = model.existJointName("rarm2_joint") ? model.getJointId("rarm2_joint") |
126 |
|
✗ |
: (Model::Index)(model.njoints - 1); |
127 |
|
✗ |
const std::string & frame_name = std::string(model.names[parent_idx] + "_frame"); |
128 |
|
✗ |
const SE3 & framePlacement = SE3::Random(); |
129 |
|
|
Model::FrameIndex frame_idx = |
130 |
|
✗ |
model.addFrame(Frame(frame_name, parent_idx, 0, framePlacement, OP_FRAME)); |
131 |
|
✗ |
pinocchio::Data data(model); |
132 |
|
✗ |
pinocchio::Data data_ref(model); |
133 |
|
|
|
134 |
|
✗ |
VectorXd q = VectorXd::Ones(model.nq); |
135 |
|
✗ |
q.middleRows<4>(3).normalize(); |
136 |
|
|
|
137 |
|
✗ |
forwardKinematics(model, data, q); |
138 |
|
✗ |
updateFramePlacement(model, data, frame_idx); |
139 |
|
|
|
140 |
|
✗ |
framesForwardKinematics(model, data_ref, q); |
141 |
|
|
|
142 |
|
✗ |
BOOST_CHECK(data.oMf[frame_idx].isApprox(data_ref.oMf[frame_idx])); |
143 |
|
|
} |
144 |
|
|
|
145 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_velocity) |
146 |
|
|
{ |
147 |
|
|
using namespace Eigen; |
148 |
|
|
using namespace pinocchio; |
149 |
|
|
|
150 |
|
✗ |
pinocchio::Model model; |
151 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model); |
152 |
|
✗ |
Model::Index parent_idx = model.existJointName("rarm2_joint") ? model.getJointId("rarm2_joint") |
153 |
|
✗ |
: (Model::Index)(model.njoints - 1); |
154 |
|
✗ |
const std::string & frame_name = std::string(model.names[parent_idx] + "_frame"); |
155 |
|
✗ |
const SE3 & framePlacement = SE3::Random(); |
156 |
|
|
Model::FrameIndex frame_idx = |
157 |
|
✗ |
model.addFrame(Frame(frame_name, parent_idx, 0, framePlacement, OP_FRAME)); |
158 |
|
✗ |
pinocchio::Data data(model); |
159 |
|
|
|
160 |
|
✗ |
VectorXd q = VectorXd::Ones(model.nq); |
161 |
|
✗ |
q.middleRows<4>(3).normalize(); |
162 |
|
✗ |
VectorXd v = VectorXd::Ones(model.nv); |
163 |
|
✗ |
forwardKinematics(model, data, q, v); |
164 |
|
|
|
165 |
|
✗ |
Motion vf = getFrameVelocity(model, data, frame_idx); |
166 |
|
|
|
167 |
|
✗ |
BOOST_CHECK(vf.isApprox(framePlacement.actInv(data.v[parent_idx]))); |
168 |
|
|
|
169 |
|
✗ |
pinocchio::Data data_ref(model); |
170 |
|
✗ |
forwardKinematics(model, data_ref, q, v); |
171 |
|
✗ |
updateFramePlacements(model, data_ref); |
172 |
|
✗ |
Motion v_ref = getFrameVelocity(model, data_ref, frame_idx); |
173 |
|
|
|
174 |
|
✗ |
BOOST_CHECK(v_ref.isApprox(getFrameVelocity(model, data, frame_idx))); |
175 |
|
✗ |
BOOST_CHECK(v_ref.isApprox(getFrameVelocity(model, data, frame_idx, LOCAL))); |
176 |
|
✗ |
BOOST_CHECK( |
177 |
|
|
data_ref.oMf[frame_idx].act(v_ref).isApprox(getFrameVelocity(model, data, frame_idx, WORLD))); |
178 |
|
✗ |
BOOST_CHECK(SE3(data_ref.oMf[frame_idx].rotation(), Eigen::Vector3d::Zero()) |
179 |
|
|
.act(v_ref) |
180 |
|
|
.isApprox(getFrameVelocity(model, data, frame_idx, LOCAL_WORLD_ALIGNED))); |
181 |
|
|
} |
182 |
|
|
|
183 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_acceleration) |
184 |
|
|
{ |
185 |
|
|
using namespace Eigen; |
186 |
|
|
using namespace pinocchio; |
187 |
|
|
|
188 |
|
✗ |
pinocchio::Model model; |
189 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model); |
190 |
|
✗ |
Model::Index parent_idx = model.existJointName("rarm2_joint") ? model.getJointId("rarm2_joint") |
191 |
|
✗ |
: (Model::Index)(model.njoints - 1); |
192 |
|
✗ |
const std::string & frame_name = std::string(model.names[parent_idx] + "_frame"); |
193 |
|
✗ |
const SE3 & framePlacement = SE3::Random(); |
194 |
|
|
Model::FrameIndex frame_idx = |
195 |
|
✗ |
model.addFrame(Frame(frame_name, parent_idx, 0, framePlacement, OP_FRAME)); |
196 |
|
✗ |
pinocchio::Data data(model); |
197 |
|
|
|
198 |
|
✗ |
VectorXd q = VectorXd::Ones(model.nq); |
199 |
|
✗ |
q.middleRows<4>(3).normalize(); |
200 |
|
✗ |
VectorXd v = VectorXd::Ones(model.nv); |
201 |
|
✗ |
VectorXd a = VectorXd::Ones(model.nv); |
202 |
|
✗ |
forwardKinematics(model, data, q, v, a); |
203 |
|
|
|
204 |
|
✗ |
Motion af = getFrameAcceleration(model, data, frame_idx); |
205 |
|
|
|
206 |
|
✗ |
BOOST_CHECK(af.isApprox(framePlacement.actInv(data.a[parent_idx]))); |
207 |
|
|
|
208 |
|
✗ |
pinocchio::Data data_ref(model); |
209 |
|
✗ |
forwardKinematics(model, data_ref, q, v, a); |
210 |
|
✗ |
updateFramePlacements(model, data_ref); |
211 |
|
✗ |
Motion a_ref = getFrameAcceleration(model, data_ref, frame_idx); |
212 |
|
|
|
213 |
|
✗ |
BOOST_CHECK(a_ref.isApprox(getFrameAcceleration(model, data, frame_idx))); |
214 |
|
✗ |
BOOST_CHECK(a_ref.isApprox(getFrameAcceleration(model, data, frame_idx, LOCAL))); |
215 |
|
✗ |
BOOST_CHECK(data_ref.oMf[frame_idx].act(a_ref).isApprox( |
216 |
|
|
getFrameAcceleration(model, data, frame_idx, WORLD))); |
217 |
|
✗ |
BOOST_CHECK(SE3(data_ref.oMf[frame_idx].rotation(), Eigen::Vector3d::Zero()) |
218 |
|
|
.act(a_ref) |
219 |
|
|
.isApprox(getFrameAcceleration(model, data, frame_idx, LOCAL_WORLD_ALIGNED))); |
220 |
|
|
} |
221 |
|
|
|
222 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_classic_acceleration) |
223 |
|
|
{ |
224 |
|
|
using namespace Eigen; |
225 |
|
|
using namespace pinocchio; |
226 |
|
|
|
227 |
|
✗ |
pinocchio::Model model; |
228 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model); |
229 |
|
✗ |
Model::Index parent_idx = model.existJointName("rarm2_joint") ? model.getJointId("rarm2_joint") |
230 |
|
✗ |
: (Model::Index)(model.njoints - 1); |
231 |
|
✗ |
const std::string & frame_name = std::string(model.names[parent_idx] + "_frame"); |
232 |
|
✗ |
const SE3 & framePlacement = SE3::Random(); |
233 |
|
|
Model::FrameIndex frame_idx = |
234 |
|
✗ |
model.addFrame(Frame(frame_name, parent_idx, 0, framePlacement, OP_FRAME)); |
235 |
|
✗ |
pinocchio::Data data(model); |
236 |
|
|
|
237 |
|
✗ |
VectorXd q = VectorXd::Ones(model.nq); |
238 |
|
✗ |
q.middleRows<4>(3).normalize(); |
239 |
|
✗ |
VectorXd v = VectorXd::Ones(model.nv); |
240 |
|
✗ |
VectorXd a = VectorXd::Ones(model.nv); |
241 |
|
✗ |
forwardKinematics(model, data, q, v, a); |
242 |
|
|
|
243 |
|
✗ |
Motion vel = framePlacement.actInv(data.v[parent_idx]); |
244 |
|
✗ |
Motion acc = framePlacement.actInv(data.a[parent_idx]); |
245 |
|
✗ |
Vector3d linear; |
246 |
|
|
|
247 |
|
✗ |
Motion acc_classical_local = acc; |
248 |
|
✗ |
linear = acc.linear() + vel.angular().cross(vel.linear()); |
249 |
|
✗ |
acc_classical_local.linear() = linear; |
250 |
|
|
|
251 |
|
✗ |
Motion af = getFrameClassicalAcceleration(model, data, frame_idx); |
252 |
|
|
|
253 |
|
✗ |
BOOST_CHECK(af.isApprox(acc_classical_local)); |
254 |
|
|
|
255 |
|
✗ |
pinocchio::Data data_ref(model); |
256 |
|
✗ |
forwardKinematics(model, data_ref, q, v, a); |
257 |
|
✗ |
updateFramePlacements(model, data_ref); |
258 |
|
|
|
259 |
|
✗ |
SE3 T_ref = data_ref.oMf[frame_idx]; |
260 |
|
✗ |
Motion v_ref = getFrameVelocity(model, data_ref, frame_idx); |
261 |
|
✗ |
Motion a_ref = getFrameAcceleration(model, data_ref, frame_idx); |
262 |
|
|
|
263 |
|
✗ |
Motion acc_classical_local_ref = a_ref; |
264 |
|
✗ |
linear = a_ref.linear() + v_ref.angular().cross(v_ref.linear()); |
265 |
|
✗ |
acc_classical_local_ref.linear() = linear; |
266 |
|
|
|
267 |
|
✗ |
BOOST_CHECK( |
268 |
|
|
acc_classical_local_ref.isApprox(getFrameClassicalAcceleration(model, data, frame_idx))); |
269 |
|
✗ |
BOOST_CHECK( |
270 |
|
|
acc_classical_local_ref.isApprox(getFrameClassicalAcceleration(model, data, frame_idx, LOCAL))); |
271 |
|
|
|
272 |
|
✗ |
Motion vel_world_ref = T_ref.act(v_ref); |
273 |
|
✗ |
Motion acc_classical_world_ref = T_ref.act(a_ref); |
274 |
|
✗ |
linear = acc_classical_world_ref.linear() + vel_world_ref.angular().cross(vel_world_ref.linear()); |
275 |
|
✗ |
acc_classical_world_ref.linear() = linear; |
276 |
|
|
|
277 |
|
✗ |
BOOST_CHECK( |
278 |
|
|
acc_classical_world_ref.isApprox(getFrameClassicalAcceleration(model, data, frame_idx, WORLD))); |
279 |
|
|
|
280 |
|
✗ |
Motion vel_aligned_ref = SE3(T_ref.rotation(), Eigen::Vector3d::Zero()).act(v_ref); |
281 |
|
✗ |
Motion acc_classical_aligned_ref = SE3(T_ref.rotation(), Eigen::Vector3d::Zero()).act(a_ref); |
282 |
|
|
linear = |
283 |
|
✗ |
acc_classical_aligned_ref.linear() + vel_aligned_ref.angular().cross(vel_aligned_ref.linear()); |
284 |
|
✗ |
acc_classical_aligned_ref.linear() = linear; |
285 |
|
|
|
286 |
|
✗ |
BOOST_CHECK(acc_classical_aligned_ref.isApprox( |
287 |
|
|
getFrameClassicalAcceleration(model, data, frame_idx, LOCAL_WORLD_ALIGNED))); |
288 |
|
|
} |
289 |
|
|
|
290 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_frame_getters) |
291 |
|
|
{ |
292 |
|
|
using namespace Eigen; |
293 |
|
|
using namespace pinocchio; |
294 |
|
|
|
295 |
|
|
// Build a simple 1R planar model |
296 |
|
✗ |
Model model; |
297 |
|
✗ |
JointIndex parentId = model.addJoint(0, JointModelRZ(), SE3::Identity(), "Joint1"); |
298 |
|
✗ |
FrameIndex frameId = model.addFrame( |
299 |
|
✗ |
Frame("Frame1", parentId, 0, SE3(Matrix3d::Identity(), Vector3d(1.0, 0.0, 0.0)), OP_FRAME)); |
300 |
|
|
|
301 |
|
✗ |
Data data(model); |
302 |
|
|
|
303 |
|
|
// Predetermined configuration values |
304 |
|
✗ |
VectorXd q(model.nq); |
305 |
|
✗ |
q << M_PI / 2; |
306 |
|
|
|
307 |
|
✗ |
VectorXd v(model.nv); |
308 |
|
✗ |
v << 1.0; |
309 |
|
|
|
310 |
|
✗ |
VectorXd a(model.nv); |
311 |
|
✗ |
a << 0.0; |
312 |
|
|
|
313 |
|
|
// Expected velocity |
314 |
|
✗ |
Motion v_local; |
315 |
|
✗ |
v_local.linear() = Vector3d(0.0, 1.0, 0.0); |
316 |
|
✗ |
v_local.angular() = Vector3d(0.0, 0.0, 1.0); |
317 |
|
|
|
318 |
|
✗ |
Motion v_world; |
319 |
|
✗ |
v_world.linear() = Vector3d::Zero(); |
320 |
|
✗ |
v_world.angular() = Vector3d(0.0, 0.0, 1.0); |
321 |
|
|
|
322 |
|
✗ |
Motion v_align; |
323 |
|
✗ |
v_align.linear() = Vector3d(-1.0, 0.0, 0.0); |
324 |
|
✗ |
v_align.angular() = Vector3d(0.0, 0.0, 1.0); |
325 |
|
|
|
326 |
|
|
// Expected classical acceleration |
327 |
|
✗ |
Motion ac_local; |
328 |
|
✗ |
ac_local.linear() = Vector3d(-1.0, 0.0, 0.0); |
329 |
|
✗ |
ac_local.angular() = Vector3d::Zero(); |
330 |
|
|
|
331 |
|
✗ |
Motion ac_world = Motion::Zero(); |
332 |
|
|
|
333 |
|
✗ |
Motion ac_align; |
334 |
|
✗ |
ac_align.linear() = Vector3d(0.0, -1.0, 0.0); |
335 |
|
✗ |
ac_align.angular() = Vector3d::Zero(); |
336 |
|
|
|
337 |
|
|
// Perform kinematics |
338 |
|
✗ |
forwardKinematics(model, data, q, v, a); |
339 |
|
|
|
340 |
|
|
// Check output velocity |
341 |
|
✗ |
BOOST_CHECK(v_local.isApprox(getFrameVelocity(model, data, frameId))); |
342 |
|
✗ |
BOOST_CHECK(v_local.isApprox(getFrameVelocity(model, data, frameId, LOCAL))); |
343 |
|
✗ |
BOOST_CHECK(v_world.isApprox(getFrameVelocity(model, data, frameId, WORLD))); |
344 |
|
✗ |
BOOST_CHECK(v_align.isApprox(getFrameVelocity(model, data, frameId, LOCAL_WORLD_ALIGNED))); |
345 |
|
|
|
346 |
|
|
// Check output acceleration (all zero) |
347 |
|
✗ |
BOOST_CHECK(getFrameAcceleration(model, data, frameId).isZero()); |
348 |
|
✗ |
BOOST_CHECK(getFrameAcceleration(model, data, frameId, LOCAL).isZero()); |
349 |
|
✗ |
BOOST_CHECK(getFrameAcceleration(model, data, frameId, WORLD).isZero()); |
350 |
|
✗ |
BOOST_CHECK(getFrameAcceleration(model, data, frameId, LOCAL_WORLD_ALIGNED).isZero()); |
351 |
|
|
|
352 |
|
|
// Check output classical acceleration |
353 |
|
✗ |
BOOST_CHECK(ac_local.isApprox(getFrameClassicalAcceleration(model, data, frameId))); |
354 |
|
✗ |
BOOST_CHECK(ac_local.isApprox(getFrameClassicalAcceleration(model, data, frameId, LOCAL))); |
355 |
|
✗ |
BOOST_CHECK(ac_world.isApprox(getFrameClassicalAcceleration(model, data, frameId, WORLD))); |
356 |
|
✗ |
BOOST_CHECK( |
357 |
|
|
ac_align.isApprox(getFrameClassicalAcceleration(model, data, frameId, LOCAL_WORLD_ALIGNED))); |
358 |
|
|
} |
359 |
|
|
|
360 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_get_frame_jacobian) |
361 |
|
|
{ |
362 |
|
|
using namespace Eigen; |
363 |
|
|
using namespace pinocchio; |
364 |
|
|
|
365 |
|
✗ |
Model model; |
366 |
|
✗ |
buildModels::humanoidRandom(model); |
367 |
|
✗ |
Model::Index parent_idx = model.existJointName("rarm2_joint") ? model.getJointId("rarm2_joint") |
368 |
|
✗ |
: (Model::Index)(model.njoints - 1); |
369 |
|
✗ |
const std::string & frame_name = std::string(model.names[parent_idx] + "_frame"); |
370 |
|
✗ |
const SE3 & framePlacement = SE3::Random(); |
371 |
|
✗ |
model.addFrame(Frame(frame_name, parent_idx, 0, framePlacement, OP_FRAME)); |
372 |
|
✗ |
BOOST_CHECK(model.existFrame(frame_name)); |
373 |
|
|
|
374 |
|
✗ |
pinocchio::Data data(model); |
375 |
|
✗ |
pinocchio::Data data_ref(model); |
376 |
|
|
|
377 |
|
✗ |
model.lowerPositionLimit.head<7>().fill(-1.); |
378 |
|
✗ |
model.upperPositionLimit.head<7>().fill(1.); |
379 |
|
✗ |
VectorXd q = randomConfiguration(model); |
380 |
|
✗ |
VectorXd v = VectorXd::Ones(model.nv); |
381 |
|
|
|
382 |
|
|
// In LOCAL frame |
383 |
|
✗ |
const Model::Index idx = model.getFrameId(frame_name); |
384 |
|
✗ |
const Frame & frame = model.frames[idx]; |
385 |
|
✗ |
BOOST_CHECK(frame.placement.isApprox_impl(framePlacement)); |
386 |
|
✗ |
Data::Matrix6x Jjj(6, model.nv); |
387 |
|
✗ |
Jjj.fill(0); |
388 |
|
✗ |
Data::Matrix6x Jff(6, model.nv); |
389 |
|
✗ |
Jff.fill(0); |
390 |
|
✗ |
Data::Matrix6x Jff2(6, model.nv); |
391 |
|
✗ |
Jff2.fill(0); |
392 |
|
|
|
393 |
|
✗ |
computeJointJacobians(model, data, q); |
394 |
|
✗ |
updateFramePlacement(model, data, idx); |
395 |
|
✗ |
getFrameJacobian(model, data, idx, LOCAL, Jff); |
396 |
|
✗ |
BOOST_CHECK(Jff.isApprox(getFrameJacobian(model, data, idx, LOCAL))); |
397 |
|
✗ |
computeJointJacobians(model, data_ref, q); |
398 |
|
✗ |
getJointJacobian(model, data_ref, parent_idx, LOCAL, Jjj); |
399 |
|
|
|
400 |
|
✗ |
Motion nu_frame = Motion(Jff * v); |
401 |
|
✗ |
Motion nu_joint = Motion(Jjj * v); |
402 |
|
|
|
403 |
|
✗ |
const SE3::ActionMatrixType jXf = frame.placement.toActionMatrix(); |
404 |
|
✗ |
Data::Matrix6x Jjj_from_frame(jXf * Jff); |
405 |
|
✗ |
BOOST_CHECK(Jjj_from_frame.isApprox(Jjj)); |
406 |
|
|
|
407 |
|
✗ |
BOOST_CHECK(nu_frame.isApprox(frame.placement.actInv(nu_joint))); |
408 |
|
|
|
409 |
|
|
// In WORLD frame |
410 |
|
✗ |
Jjj.fill(0); |
411 |
|
✗ |
Jff.fill(0); |
412 |
|
✗ |
Jff2.fill(0); |
413 |
|
✗ |
getFrameJacobian(model, data, idx, WORLD, Jff); |
414 |
|
✗ |
BOOST_CHECK(Jff.isApprox(getFrameJacobian(model, data, idx, WORLD))); |
415 |
|
✗ |
getJointJacobian(model, data_ref, parent_idx, WORLD, Jjj); |
416 |
|
✗ |
BOOST_CHECK(Jff.isApprox(Jjj)); |
417 |
|
|
|
418 |
|
✗ |
computeFrameJacobian(model, data, q, idx, WORLD, Jff2); |
419 |
|
|
|
420 |
|
✗ |
BOOST_CHECK(Jff2.isApprox(Jjj)); |
421 |
|
|
|
422 |
|
|
// In WORLD frame |
423 |
|
✗ |
Jjj.fill(0); |
424 |
|
✗ |
Jff.fill(0); |
425 |
|
✗ |
Jff2.fill(0); |
426 |
|
✗ |
getFrameJacobian(model, data, idx, LOCAL_WORLD_ALIGNED, Jff); |
427 |
|
✗ |
BOOST_CHECK(Jff.isApprox(getFrameJacobian(model, data, idx, LOCAL_WORLD_ALIGNED))); |
428 |
|
|
|
429 |
|
✗ |
getJointJacobian(model, data_ref, parent_idx, WORLD, Jjj); |
430 |
|
✗ |
const SE3 oMf_translation(SE3::Matrix3::Identity(), data.oMf[idx].translation()); |
431 |
|
✗ |
Jjj = oMf_translation.toActionMatrixInverse() * Jjj; |
432 |
|
✗ |
BOOST_CHECK(Jff.isApprox(Jjj)); |
433 |
|
|
|
434 |
|
✗ |
computeFrameJacobian(model, data, q, idx, LOCAL_WORLD_ALIGNED, Jff2); |
435 |
|
✗ |
BOOST_CHECK(Jff2.isApprox(Jff)); |
436 |
|
|
} |
437 |
|
|
|
438 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_frame_jacobian) |
439 |
|
|
{ |
440 |
|
|
using namespace Eigen; |
441 |
|
|
using namespace pinocchio; |
442 |
|
|
|
443 |
|
✗ |
Model model; |
444 |
|
✗ |
buildModels::humanoidRandom(model); |
445 |
|
✗ |
Model::Index parent_idx = model.existJointName("rarm2_joint") ? model.getJointId("rarm2_joint") |
446 |
|
✗ |
: (Model::Index)(model.njoints - 1); |
447 |
|
✗ |
const std::string & frame_name = std::string(model.names[parent_idx] + "_frame"); |
448 |
|
✗ |
const SE3 & framePlacement = SE3::Random(); |
449 |
|
✗ |
model.addFrame(Frame(frame_name, parent_idx, 0, framePlacement, OP_FRAME)); |
450 |
|
✗ |
BOOST_CHECK(model.existFrame(frame_name)); |
451 |
|
|
|
452 |
|
✗ |
pinocchio::Data data(model); |
453 |
|
✗ |
pinocchio::Data data_ref(model); |
454 |
|
|
|
455 |
|
✗ |
model.lowerPositionLimit.head<7>().fill(-1.); |
456 |
|
✗ |
model.upperPositionLimit.head<7>().fill(1.); |
457 |
|
✗ |
VectorXd q = randomConfiguration(model); |
458 |
|
✗ |
VectorXd v = VectorXd::Ones(model.nv); |
459 |
|
|
|
460 |
|
✗ |
Model::Index idx = model.getFrameId(frame_name); |
461 |
|
✗ |
const Frame & frame = model.frames[idx]; |
462 |
|
✗ |
BOOST_CHECK(frame.placement.isApprox_impl(framePlacement)); |
463 |
|
✗ |
Data::Matrix6x Jf(6, model.nv); |
464 |
|
✗ |
Jf.fill(0); |
465 |
|
✗ |
Data::Matrix6x Jf2(6, model.nv); |
466 |
|
✗ |
Jf2.fill(0); |
467 |
|
✗ |
Data::Matrix6x Jf_ref(6, model.nv); |
468 |
|
✗ |
Jf_ref.fill(0); |
469 |
|
|
|
470 |
|
✗ |
computeFrameJacobian(model, data_ref, q, idx, Jf); |
471 |
|
|
|
472 |
|
✗ |
computeJointJacobians(model, data_ref, q); |
473 |
|
✗ |
updateFramePlacement(model, data_ref, idx); |
474 |
|
✗ |
getFrameJacobian(model, data_ref, idx, LOCAL, Jf_ref); |
475 |
|
|
|
476 |
|
✗ |
BOOST_CHECK(Jf.isApprox(Jf_ref)); |
477 |
|
|
|
478 |
|
✗ |
computeFrameJacobian(model, data, q, idx, LOCAL, Jf2); |
479 |
|
|
|
480 |
|
✗ |
BOOST_CHECK(Jf2.isApprox(Jf_ref)); |
481 |
|
|
} |
482 |
|
|
|
483 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_frame_jacobian_local_world_oriented) |
484 |
|
|
{ |
485 |
|
|
using namespace Eigen; |
486 |
|
|
using namespace pinocchio; |
487 |
|
|
|
488 |
|
✗ |
Model model; |
489 |
|
✗ |
buildModels::humanoidRandom(model); |
490 |
|
✗ |
Model::Index parent_idx = model.existJointName("rarm2_joint") ? model.getJointId("rarm2_joint") |
491 |
|
✗ |
: (Model::Index)(model.njoints - 1); |
492 |
|
✗ |
const std::string & frame_name = std::string(model.names[parent_idx] + "_frame"); |
493 |
|
✗ |
const SE3 & framePlacement = SE3::Random(); |
494 |
|
✗ |
model.addFrame(Frame(frame_name, parent_idx, 0, framePlacement, OP_FRAME)); |
495 |
|
✗ |
BOOST_CHECK(model.existFrame(frame_name)); |
496 |
|
|
|
497 |
|
✗ |
pinocchio::Data data(model); |
498 |
|
|
|
499 |
|
✗ |
model.lowerPositionLimit.head<7>().fill(-1.); |
500 |
|
✗ |
model.upperPositionLimit.head<7>().fill(1.); |
501 |
|
✗ |
VectorXd q = randomConfiguration(model); |
502 |
|
✗ |
VectorXd v = VectorXd::Ones(model.nv); |
503 |
|
|
|
504 |
|
✗ |
Model::Index idx = model.getFrameId(frame_name); |
505 |
|
✗ |
Data::Matrix6x Jf(6, model.nv); |
506 |
|
✗ |
Jf.fill(0); |
507 |
|
✗ |
Data::Matrix6x Jf2(6, model.nv); |
508 |
|
✗ |
Jf2.fill(0); |
509 |
|
✗ |
Data::Matrix6x Jf_ref(6, model.nv); |
510 |
|
✗ |
Jf_ref.fill(0); |
511 |
|
|
|
512 |
|
✗ |
computeJointJacobians(model, data, q); |
513 |
|
✗ |
updateFramePlacement(model, data, idx); |
514 |
|
✗ |
getFrameJacobian(model, data, idx, LOCAL, Jf_ref); |
515 |
|
|
|
516 |
|
|
// Compute the jacobians. |
517 |
|
✗ |
Jf_ref = SE3(data.oMf[idx].rotation(), Eigen::Vector3d::Zero()).toActionMatrix() * Jf_ref; |
518 |
|
✗ |
getFrameJacobian(model, data, idx, LOCAL_WORLD_ALIGNED, Jf); |
519 |
|
|
|
520 |
|
✗ |
BOOST_CHECK(Jf.isApprox(Jf_ref)); |
521 |
|
|
|
522 |
|
✗ |
computeFrameJacobian(model, data, q, idx, LOCAL_WORLD_ALIGNED, Jf2); |
523 |
|
|
|
524 |
|
✗ |
BOOST_CHECK(Jf2.isApprox(Jf_ref)); |
525 |
|
|
} |
526 |
|
|
|
527 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_frame_jacobian_time_variation) |
528 |
|
|
{ |
529 |
|
|
using namespace Eigen; |
530 |
|
|
using namespace pinocchio; |
531 |
|
|
|
532 |
|
✗ |
pinocchio::Model model; |
533 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model); |
534 |
|
✗ |
Model::Index parent_idx = model.existJointName("rarm2_joint") ? model.getJointId("rarm2_joint") |
535 |
|
✗ |
: (Model::Index)(model.njoints - 1); |
536 |
|
✗ |
const std::string & frame_name = std::string(model.names[parent_idx] + "_frame"); |
537 |
|
✗ |
const SE3 & framePlacement = SE3::Random(); |
538 |
|
✗ |
model.addFrame(Frame(frame_name, parent_idx, 0, framePlacement, OP_FRAME)); |
539 |
|
✗ |
pinocchio::Data data(model); |
540 |
|
✗ |
pinocchio::Data data_ref(model); |
541 |
|
|
|
542 |
|
|
VectorXd q = randomConfiguration( |
543 |
|
✗ |
model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq)); |
544 |
|
✗ |
VectorXd v = VectorXd::Random(model.nv); |
545 |
|
✗ |
VectorXd a = VectorXd::Random(model.nv); |
546 |
|
|
|
547 |
|
✗ |
computeJointJacobiansTimeVariation(model, data, q, v); |
548 |
|
✗ |
updateFramePlacements(model, data); |
549 |
|
|
|
550 |
|
✗ |
forwardKinematics(model, data_ref, q, v, a); |
551 |
|
✗ |
updateFramePlacements(model, data_ref); |
552 |
|
|
|
553 |
|
✗ |
BOOST_CHECK(isFinite(data.dJ)); |
554 |
|
|
|
555 |
|
✗ |
Model::Index idx = model.getFrameId(frame_name); |
556 |
|
✗ |
const Frame & frame = model.frames[idx]; |
557 |
|
✗ |
BOOST_CHECK(frame.placement.isApprox_impl(framePlacement)); |
558 |
|
|
|
559 |
|
✗ |
Data::Matrix6x J(6, model.nv); |
560 |
|
✗ |
J.fill(0.); |
561 |
|
✗ |
Data::Matrix6x dJ(6, model.nv); |
562 |
|
✗ |
dJ.fill(0.); |
563 |
|
|
|
564 |
|
|
// Regarding to the WORLD origin |
565 |
|
✗ |
getFrameJacobian(model, data, idx, WORLD, J); |
566 |
|
✗ |
getFrameJacobianTimeVariation(model, data, idx, WORLD, dJ); |
567 |
|
|
|
568 |
|
✗ |
Motion v_idx(J * v); |
569 |
|
✗ |
const Motion & v_ref_local = frame.placement.actInv(data_ref.v[parent_idx]); |
570 |
|
✗ |
const Motion & v_ref = data_ref.oMf[idx].act(v_ref_local); |
571 |
|
|
|
572 |
|
✗ |
const SE3 & wMf = SE3(data_ref.oMf[idx].rotation(), SE3::Vector3::Zero()); |
573 |
|
✗ |
const Motion & v_ref_local_world_aligned = wMf.act(v_ref_local); |
574 |
|
✗ |
BOOST_CHECK(v_idx.isApprox(v_ref)); |
575 |
|
|
|
576 |
|
✗ |
Motion a_idx(J * a + dJ * v); |
577 |
|
✗ |
const Motion & a_ref_local = frame.placement.actInv(data_ref.a[parent_idx]); |
578 |
|
✗ |
const Motion & a_ref = data_ref.oMf[idx].act(a_ref_local); |
579 |
|
✗ |
const Motion & a_ref_local_world_aligned = wMf.act(a_ref_local); |
580 |
|
✗ |
BOOST_CHECK(a_idx.isApprox(a_ref)); |
581 |
|
|
|
582 |
|
✗ |
J.fill(0.); |
583 |
|
✗ |
dJ.fill(0.); |
584 |
|
|
// Regarding to the LOCAL frame |
585 |
|
✗ |
getFrameJacobian(model, data, idx, LOCAL, J); |
586 |
|
✗ |
getFrameJacobianTimeVariation(model, data, idx, LOCAL, dJ); |
587 |
|
|
|
588 |
|
✗ |
v_idx = (Motion::Vector6)(J * v); |
589 |
|
✗ |
BOOST_CHECK(v_idx.isApprox(v_ref_local)); |
590 |
|
|
|
591 |
|
✗ |
a_idx = (Motion::Vector6)(J * a + dJ * v); |
592 |
|
✗ |
BOOST_CHECK(a_idx.isApprox(a_ref_local)); |
593 |
|
|
|
594 |
|
|
// Regarding to the LOCAL_WORLD_ALIGNED frame |
595 |
|
✗ |
getFrameJacobian(model, data, idx, LOCAL_WORLD_ALIGNED, J); |
596 |
|
✗ |
getFrameJacobianTimeVariation(model, data, idx, LOCAL_WORLD_ALIGNED, dJ); |
597 |
|
|
|
598 |
|
✗ |
v_idx = (Motion::Vector6)(J * v); |
599 |
|
✗ |
BOOST_CHECK(v_idx.isApprox(v_ref_local_world_aligned)); |
600 |
|
|
|
601 |
|
✗ |
a_idx = (Motion::Vector6)(J * a + dJ * v); |
602 |
|
✗ |
BOOST_CHECK(a_idx.isApprox(a_ref_local_world_aligned)); |
603 |
|
|
|
604 |
|
|
// compare to finite differencies |
605 |
|
|
{ |
606 |
|
✗ |
Data data_ref(model), data_ref_plus(model); |
607 |
|
|
|
608 |
|
✗ |
const double alpha = 1e-8; |
609 |
|
✗ |
Eigen::VectorXd q_plus(model.nq); |
610 |
|
✗ |
q_plus = integrate(model, q, alpha * v); |
611 |
|
|
|
612 |
|
|
// data_ref |
613 |
|
✗ |
Data::Matrix6x J_ref_world(6, model.nv), J_ref_local(6, model.nv), |
614 |
|
✗ |
J_ref_local_world_aligned(6, model.nv); |
615 |
|
✗ |
J_ref_world.fill(0.); |
616 |
|
✗ |
J_ref_local.fill(0.); |
617 |
|
✗ |
J_ref_local_world_aligned.fill(0.); |
618 |
|
✗ |
computeJointJacobians(model, data_ref, q); |
619 |
|
✗ |
updateFramePlacements(model, data_ref); |
620 |
|
✗ |
const SE3 & oMf_q = data_ref.oMf[idx]; |
621 |
|
✗ |
getFrameJacobian(model, data_ref, idx, WORLD, J_ref_world); |
622 |
|
✗ |
getFrameJacobian(model, data_ref, idx, LOCAL, J_ref_local); |
623 |
|
✗ |
getFrameJacobian(model, data_ref, idx, LOCAL_WORLD_ALIGNED, J_ref_local_world_aligned); |
624 |
|
|
|
625 |
|
|
// data_ref_plus |
626 |
|
✗ |
Data::Matrix6x J_ref_plus_world(6, model.nv), J_ref_plus_local(6, model.nv), |
627 |
|
✗ |
J_ref_plus_local_world_aligned(6, model.nv); |
628 |
|
✗ |
J_ref_plus_world.fill(0.); |
629 |
|
✗ |
J_ref_plus_local.fill(0.); |
630 |
|
✗ |
J_ref_plus_local_world_aligned.fill(0.); |
631 |
|
✗ |
computeJointJacobians(model, data_ref_plus, q_plus); |
632 |
|
✗ |
updateFramePlacements(model, data_ref_plus); |
633 |
|
✗ |
const SE3 & oMf_q_plus = data_ref_plus.oMf[idx]; |
634 |
|
✗ |
getFrameJacobian(model, data_ref_plus, idx, WORLD, J_ref_plus_world); |
635 |
|
✗ |
getFrameJacobian(model, data_ref_plus, idx, LOCAL, J_ref_plus_local); |
636 |
|
✗ |
getFrameJacobian( |
637 |
|
|
model, data_ref_plus, idx, LOCAL_WORLD_ALIGNED, J_ref_plus_local_world_aligned); |
638 |
|
|
|
639 |
|
|
// Move J_ref_plus_local to reference frame |
640 |
|
✗ |
J_ref_plus_local = (oMf_q.inverse() * oMf_q_plus).toActionMatrix() * (J_ref_plus_local); |
641 |
|
|
|
642 |
|
|
// Move J_ref_plus_local_world_aligned to reference frame |
643 |
|
✗ |
SE3 oMf_translation = SE3::Identity(); |
644 |
|
✗ |
oMf_translation.translation() = oMf_q_plus.translation() - oMf_q.translation(); |
645 |
|
|
J_ref_plus_local_world_aligned = |
646 |
|
✗ |
oMf_translation.toActionMatrix() * (J_ref_plus_local_world_aligned); |
647 |
|
|
|
648 |
|
✗ |
Data::Matrix6x dJ_ref_world(6, model.nv), dJ_ref_local(6, model.nv), |
649 |
|
✗ |
dJ_ref_local_world_aligned(6, model.nv); |
650 |
|
✗ |
dJ_ref_world.fill(0.); |
651 |
|
✗ |
dJ_ref_local.fill(0.); |
652 |
|
✗ |
dJ_ref_local_world_aligned.fill(0.); |
653 |
|
✗ |
dJ_ref_world = (J_ref_plus_world - J_ref_world) / alpha; |
654 |
|
✗ |
dJ_ref_local = (J_ref_plus_local - J_ref_local) / alpha; |
655 |
|
|
dJ_ref_local_world_aligned = |
656 |
|
✗ |
(J_ref_plus_local_world_aligned - J_ref_local_world_aligned) / alpha; |
657 |
|
|
|
658 |
|
|
// data |
659 |
|
✗ |
computeJointJacobiansTimeVariation(model, data, q, v); |
660 |
|
✗ |
forwardKinematics(model, data, q, v); |
661 |
|
✗ |
updateFramePlacements(model, data); |
662 |
|
✗ |
Data::Matrix6x dJ_world(6, model.nv), dJ_local(6, model.nv), |
663 |
|
✗ |
dJ_local_world_aligned(6, model.nv); |
664 |
|
✗ |
dJ_world.fill(0.); |
665 |
|
✗ |
dJ_local.fill(0.); |
666 |
|
✗ |
dJ_local_world_aligned.fill(0.); |
667 |
|
✗ |
getFrameJacobianTimeVariation(model, data, idx, WORLD, dJ_world); |
668 |
|
✗ |
getFrameJacobianTimeVariation(model, data, idx, LOCAL, dJ_local); |
669 |
|
✗ |
getFrameJacobianTimeVariation(model, data, idx, LOCAL_WORLD_ALIGNED, dJ_local_world_aligned); |
670 |
|
|
|
671 |
|
✗ |
BOOST_CHECK(dJ_world.isApprox(dJ_ref_world, sqrt(alpha))); |
672 |
|
✗ |
BOOST_CHECK(dJ_local.isApprox(dJ_ref_local, sqrt(alpha))); |
673 |
|
✗ |
BOOST_CHECK(dJ_local_world_aligned.isApprox(dJ_ref_local_world_aligned, sqrt(alpha))); |
674 |
|
|
} |
675 |
|
|
} |
676 |
|
|
|
677 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_supported_inertia_and_force) |
678 |
|
|
{ |
679 |
|
|
using namespace Eigen; |
680 |
|
|
using namespace pinocchio; |
681 |
|
|
|
682 |
|
|
// Create a humanoid robot |
683 |
|
✗ |
Model model_free; |
684 |
|
✗ |
pinocchio::buildModels::humanoid(model_free, true); |
685 |
|
✗ |
Data data_free(model_free); |
686 |
|
|
|
687 |
|
|
// Set freeflyer limits to allow for randomConfiguration |
688 |
|
✗ |
model_free.lowerPositionLimit.segment(0, 3) << Vector3d::Constant(-1); |
689 |
|
✗ |
model_free.upperPositionLimit.segment(0, 3) << Vector3d::Constant(1); |
690 |
|
|
|
691 |
|
|
// Joint of interest (that will be converted to frame) |
692 |
|
✗ |
const std::string joint_name = "chest2_joint"; |
693 |
|
✗ |
const JointIndex joint_id = model_free.getJointId(joint_name); |
694 |
|
|
|
695 |
|
|
// Duplicate of the model with the joint of interest fixed (to make a frame) |
696 |
|
✗ |
pinocchio::Model model_fix = buildReducedModel(model_free, {joint_id}, neutral(model_free)); |
697 |
|
✗ |
Data data_fix(model_fix); |
698 |
|
✗ |
const FrameIndex frame_id = model_fix.getFrameId(joint_name); |
699 |
|
|
|
700 |
|
|
// Const variable to help convert q, v, a from one model to another |
701 |
|
✗ |
const int joint_idx_q(model_free.joints[joint_id].idx_q()); |
702 |
|
✗ |
const int joint_idx_v(model_free.joints[joint_id].idx_v()); |
703 |
|
|
|
704 |
|
|
// Pick random q, v, a |
705 |
|
✗ |
VectorXd q_free = randomConfiguration(model_free); |
706 |
|
✗ |
VectorXd v_free(VectorXd::Random(model_free.nv)); |
707 |
|
✗ |
VectorXd a_free(VectorXd::Random(model_free.nv)); |
708 |
|
|
|
709 |
|
|
// Set joint of interest to q, v, a = 0 to mimic the fixed joint |
710 |
|
✗ |
q_free[joint_idx_q] = 0; |
711 |
|
✗ |
v_free[joint_idx_v] = 0; |
712 |
|
✗ |
a_free[joint_idx_v] = 0; |
713 |
|
|
|
714 |
|
|
// Adapt configuration for fixed joint model |
715 |
|
✗ |
VectorXd q_fix(model_fix.nq); |
716 |
|
✗ |
q_fix << q_free.head(joint_idx_q), q_free.tail(model_free.nq - joint_idx_q - 1); |
717 |
|
✗ |
VectorXd v_fix(model_fix.nv); |
718 |
|
✗ |
v_fix << v_free.head(joint_idx_v), v_free.tail(model_free.nv - joint_idx_v - 1); |
719 |
|
✗ |
VectorXd a_fix(model_fix.nv); |
720 |
|
✗ |
a_fix << a_free.head(joint_idx_v), a_free.tail(model_free.nv - joint_idx_v - 1); |
721 |
|
|
|
722 |
|
|
// Compute inertia/force for both models differently |
723 |
|
✗ |
forwardKinematics(model_fix, data_fix, q_fix, v_fix, a_fix); |
724 |
|
✗ |
crba(model_free, data_free, q_free, Convention::WORLD); |
725 |
|
|
|
726 |
|
✗ |
Inertia inertia_fix = computeSupportedInertiaByFrame(model_fix, data_fix, frame_id, false); |
727 |
|
✗ |
Inertia inertia_free(model_free.inertias[joint_id]); |
728 |
|
✗ |
BOOST_CHECK(inertia_fix.isApprox(inertia_free)); |
729 |
|
|
|
730 |
|
✗ |
inertia_fix = computeSupportedInertiaByFrame(model_fix, data_fix, frame_id, true); |
731 |
|
✗ |
inertia_free = data_free.oMi[joint_id].actInv(data_free.oYcrb[joint_id]); |
732 |
|
✗ |
BOOST_CHECK(inertia_fix.isApprox(inertia_free)); |
733 |
|
|
|
734 |
|
✗ |
rnea(model_fix, data_fix, q_fix, v_fix, a_fix); |
735 |
|
✗ |
rnea(model_free, data_free, q_free, v_free, a_free); |
736 |
|
|
|
737 |
|
✗ |
Force force_fix = computeSupportedForceByFrame(model_fix, data_fix, frame_id); |
738 |
|
✗ |
Force force_free(data_free.f[joint_id]); |
739 |
|
✗ |
BOOST_CHECK(force_fix.isApprox(force_free)); |
740 |
|
|
} |
741 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
742 |
|
|
|