Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2016-2022 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include "pinocchio/multibody/data.hpp" |
6 |
|
|
#include "pinocchio/multibody/model.hpp" |
7 |
|
|
|
8 |
|
|
#include "pinocchio/algorithm/check.hpp" |
9 |
|
|
#include "pinocchio/algorithm/model.hpp" |
10 |
|
|
#include "pinocchio/algorithm/kinematics.hpp" |
11 |
|
|
#include "pinocchio/algorithm/frames.hpp" |
12 |
|
|
#include "pinocchio/algorithm/joint-configuration.hpp" |
13 |
|
|
#include "pinocchio/algorithm/geometry.hpp" |
14 |
|
|
#include "pinocchio/algorithm/center-of-mass.hpp" |
15 |
|
|
|
16 |
|
|
#include "pinocchio/multibody/sample-models.hpp" |
17 |
|
|
#include "pinocchio/spatial/fwd.hpp" |
18 |
|
|
|
19 |
|
|
#include <boost/test/unit_test.hpp> |
20 |
|
|
#include <boost/utility/binary.hpp> |
21 |
|
|
|
22 |
|
|
using namespace pinocchio; |
23 |
|
|
|
24 |
|
|
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
25 |
|
|
|
26 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_model_subtree) |
27 |
|
|
{ |
28 |
|
✗ |
Model model; |
29 |
|
✗ |
buildModels::humanoidRandom(model); |
30 |
|
|
|
31 |
|
✗ |
Model::JointIndex idx_larm1 = model.getJointId("larm1_joint"); |
32 |
|
✗ |
BOOST_CHECK(idx_larm1 < (Model::JointIndex)model.njoints); |
33 |
|
✗ |
Model::IndexVector & subtree = model.subtrees[idx_larm1]; |
34 |
|
✗ |
BOOST_CHECK(subtree.size() == 6); |
35 |
|
|
|
36 |
|
✗ |
for (size_t joint_id = 0; joint_id < model.joints.size(); ++joint_id) |
37 |
|
|
{ |
38 |
|
✗ |
const Model::IndexVector & children = model.children[joint_id]; |
39 |
|
✗ |
for (size_t i = 0; i < children.size(); ++i) |
40 |
|
|
{ |
41 |
|
✗ |
BOOST_CHECK(model.parents[children[i]] == joint_id); |
42 |
|
|
} |
43 |
|
|
} |
44 |
|
|
|
45 |
|
✗ |
for (size_t i = 1; i < subtree.size(); ++i) |
46 |
|
|
{ |
47 |
|
✗ |
BOOST_CHECK(model.parents[subtree[i]] == subtree[i - 1]); |
48 |
|
|
} |
49 |
|
|
|
50 |
|
|
// Check that i starts subtree[i] |
51 |
|
✗ |
for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) |
52 |
|
|
{ |
53 |
|
✗ |
BOOST_CHECK(model.subtrees[joint_id][0] == joint_id); |
54 |
|
|
} |
55 |
|
|
|
56 |
|
|
// Check that subtree[0] contains all joint ids |
57 |
|
✗ |
for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) |
58 |
|
|
{ |
59 |
|
✗ |
BOOST_CHECK(model.subtrees[0][joint_id - 1] == joint_id); |
60 |
|
|
} |
61 |
|
|
} |
62 |
|
|
|
63 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_model_get_frame_id) |
64 |
|
|
{ |
65 |
|
✗ |
Model model; |
66 |
|
✗ |
buildModels::humanoidRandom(model); |
67 |
|
|
|
68 |
|
✗ |
for (FrameIndex i = 0; i < static_cast<FrameIndex>(model.nframes); i++) |
69 |
|
|
{ |
70 |
|
✗ |
BOOST_CHECK_EQUAL(i, model.getFrameId(model.frames[i].name)); |
71 |
|
|
} |
72 |
|
✗ |
BOOST_CHECK_EQUAL(model.nframes, model.getFrameId("NOT_A_FRAME")); |
73 |
|
|
} |
74 |
|
|
|
75 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_model_support) |
76 |
|
|
{ |
77 |
|
✗ |
Model model; |
78 |
|
✗ |
buildModels::humanoidRandom(model); |
79 |
|
✗ |
const Model::IndexVector support0_ref(1, 0); |
80 |
|
✗ |
BOOST_CHECK(model.supports[0] == support0_ref); |
81 |
|
|
|
82 |
|
|
// Check that i ends supports[i] |
83 |
|
✗ |
for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) |
84 |
|
|
{ |
85 |
|
✗ |
BOOST_CHECK(model.supports[joint_id].back() == joint_id); |
86 |
|
✗ |
Model::IndexVector & support = model.supports[joint_id]; |
87 |
|
|
|
88 |
|
✗ |
size_t current_id = support.size() - 2; |
89 |
|
✗ |
for (JointIndex parent_id = model.parents[joint_id]; parent_id > 0; |
90 |
|
✗ |
parent_id = model.parents[parent_id], current_id--) |
91 |
|
|
{ |
92 |
|
✗ |
BOOST_CHECK(parent_id == support[current_id]); |
93 |
|
|
} |
94 |
|
|
} |
95 |
|
|
} |
96 |
|
|
|
97 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_model_subspace_dimensions) |
98 |
|
|
{ |
99 |
|
✗ |
Model model; |
100 |
|
✗ |
buildModels::humanoidRandom(model); |
101 |
|
|
|
102 |
|
|
// Check that i ends supports[i] |
103 |
|
✗ |
for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) |
104 |
|
|
{ |
105 |
|
✗ |
const Model::JointModel & jmodel = model.joints[joint_id]; |
106 |
|
|
|
107 |
|
✗ |
BOOST_CHECK(model.nqs[joint_id] == jmodel.nq()); |
108 |
|
✗ |
BOOST_CHECK(model.idx_qs[joint_id] == jmodel.idx_q()); |
109 |
|
|
|
110 |
|
✗ |
BOOST_CHECK(model.nvs[joint_id] == jmodel.nv()); |
111 |
|
✗ |
BOOST_CHECK(model.idx_vs[joint_id] == jmodel.idx_v()); |
112 |
|
|
} |
113 |
|
|
} |
114 |
|
|
|
115 |
|
✗ |
BOOST_AUTO_TEST_CASE(comparison) |
116 |
|
|
{ |
117 |
|
✗ |
Model model; |
118 |
|
✗ |
buildModels::humanoidRandom(model); |
119 |
|
|
|
120 |
|
✗ |
BOOST_CHECK(model == model); |
121 |
|
|
} |
122 |
|
|
|
123 |
|
✗ |
BOOST_AUTO_TEST_CASE(cast) |
124 |
|
|
{ |
125 |
|
✗ |
Model model; |
126 |
|
✗ |
buildModels::humanoidRandom(model); |
127 |
|
|
|
128 |
|
✗ |
BOOST_CHECK(model.cast<double>() == model.cast<double>()); |
129 |
|
✗ |
BOOST_CHECK(model.cast<double>().cast<long double>() == model.cast<long double>()); |
130 |
|
|
|
131 |
|
|
typedef ModelTpl<long double> Modelld; |
132 |
|
|
|
133 |
|
✗ |
Modelld model2(model); |
134 |
|
✗ |
BOOST_CHECK(model2 == model.cast<long double>()); |
135 |
|
|
} |
136 |
|
|
|
137 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_std_vector_of_Model) |
138 |
|
|
{ |
139 |
|
✗ |
Model model; |
140 |
|
✗ |
buildModels::humanoid(model); |
141 |
|
|
|
142 |
|
✗ |
PINOCCHIO_ALIGNED_STD_VECTOR(Model) models; |
143 |
|
✗ |
for (size_t k = 0; k < 20; ++k) |
144 |
|
|
{ |
145 |
|
✗ |
models.push_back(Model()); |
146 |
|
✗ |
buildModels::humanoid(models.back()); |
147 |
|
|
|
148 |
|
✗ |
BOOST_CHECK(model == models.back()); |
149 |
|
|
} |
150 |
|
|
} |
151 |
|
|
|
152 |
|
|
#ifdef PINOCCHIO_WITH_HPP_FCL |
153 |
|
|
struct AddPrefix |
154 |
|
|
{ |
155 |
|
|
std::string p; |
156 |
|
✗ |
std::string operator()(const std::string & n) |
157 |
|
|
{ |
158 |
|
✗ |
return p + n; |
159 |
|
|
} |
160 |
|
✗ |
Frame operator()(const Frame & _f) |
161 |
|
|
{ |
162 |
|
✗ |
Frame f(_f); |
163 |
|
✗ |
f.name = p + f.name; |
164 |
|
✗ |
return f; |
165 |
|
|
} |
166 |
|
✗ |
AddPrefix(const char * _p) |
167 |
|
✗ |
: p(_p) |
168 |
|
|
{ |
169 |
|
|
} |
170 |
|
|
}; |
171 |
|
|
|
172 |
|
✗ |
BOOST_AUTO_TEST_CASE(append) |
173 |
|
|
{ |
174 |
|
✗ |
Model manipulator, humanoid; |
175 |
|
✗ |
GeometryModel geomManipulator, geomHumanoid; |
176 |
|
|
|
177 |
|
✗ |
buildModels::manipulator(manipulator); |
178 |
|
✗ |
buildModels::manipulatorGeometries(manipulator, geomManipulator); |
179 |
|
✗ |
geomManipulator.addAllCollisionPairs(); |
180 |
|
|
// Add prefix to joint and frame names |
181 |
|
✗ |
AddPrefix addManipulatorPrefix("manipulator/"); |
182 |
|
✗ |
std::transform( |
183 |
|
✗ |
++manipulator.names.begin(), manipulator.names.end(), ++manipulator.names.begin(), |
184 |
|
|
addManipulatorPrefix); |
185 |
|
✗ |
std::transform( |
186 |
|
✗ |
++manipulator.frames.begin(), manipulator.frames.end(), ++manipulator.frames.begin(), |
187 |
|
|
addManipulatorPrefix); |
188 |
|
|
|
189 |
|
✗ |
BOOST_TEST_MESSAGE(manipulator); |
190 |
|
|
|
191 |
|
✗ |
buildModels::humanoid(humanoid); |
192 |
|
✗ |
buildModels::humanoidGeometries(humanoid, geomHumanoid); |
193 |
|
✗ |
geomHumanoid.addAllCollisionPairs(); |
194 |
|
|
// Add prefix to joint and frame names |
195 |
|
✗ |
AddPrefix addHumanoidPrefix("humanoid/"); |
196 |
|
✗ |
std::transform( |
197 |
|
✗ |
++humanoid.names.begin(), humanoid.names.end(), ++humanoid.names.begin(), addHumanoidPrefix); |
198 |
|
✗ |
std::transform( |
199 |
|
✗ |
++humanoid.frames.begin(), humanoid.frames.end(), ++humanoid.frames.begin(), addHumanoidPrefix); |
200 |
|
|
|
201 |
|
✗ |
BOOST_TEST_MESSAGE(humanoid); |
202 |
|
|
|
203 |
|
✗ |
typename Model::ConfigVectorType humanoid_config_vector(humanoid.nq); |
204 |
|
✗ |
typename Model::ConfigVectorType manipulator_config_vector(manipulator.nq); |
205 |
|
✗ |
humanoid_config_vector = randomConfiguration(humanoid); |
206 |
|
✗ |
manipulator_config_vector = randomConfiguration(manipulator); |
207 |
|
✗ |
humanoid.referenceConfigurations.insert(std::make_pair("common_key", humanoid_config_vector)); |
208 |
|
✗ |
manipulator.referenceConfigurations.insert( |
209 |
|
✗ |
std::make_pair("common_key", manipulator_config_vector)); |
210 |
|
|
|
211 |
|
✗ |
humanoid_config_vector = randomConfiguration(humanoid); |
212 |
|
✗ |
manipulator_config_vector = randomConfiguration(manipulator); |
213 |
|
✗ |
humanoid.referenceConfigurations.insert(std::make_pair("humanoid_key", humanoid_config_vector)); |
214 |
|
✗ |
manipulator.referenceConfigurations.insert( |
215 |
|
✗ |
std::make_pair("manipulator_key", manipulator_config_vector)); |
216 |
|
|
|
217 |
|
|
// TODO fix inertia of the base |
218 |
|
✗ |
manipulator.inertias[0].setRandom(); |
219 |
|
✗ |
SE3 aMb = SE3::Random(); |
220 |
|
|
|
221 |
|
|
// First append a model to the universe frame. |
222 |
|
✗ |
Model model1; |
223 |
|
✗ |
GeometryModel geomModel1; |
224 |
|
✗ |
FrameIndex fid = 0; |
225 |
|
✗ |
appendModel( |
226 |
|
✗ |
humanoid, manipulator, geomHumanoid, geomManipulator, fid, SE3::Identity(), model1, geomModel1); |
227 |
|
|
typedef typename Model::ConfigVectorMap ConfigVectorMap; |
228 |
|
|
|
229 |
|
✗ |
typename Model::ConfigVectorType neutral_config_vector(model1.nq); |
230 |
|
✗ |
neutral(model1, neutral_config_vector); |
231 |
|
|
|
232 |
|
✗ |
BOOST_CHECK(model1.referenceConfigurations.size() == 3); |
233 |
|
✗ |
for (typename ConfigVectorMap::const_iterator config_it = model1.referenceConfigurations.begin(); |
234 |
|
✗ |
config_it != model1.referenceConfigurations.end(); ++config_it) |
235 |
|
|
{ |
236 |
|
✗ |
const std::string & config_name = config_it->first; |
237 |
|
✗ |
const typename Model::ConfigVectorType & config_vector = config_it->second; |
238 |
|
|
|
239 |
|
|
typename ConfigVectorMap::const_iterator humanoid_config = |
240 |
|
✗ |
humanoid.referenceConfigurations.find(config_name); |
241 |
|
|
typename ConfigVectorMap::const_iterator manipulator_config = |
242 |
|
✗ |
manipulator.referenceConfigurations.find(config_name); |
243 |
|
✗ |
for (JointIndex joint_id = 1; joint_id < model1.joints.size(); ++joint_id) |
244 |
|
|
{ |
245 |
|
✗ |
const JointModel & joint_model1 = model1.joints[joint_id]; |
246 |
|
✗ |
if ( |
247 |
|
✗ |
humanoid_config != humanoid.referenceConfigurations.end() |
248 |
|
✗ |
&& humanoid.existJointName(model1.names[joint_id])) |
249 |
|
|
{ // key and joint exists in humanoid |
250 |
|
|
const JointModel & joint_model_humanoid = |
251 |
|
✗ |
humanoid.joints[humanoid.getJointId(model1.names[joint_id])]; |
252 |
|
✗ |
BOOST_CHECK( |
253 |
|
|
joint_model_humanoid.jointConfigSelector(humanoid_config->second) |
254 |
|
|
== joint_model1.jointConfigSelector(config_vector)); |
255 |
|
|
// std::cerr<<"humanoid "<<config_name<<" "<<model1.names[joint_id]<<std::endl; |
256 |
|
|
} |
257 |
|
✗ |
else if ( |
258 |
|
✗ |
manipulator_config != manipulator.referenceConfigurations.end() |
259 |
|
✗ |
&& manipulator.existJointName(model1.names[joint_id])) |
260 |
|
|
{ // key and joint exists in manipulator. |
261 |
|
|
const JointModel & joint_model_manipulator = |
262 |
|
✗ |
manipulator.joints[manipulator.getJointId(model1.names[joint_id])]; |
263 |
|
✗ |
BOOST_CHECK( |
264 |
|
|
joint_model_manipulator.jointConfigSelector(manipulator_config->second) |
265 |
|
|
== joint_model1.jointConfigSelector(config_vector)); |
266 |
|
|
// std::cerr<<"manipulator "<<config_name<<" "<<model1.names[joint_id]<<std::endl; |
267 |
|
|
} |
268 |
|
|
else |
269 |
|
|
{ // joint and key combo not found, should with neutral |
270 |
|
✗ |
BOOST_CHECK( |
271 |
|
|
joint_model1.jointConfigSelector(neutral_config_vector) |
272 |
|
|
== joint_model1.jointConfigSelector(config_vector)); |
273 |
|
|
// std::cerr<<"neutral "<<config_name<<" "<<model1.names[joint_id]<<std::endl; |
274 |
|
|
} |
275 |
|
|
} |
276 |
|
|
} |
277 |
|
|
|
278 |
|
|
{ |
279 |
|
✗ |
Model model2 = appendModel(humanoid, manipulator, fid, SE3::Identity()); |
280 |
|
✗ |
Model model3; |
281 |
|
✗ |
appendModel(humanoid, manipulator, fid, SE3::Identity(), model3); |
282 |
|
✗ |
BOOST_CHECK(model1 == model2); |
283 |
|
✗ |
BOOST_CHECK(model1 == model3); |
284 |
|
✗ |
BOOST_CHECK(model2 == model3); |
285 |
|
|
} |
286 |
|
|
|
287 |
|
✗ |
Data data1(model1); |
288 |
|
✗ |
BOOST_CHECK(model1.check(data1)); |
289 |
|
|
|
290 |
|
✗ |
BOOST_TEST_MESSAGE(model1); |
291 |
|
|
|
292 |
|
|
// Second, append a model to a moving frame. |
293 |
|
✗ |
Model model; |
294 |
|
|
|
295 |
|
✗ |
GeometryModel geomModel; |
296 |
|
✗ |
fid = humanoid.addFrame(Frame( |
297 |
|
|
"humanoid/add_manipulator", humanoid.getJointId("humanoid/chest2_joint"), |
298 |
|
|
humanoid.getFrameId("humanoid/chest2_joint"), aMb, OP_FRAME)); |
299 |
|
|
|
300 |
|
|
// Append manipulator to chest2_joint of humanoid |
301 |
|
✗ |
appendModel( |
302 |
|
✗ |
humanoid, manipulator, geomHumanoid, geomManipulator, fid, SE3::Identity(), model, geomModel); |
303 |
|
|
|
304 |
|
✗ |
neutral_config_vector.resize(model.nq); |
305 |
|
✗ |
neutral(model, neutral_config_vector); |
306 |
|
|
|
307 |
|
✗ |
BOOST_CHECK(model.referenceConfigurations.size() == 3); |
308 |
|
✗ |
for (typename ConfigVectorMap::const_iterator config_it = model.referenceConfigurations.begin(); |
309 |
|
✗ |
config_it != model.referenceConfigurations.end(); ++config_it) |
310 |
|
|
{ |
311 |
|
✗ |
const std::string & config_name = config_it->first; |
312 |
|
✗ |
const typename Model::ConfigVectorType & config_vector = config_it->second; |
313 |
|
|
|
314 |
|
|
typename ConfigVectorMap::const_iterator humanoid_config = |
315 |
|
✗ |
humanoid.referenceConfigurations.find(config_name); |
316 |
|
|
typename ConfigVectorMap::const_iterator manipulator_config = |
317 |
|
✗ |
manipulator.referenceConfigurations.find(config_name); |
318 |
|
✗ |
for (JointIndex joint_id = 1; joint_id < model.joints.size(); ++joint_id) |
319 |
|
|
{ |
320 |
|
✗ |
const JointModel & joint_model = model.joints[joint_id]; |
321 |
|
✗ |
if ( |
322 |
|
✗ |
humanoid_config != humanoid.referenceConfigurations.end() |
323 |
|
✗ |
&& humanoid.existJointName(model.names[joint_id])) |
324 |
|
|
{ // key and joint exists in humanoid |
325 |
|
|
const JointModel & joint_model_humanoid = |
326 |
|
✗ |
humanoid.joints[humanoid.getJointId(model.names[joint_id])]; |
327 |
|
✗ |
BOOST_CHECK( |
328 |
|
|
joint_model_humanoid.jointConfigSelector(humanoid_config->second) |
329 |
|
|
== joint_model.jointConfigSelector(config_vector)); |
330 |
|
|
// std::cerr<<"humanoid "<<config_name<<" "<<model.names[joint_id]<<std::endl; |
331 |
|
|
} |
332 |
|
✗ |
else if ( |
333 |
|
✗ |
manipulator_config != manipulator.referenceConfigurations.end() |
334 |
|
✗ |
&& manipulator.existJointName(model.names[joint_id])) |
335 |
|
|
{ // key and joint exists in manipulator. |
336 |
|
|
const JointModel & joint_model_manipulator = |
337 |
|
✗ |
manipulator.joints[manipulator.getJointId(model.names[joint_id])]; |
338 |
|
✗ |
BOOST_CHECK( |
339 |
|
|
joint_model_manipulator.jointConfigSelector(manipulator_config->second) |
340 |
|
|
== joint_model.jointConfigSelector(config_vector)); |
341 |
|
|
// std::cerr<<"manipulator "<<config_name<<" "<<model.names[joint_id]<<std::endl; |
342 |
|
|
} |
343 |
|
|
else |
344 |
|
|
{ // joint and key combo not found, should with neutral |
345 |
|
✗ |
BOOST_CHECK( |
346 |
|
|
joint_model.jointConfigSelector(neutral_config_vector) |
347 |
|
|
== joint_model.jointConfigSelector(config_vector)); |
348 |
|
|
// std::cerr<<"neutral "<<config_name<<" "<<model.names[joint_id]<<std::endl; |
349 |
|
|
} |
350 |
|
|
} |
351 |
|
|
} |
352 |
|
|
|
353 |
|
|
{ |
354 |
|
✗ |
Model model2 = appendModel(humanoid, manipulator, fid, SE3::Identity()); |
355 |
|
✗ |
Model model3; |
356 |
|
✗ |
appendModel(humanoid, manipulator, fid, SE3::Identity(), model3); |
357 |
|
✗ |
BOOST_CHECK(model == model2); |
358 |
|
✗ |
BOOST_CHECK(model == model3); |
359 |
|
✗ |
BOOST_CHECK(model2 == model3); |
360 |
|
|
} |
361 |
|
|
|
362 |
|
✗ |
BOOST_TEST_MESSAGE(model); |
363 |
|
|
|
364 |
|
✗ |
Data data(model); |
365 |
|
✗ |
BOOST_CHECK(model.check(data)); |
366 |
|
|
|
367 |
|
|
// Check the model |
368 |
|
✗ |
BOOST_CHECK_EQUAL( |
369 |
|
|
model.getJointId("humanoid/chest2_joint"), |
370 |
|
|
model.parents[model.getJointId("manipulator/shoulder1_joint")]); |
371 |
|
|
|
372 |
|
|
// check the joint order and the inertias |
373 |
|
|
// All the joints of the manipulator should be at the end of the merged model |
374 |
|
✗ |
JointIndex hnj = (JointIndex)humanoid.njoints; |
375 |
|
✗ |
JointIndex chest2 = model.getJointId("humanoid/chest2_joint"); |
376 |
|
✗ |
for (JointIndex jid = 1; jid < hnj; ++jid) |
377 |
|
|
{ |
378 |
|
✗ |
BOOST_TEST_MESSAGE("Checking joint " << jid << " " << model.names[jid]); |
379 |
|
✗ |
BOOST_CHECK_EQUAL(model.names[jid], humanoid.names[jid]); |
380 |
|
✗ |
if (jid != chest2) |
381 |
|
✗ |
BOOST_CHECK_EQUAL(model.inertias[jid], humanoid.inertias[jid]); |
382 |
|
|
else |
383 |
|
✗ |
BOOST_CHECK_MESSAGE( |
384 |
|
|
model.inertias[jid].isApprox( |
385 |
|
|
manipulator.inertias[0].se3Action(aMb) + humanoid.inertias[jid]), |
386 |
|
|
model.inertias[jid] << " != " |
387 |
|
|
<< manipulator.inertias[0].se3Action(aMb) + humanoid.inertias[jid]); |
388 |
|
|
|
389 |
|
✗ |
BOOST_CHECK_EQUAL(model.jointPlacements[jid], humanoid.jointPlacements[jid]); |
390 |
|
|
} |
391 |
|
✗ |
for (JointIndex jid = 1; jid < manipulator.joints.size() - 1; ++jid) |
392 |
|
|
{ |
393 |
|
✗ |
BOOST_TEST_MESSAGE("Checking joint " << hnj - 1 + jid << " " << model.names[hnj + jid]); |
394 |
|
✗ |
BOOST_CHECK_EQUAL(model.names[hnj - 1 + jid], manipulator.names[jid]); |
395 |
|
✗ |
BOOST_CHECK_EQUAL(model.inertias[hnj - 1 + jid], manipulator.inertias[jid]); |
396 |
|
✗ |
if (jid == 1) |
397 |
|
✗ |
BOOST_CHECK_EQUAL( |
398 |
|
|
model.jointPlacements[hnj - 1 + jid], aMb * manipulator.jointPlacements[jid]); |
399 |
|
|
else |
400 |
|
✗ |
BOOST_CHECK_EQUAL(model.jointPlacements[hnj - 1 + jid], manipulator.jointPlacements[jid]); |
401 |
|
|
} |
402 |
|
|
// Check the frames |
403 |
|
✗ |
for (FrameIndex fid = 1; fid < humanoid.frames.size(); ++fid) |
404 |
|
|
{ |
405 |
|
✗ |
const Frame &frame = humanoid.frames[fid], parent = humanoid.frames[frame.parentFrame]; |
406 |
|
✗ |
BOOST_CHECK(model.existFrame(frame.name, frame.type)); |
407 |
|
✗ |
const Frame &nframe = model.frames[model.getFrameId(frame.name, frame.type)], |
408 |
|
✗ |
nparent = model.frames[nframe.parentFrame]; |
409 |
|
✗ |
BOOST_CHECK_EQUAL(parent.name, nparent.name); |
410 |
|
✗ |
BOOST_CHECK_EQUAL(frame.placement, nframe.placement); |
411 |
|
|
} |
412 |
|
✗ |
for (FrameIndex fid = 1; fid < manipulator.frames.size(); ++fid) |
413 |
|
|
{ |
414 |
|
✗ |
const Frame &frame = manipulator.frames[fid], parent = manipulator.frames[frame.parentFrame]; |
415 |
|
✗ |
BOOST_CHECK(model.existFrame(frame.name, frame.type)); |
416 |
|
✗ |
const Frame &nframe = model.frames[model.getFrameId(frame.name, frame.type)], |
417 |
|
✗ |
nparent = model.frames[nframe.parentFrame]; |
418 |
|
✗ |
if (frame.parentFrame > 0) |
419 |
|
|
{ |
420 |
|
✗ |
BOOST_CHECK_EQUAL(parent.name, nparent.name); |
421 |
|
✗ |
BOOST_CHECK_EQUAL(frame.placement, nframe.placement); |
422 |
|
|
} |
423 |
|
|
} |
424 |
|
|
|
425 |
|
|
{ |
426 |
|
✗ |
Inertia inertia(2., Eigen::Vector3d(0.1, 0.1, 0.1), Eigen::Matrix3d::Identity()); |
427 |
|
✗ |
Frame additional_frame("inertial_frame", 2, SE3::Identity(), FrameType::JOINT, inertia); |
428 |
|
✗ |
humanoid.addFrame(additional_frame); |
429 |
|
✗ |
double mass_humanoid = computeTotalMass(humanoid); |
430 |
|
✗ |
double mass_manipulator = computeTotalMass(manipulator); |
431 |
|
✗ |
double total_mass = mass_manipulator + mass_humanoid; |
432 |
|
|
|
433 |
|
✗ |
Model model4; |
434 |
|
✗ |
GeometryModel geomModel4; |
435 |
|
✗ |
appendModel( |
436 |
|
✗ |
humanoid, manipulator, geomHumanoid, geomManipulator, 0, SE3::Identity(), model4, geomModel4); |
437 |
|
✗ |
BOOST_CHECK_CLOSE(computeTotalMass(model4), total_mass, 1e-6); |
438 |
|
|
} |
439 |
|
|
{ |
440 |
|
✗ |
Model ff_model; |
441 |
|
✗ |
auto ff_id = ff_model.addJoint(0, JointModelFreeFlyer(), SE3::Identity(), "floating_base"); |
442 |
|
✗ |
ff_model.addJointFrame(ff_id); |
443 |
|
✗ |
GeometryModel ff_geom_model = GeometryModel(); |
444 |
|
✗ |
FrameIndex frame_id = ff_model.getFrameId("floating_base"); |
445 |
|
✗ |
Model model4; |
446 |
|
✗ |
GeometryModel geomModel4; |
447 |
|
✗ |
appendModel( |
448 |
|
✗ |
ff_model, manipulator, ff_geom_model, geomManipulator, frame_id, SE3::Identity(), model4, |
449 |
|
|
geomModel4); |
450 |
|
✗ |
BOOST_CHECK(model4.inertias[1] == model4.inertias[1]); |
451 |
|
|
} |
452 |
|
|
|
453 |
|
|
{ |
454 |
|
✗ |
Model model5, gripperModel; |
455 |
|
|
|
456 |
|
✗ |
Inertia inertia(1., SE3::Vector3(0.5, 0., 0.0), SE3::Matrix3::Identity()); |
457 |
|
✗ |
SE3 pos(1); |
458 |
|
|
|
459 |
|
✗ |
pos.translation() = SE3::LinearType(0.1, 0., 0.); |
460 |
|
✗ |
JointIndex idx = gripperModel.addJoint(0, JointModelPX(), pos, "left_finger"); |
461 |
|
✗ |
gripperModel.addJointFrame(idx); |
462 |
|
|
|
463 |
|
✗ |
pos.translation() = SE3::LinearType(-0.1, 0., 0.); |
464 |
|
✗ |
idx = gripperModel.addJoint(0, JointModelPX(), pos, "right_finger"); |
465 |
|
✗ |
gripperModel.addJointFrame(idx); |
466 |
|
|
|
467 |
|
✗ |
SE3 transformManGr = SE3::Random(); |
468 |
|
✗ |
FrameIndex fid = (FrameIndex)(manipulator.frames.size() - 1); |
469 |
|
✗ |
appendModel(manipulator, gripperModel, fid, transformManGr, model5); |
470 |
|
|
|
471 |
|
✗ |
JointIndex jid5 = model5.getJointId("left_finger"); |
472 |
|
✗ |
JointIndex jidG = gripperModel.getJointId("left_finger"); |
473 |
|
✗ |
BOOST_CHECK( |
474 |
|
|
model5.jointPlacements[jid5].isApprox(transformManGr * gripperModel.jointPlacements[jidG])); |
475 |
|
|
|
476 |
|
✗ |
jid5 = model5.getJointId("right_finger"); |
477 |
|
✗ |
jidG = gripperModel.getJointId("right_finger"); |
478 |
|
✗ |
BOOST_CHECK( |
479 |
|
|
model5.jointPlacements[jid5].isApprox(transformManGr * gripperModel.jointPlacements[jidG])); |
480 |
|
|
} |
481 |
|
|
} |
482 |
|
|
#endif |
483 |
|
|
|
484 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_buildReducedModel_empty) |
485 |
|
|
{ |
486 |
|
✗ |
Model humanoid_model; |
487 |
|
✗ |
buildModels::humanoid(humanoid_model); |
488 |
|
|
|
489 |
|
✗ |
static const std::vector<JointIndex> empty_index_vector; |
490 |
|
|
|
491 |
|
✗ |
humanoid_model.lowerPositionLimit.head<3>().fill(-1.); |
492 |
|
✗ |
humanoid_model.upperPositionLimit.head<3>().fill(1.); |
493 |
|
|
|
494 |
|
✗ |
humanoid_model.referenceConfigurations.insert( |
495 |
|
✗ |
std::pair<std::string, Eigen::VectorXd>("neutral", neutral(humanoid_model))); |
496 |
|
✗ |
Eigen::VectorXd reference_config_humanoid = randomConfiguration(humanoid_model); |
497 |
|
|
Model humanoid_copy_model = |
498 |
|
✗ |
buildReducedModel(humanoid_model, empty_index_vector, reference_config_humanoid); |
499 |
|
|
|
500 |
|
✗ |
BOOST_CHECK(humanoid_copy_model.names == humanoid_model.names); |
501 |
|
✗ |
BOOST_CHECK(humanoid_copy_model.joints == humanoid_model.joints); |
502 |
|
✗ |
BOOST_CHECK(humanoid_copy_model == humanoid_model); |
503 |
|
✗ |
BOOST_CHECK( |
504 |
|
|
humanoid_copy_model.referenceConfigurations["neutral"].isApprox(neutral(humanoid_copy_model))); |
505 |
|
|
|
506 |
|
✗ |
const std::vector<JointIndex> empty_joints_to_lock; |
507 |
|
|
|
508 |
|
|
const Model reduced_humanoid_model = |
509 |
|
✗ |
buildReducedModel(humanoid_model, empty_joints_to_lock, reference_config_humanoid); |
510 |
|
✗ |
BOOST_CHECK(reduced_humanoid_model.njoints == humanoid_model.njoints); |
511 |
|
✗ |
BOOST_CHECK(reduced_humanoid_model.frames == humanoid_model.frames); |
512 |
|
✗ |
BOOST_CHECK(reduced_humanoid_model.jointPlacements == humanoid_model.jointPlacements); |
513 |
|
✗ |
BOOST_CHECK(reduced_humanoid_model.joints == humanoid_model.joints); |
514 |
|
|
|
515 |
|
✗ |
for (JointIndex joint_id = 1; joint_id < (JointIndex)reduced_humanoid_model.njoints; ++joint_id) |
516 |
|
|
{ |
517 |
|
✗ |
BOOST_CHECK( |
518 |
|
|
reduced_humanoid_model.inertias[joint_id].isApprox(humanoid_model.inertias[joint_id])); |
519 |
|
|
} |
520 |
|
|
} |
521 |
|
|
|
522 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_buildReducedModel) |
523 |
|
|
{ |
524 |
|
✗ |
Model humanoid_model; |
525 |
|
✗ |
buildModels::humanoid(humanoid_model); |
526 |
|
|
|
527 |
|
✗ |
static const std::vector<JointIndex> empty_index_vector; |
528 |
|
|
|
529 |
|
✗ |
humanoid_model.lowerPositionLimit.head<3>().fill(-1.); |
530 |
|
✗ |
humanoid_model.upperPositionLimit.head<3>().fill(1.); |
531 |
|
|
|
532 |
|
✗ |
humanoid_model.referenceConfigurations.insert( |
533 |
|
✗ |
std::pair<std::string, Eigen::VectorXd>("neutral", neutral(humanoid_model))); |
534 |
|
✗ |
Eigen::VectorXd reference_config_humanoid = randomConfiguration(humanoid_model); |
535 |
|
|
Model humanoid_copy_model = |
536 |
|
✗ |
buildReducedModel(humanoid_model, empty_index_vector, reference_config_humanoid); |
537 |
|
|
|
538 |
|
✗ |
BOOST_CHECK(humanoid_copy_model.names == humanoid_model.names); |
539 |
|
✗ |
BOOST_CHECK(humanoid_copy_model.joints == humanoid_model.joints); |
540 |
|
✗ |
BOOST_CHECK(humanoid_copy_model == humanoid_model); |
541 |
|
✗ |
BOOST_CHECK( |
542 |
|
|
humanoid_copy_model.referenceConfigurations["neutral"].isApprox(neutral(humanoid_copy_model))); |
543 |
|
|
|
544 |
|
✗ |
std::vector<JointIndex> joints_to_lock; |
545 |
|
✗ |
const std::string joint1_to_lock = "rarm_shoulder2_joint"; |
546 |
|
✗ |
joints_to_lock.push_back(humanoid_model.getJointId(joint1_to_lock)); |
547 |
|
✗ |
const std::string joint2_to_lock = "larm_shoulder2_joint"; |
548 |
|
✗ |
joints_to_lock.push_back(humanoid_model.getJointId(joint2_to_lock)); |
549 |
|
|
|
550 |
|
|
Model reduced_humanoid_model = |
551 |
|
✗ |
buildReducedModel(humanoid_model, joints_to_lock, reference_config_humanoid); |
552 |
|
✗ |
BOOST_CHECK( |
553 |
|
|
reduced_humanoid_model.njoints == humanoid_model.njoints - (int)joints_to_lock.size()); |
554 |
|
|
|
555 |
|
✗ |
BOOST_CHECK(reduced_humanoid_model != humanoid_model); |
556 |
|
|
|
557 |
|
✗ |
Model reduced_humanoid_model_other_signature; |
558 |
|
✗ |
buildReducedModel( |
559 |
|
|
humanoid_model, joints_to_lock, reference_config_humanoid, |
560 |
|
|
reduced_humanoid_model_other_signature); |
561 |
|
✗ |
BOOST_CHECK(reduced_humanoid_model == reduced_humanoid_model_other_signature); |
562 |
|
|
|
563 |
|
|
// Check that forward kinematics give same results |
564 |
|
✗ |
Data data(humanoid_model), reduced_data(reduced_humanoid_model); |
565 |
|
✗ |
Eigen::VectorXd q = reference_config_humanoid; |
566 |
|
✗ |
Eigen::VectorXd reduced_q(reduced_humanoid_model.nq); |
567 |
|
|
|
568 |
|
✗ |
for (JointIndex joint_id = 1; joint_id < (JointIndex)reduced_humanoid_model.njoints; ++joint_id) |
569 |
|
|
{ |
570 |
|
|
const JointIndex reference_joint_id = |
571 |
|
✗ |
humanoid_model.getJointId(reduced_humanoid_model.names[joint_id]); |
572 |
|
|
|
573 |
|
✗ |
reduced_humanoid_model.joints[joint_id].jointConfigSelector(reduced_q) = |
574 |
|
✗ |
humanoid_model.joints[reference_joint_id].jointConfigSelector(q); |
575 |
|
|
} |
576 |
|
|
|
577 |
|
✗ |
BOOST_CHECK(reduced_humanoid_model.referenceConfigurations["neutral"].isApprox( |
578 |
|
|
neutral(reduced_humanoid_model))); |
579 |
|
|
|
580 |
|
✗ |
framesForwardKinematics(humanoid_model, data, q); |
581 |
|
✗ |
framesForwardKinematics(reduced_humanoid_model, reduced_data, reduced_q); |
582 |
|
|
|
583 |
|
✗ |
for (size_t frame_id = 0; frame_id < reduced_humanoid_model.frames.size(); ++frame_id) |
584 |
|
|
{ |
585 |
|
✗ |
const Frame & reduced_frame = reduced_humanoid_model.frames[frame_id]; |
586 |
|
✗ |
switch (reduced_frame.type) |
587 |
|
|
{ |
588 |
|
✗ |
case JOINT: |
589 |
|
|
case FIXED_JOINT: { |
590 |
|
|
// May not be present in the original model |
591 |
|
✗ |
if (humanoid_model.existJointName(reduced_frame.name)) |
592 |
|
|
{ |
593 |
|
✗ |
const JointIndex joint_id = humanoid_model.getJointId(reduced_frame.name); |
594 |
|
✗ |
BOOST_CHECK(data.oMi[joint_id].isApprox(reduced_data.oMf[frame_id])); |
595 |
|
|
} |
596 |
|
✗ |
else if (humanoid_model.existFrame(reduced_frame.name)) |
597 |
|
|
{ |
598 |
|
✗ |
const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name); |
599 |
|
✗ |
BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id])); |
600 |
|
|
} |
601 |
|
|
else |
602 |
|
|
{ |
603 |
|
✗ |
BOOST_CHECK_MESSAGE( |
604 |
|
|
false, "The frame " << reduced_frame.name << " is not presend in the humanoid_model"); |
605 |
|
|
} |
606 |
|
✗ |
break; |
607 |
|
|
} |
608 |
|
✗ |
default: { |
609 |
|
✗ |
BOOST_CHECK(humanoid_model.existFrame(reduced_frame.name)); |
610 |
|
✗ |
const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name); |
611 |
|
✗ |
BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id])); |
612 |
|
✗ |
break; |
613 |
|
|
} |
614 |
|
|
} |
615 |
|
|
} |
616 |
|
|
} |
617 |
|
|
|
618 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_aligned_vector_of_model) |
619 |
|
|
{ |
620 |
|
|
typedef PINOCCHIO_ALIGNED_STD_VECTOR(Model) VectorOfModels; |
621 |
|
|
|
622 |
|
✗ |
VectorOfModels models; |
623 |
|
✗ |
for (size_t k = 0; k < 100; ++k) |
624 |
|
|
{ |
625 |
|
✗ |
models.push_back(Model()); |
626 |
|
✗ |
buildModels::humanoidRandom(models[k]); |
627 |
|
|
} |
628 |
|
|
} |
629 |
|
|
|
630 |
|
|
#ifdef PINOCCHIO_WITH_HPP_FCL |
631 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_buildReducedModel_with_geom) |
632 |
|
|
{ |
633 |
|
✗ |
Model humanoid_model; |
634 |
|
✗ |
buildModels::humanoid(humanoid_model); |
635 |
|
|
|
636 |
|
✗ |
humanoid_model.lowerPositionLimit.head<3>().fill(-1.); |
637 |
|
✗ |
humanoid_model.upperPositionLimit.head<3>().fill(1.); |
638 |
|
✗ |
const Eigen::VectorXd reference_config_humanoid = randomConfiguration(humanoid_model); |
639 |
|
|
|
640 |
|
✗ |
GeometryModel humanoid_geometry; |
641 |
|
✗ |
buildModels::humanoidGeometries(humanoid_model, humanoid_geometry); |
642 |
|
|
|
643 |
|
✗ |
static const std::vector<JointIndex> empty_index_vector; |
644 |
|
|
|
645 |
|
✗ |
Model humanoid_copy_model; |
646 |
|
✗ |
GeometryModel humanoid_copy_geometry; |
647 |
|
✗ |
buildReducedModel( |
648 |
|
|
humanoid_model, humanoid_geometry, empty_index_vector, reference_config_humanoid, |
649 |
|
|
humanoid_copy_model, humanoid_copy_geometry); |
650 |
|
|
|
651 |
|
✗ |
BOOST_CHECK(humanoid_copy_model == humanoid_model); |
652 |
|
✗ |
BOOST_CHECK(humanoid_copy_geometry == humanoid_geometry); |
653 |
|
|
|
654 |
|
✗ |
std::vector<JointIndex> joints_to_lock; |
655 |
|
✗ |
const std::string joint1_to_lock = "rarm_shoulder2_joint"; |
656 |
|
✗ |
joints_to_lock.push_back(humanoid_model.getJointId(joint1_to_lock)); |
657 |
|
✗ |
const std::string joint2_to_lock = "larm_shoulder2_joint"; |
658 |
|
✗ |
joints_to_lock.push_back(humanoid_model.getJointId(joint2_to_lock)); |
659 |
|
|
|
660 |
|
✗ |
Model reduced_humanoid_model; |
661 |
|
✗ |
GeometryModel reduced_humanoid_geometry; |
662 |
|
✗ |
buildReducedModel( |
663 |
|
|
humanoid_model, humanoid_geometry, joints_to_lock, reference_config_humanoid, |
664 |
|
|
reduced_humanoid_model, reduced_humanoid_geometry); |
665 |
|
|
|
666 |
|
✗ |
BOOST_CHECK(reduced_humanoid_geometry.ngeoms == humanoid_geometry.ngeoms); |
667 |
|
✗ |
BOOST_CHECK(reduced_humanoid_geometry.collisionPairs == humanoid_geometry.collisionPairs); |
668 |
|
✗ |
BOOST_CHECK( |
669 |
|
|
reduced_humanoid_geometry.geometryObjects.size() == humanoid_geometry.geometryObjects.size()); |
670 |
|
|
|
671 |
|
✗ |
for (Index i = 0; i < humanoid_geometry.geometryObjects.size(); ++i) |
672 |
|
|
{ |
673 |
|
✗ |
const GeometryObject & go1 = humanoid_geometry.geometryObjects[i]; |
674 |
|
✗ |
const GeometryObject & go2 = reduced_humanoid_geometry.geometryObjects[i]; |
675 |
|
✗ |
BOOST_CHECK_EQUAL(go1.name, go2.name); |
676 |
|
✗ |
BOOST_CHECK_EQUAL(go1.geometry, go2.geometry); |
677 |
|
✗ |
BOOST_CHECK_EQUAL(go1.meshPath, go2.meshPath); |
678 |
|
✗ |
BOOST_CHECK_EQUAL(go1.meshScale, go2.meshScale); |
679 |
|
✗ |
BOOST_CHECK_EQUAL(go1.overrideMaterial, go2.overrideMaterial); |
680 |
|
✗ |
BOOST_CHECK_EQUAL(go1.meshColor, go2.meshColor); |
681 |
|
✗ |
BOOST_CHECK_EQUAL(go1.meshTexturePath, go2.meshTexturePath); |
682 |
|
✗ |
BOOST_CHECK_EQUAL(go1.parentFrame, go2.parentFrame); |
683 |
|
✗ |
BOOST_CHECK_EQUAL( |
684 |
|
|
humanoid_model.frames[go1.parentFrame].name, |
685 |
|
|
reduced_humanoid_model.frames[go2.parentFrame].name); |
686 |
|
|
} |
687 |
|
|
|
688 |
|
✗ |
Data data(humanoid_model), reduced_data(reduced_humanoid_model); |
689 |
|
✗ |
const Eigen::VectorXd q = reference_config_humanoid; |
690 |
|
✗ |
Eigen::VectorXd reduced_q(reduced_humanoid_model.nq); |
691 |
|
|
|
692 |
|
✗ |
for (JointIndex joint_id = 1; joint_id < (JointIndex)reduced_humanoid_model.njoints; ++joint_id) |
693 |
|
|
{ |
694 |
|
|
const JointIndex reference_joint_id = |
695 |
|
✗ |
humanoid_model.getJointId(reduced_humanoid_model.names[joint_id]); |
696 |
|
|
|
697 |
|
✗ |
reduced_humanoid_model.joints[joint_id].jointConfigSelector(reduced_q) = |
698 |
|
✗ |
humanoid_model.joints[reference_joint_id].jointConfigSelector(q); |
699 |
|
|
} |
700 |
|
|
|
701 |
|
✗ |
framesForwardKinematics(humanoid_model, data, q); |
702 |
|
✗ |
framesForwardKinematics(reduced_humanoid_model, reduced_data, reduced_q); |
703 |
|
|
|
704 |
|
✗ |
for (size_t frame_id = 0; frame_id < reduced_humanoid_model.frames.size(); ++frame_id) |
705 |
|
|
{ |
706 |
|
✗ |
const Frame & reduced_frame = reduced_humanoid_model.frames[frame_id]; |
707 |
|
✗ |
switch (reduced_frame.type) |
708 |
|
|
{ |
709 |
|
✗ |
case JOINT: |
710 |
|
|
case FIXED_JOINT: { |
711 |
|
|
// May not be present in the original model |
712 |
|
✗ |
if (humanoid_model.existJointName(reduced_frame.name)) |
713 |
|
|
{ |
714 |
|
✗ |
const JointIndex joint_id = humanoid_model.getJointId(reduced_frame.name); |
715 |
|
✗ |
BOOST_CHECK(data.oMi[joint_id].isApprox(reduced_data.oMf[frame_id])); |
716 |
|
|
} |
717 |
|
✗ |
else if (humanoid_model.existFrame(reduced_frame.name)) |
718 |
|
|
{ |
719 |
|
✗ |
const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name); |
720 |
|
✗ |
BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id])); |
721 |
|
|
} |
722 |
|
|
else |
723 |
|
|
{ |
724 |
|
✗ |
BOOST_CHECK_MESSAGE( |
725 |
|
|
false, "The frame " << reduced_frame.name << " is not presend in the humanoid_model"); |
726 |
|
|
} |
727 |
|
✗ |
break; |
728 |
|
|
} |
729 |
|
✗ |
default: { |
730 |
|
✗ |
BOOST_CHECK(humanoid_model.existFrame(reduced_frame.name)); |
731 |
|
✗ |
const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name); |
732 |
|
✗ |
BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id])); |
733 |
|
✗ |
break; |
734 |
|
|
} |
735 |
|
|
} |
736 |
|
|
} |
737 |
|
|
|
738 |
|
|
// Test GeometryObject placements |
739 |
|
✗ |
GeometryData geom_data(humanoid_geometry), reduded_geom_data(reduced_humanoid_geometry); |
740 |
|
✗ |
updateGeometryPlacements(humanoid_model, data, humanoid_geometry, geom_data); |
741 |
|
✗ |
updateGeometryPlacements( |
742 |
|
|
reduced_humanoid_model, reduced_data, reduced_humanoid_geometry, reduded_geom_data); |
743 |
|
|
|
744 |
|
✗ |
BOOST_CHECK(geom_data.oMg.size() == reduded_geom_data.oMg.size()); |
745 |
|
✗ |
for (FrameIndex i = 0; i < geom_data.oMg.size(); ++i) |
746 |
|
|
{ |
747 |
|
✗ |
BOOST_CHECK(geom_data.oMg[i].isApprox(reduded_geom_data.oMg[i])); |
748 |
|
|
} |
749 |
|
|
|
750 |
|
|
// Test other signature |
751 |
|
✗ |
std::vector<GeometryModel> full_geometry_models; |
752 |
|
✗ |
full_geometry_models.push_back(humanoid_geometry); |
753 |
|
✗ |
full_geometry_models.push_back(humanoid_geometry); |
754 |
|
✗ |
full_geometry_models.push_back(humanoid_geometry); |
755 |
|
|
|
756 |
|
✗ |
std::vector<GeometryModel> reduced_geometry_models; |
757 |
|
|
|
758 |
|
✗ |
Model reduced_humanoid_model_other_sig; |
759 |
|
✗ |
buildReducedModel( |
760 |
|
|
humanoid_model, full_geometry_models, joints_to_lock, reference_config_humanoid, |
761 |
|
|
reduced_humanoid_model_other_sig, reduced_geometry_models); |
762 |
|
|
|
763 |
|
✗ |
BOOST_CHECK(reduced_geometry_models[0] == reduced_humanoid_geometry); |
764 |
|
✗ |
BOOST_CHECK(reduced_geometry_models[1] == reduced_humanoid_geometry); |
765 |
|
✗ |
BOOST_CHECK(reduced_geometry_models[2] == reduced_humanoid_geometry); |
766 |
|
|
} |
767 |
|
|
#endif // PINOCCHIO_WITH_HPP_FCL |
768 |
|
|
|
769 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_findCommonAncestor) |
770 |
|
|
{ |
771 |
|
✗ |
Model model; |
772 |
|
✗ |
buildModels::humanoid(model); |
773 |
|
|
|
774 |
|
|
{ |
775 |
|
|
size_t id_ancestor1, id_ancestor2; |
776 |
|
✗ |
JointIndex ancestor = findCommonAncestor(model, 0, 0, id_ancestor1, id_ancestor2); |
777 |
|
✗ |
BOOST_CHECK(ancestor == 0); |
778 |
|
✗ |
BOOST_CHECK(id_ancestor1 == 0); |
779 |
|
✗ |
BOOST_CHECK(id_ancestor2 == 0); |
780 |
|
|
} |
781 |
|
|
|
782 |
|
|
{ |
783 |
|
|
size_t id_ancestor1, id_ancestor2; |
784 |
|
|
JointIndex ancestor = |
785 |
|
✗ |
findCommonAncestor(model, 0, (JointIndex)(model.njoints - 1), id_ancestor1, id_ancestor2); |
786 |
|
✗ |
BOOST_CHECK(ancestor == 0); |
787 |
|
✗ |
BOOST_CHECK(id_ancestor1 == 0); |
788 |
|
✗ |
BOOST_CHECK(id_ancestor2 == 0); |
789 |
|
|
} |
790 |
|
|
|
791 |
|
|
{ |
792 |
|
|
size_t id_ancestor1, id_ancestor2; |
793 |
|
|
JointIndex ancestor = |
794 |
|
✗ |
findCommonAncestor(model, (JointIndex)(model.njoints - 1), 0, id_ancestor1, id_ancestor2); |
795 |
|
✗ |
BOOST_CHECK(ancestor == 0); |
796 |
|
✗ |
BOOST_CHECK(id_ancestor1 == 0); |
797 |
|
✗ |
BOOST_CHECK(id_ancestor2 == 0); |
798 |
|
|
} |
799 |
|
|
|
800 |
|
|
{ |
801 |
|
|
size_t id_ancestor1, id_ancestor2; |
802 |
|
|
JointIndex ancestor = |
803 |
|
✗ |
findCommonAncestor(model, (JointIndex)(model.njoints - 1), 1, id_ancestor1, id_ancestor2); |
804 |
|
✗ |
BOOST_CHECK(ancestor == 1); |
805 |
|
✗ |
BOOST_CHECK(model.supports[(JointIndex)(model.njoints - 1)][id_ancestor1] == ancestor); |
806 |
|
✗ |
BOOST_CHECK(model.supports[1][id_ancestor2] == ancestor); |
807 |
|
|
} |
808 |
|
|
} |
809 |
|
|
|
810 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_has_configuration_limit) |
811 |
|
|
{ |
812 |
|
|
using namespace Eigen; |
813 |
|
|
|
814 |
|
|
// Test joint specific function hasConfigurationLimit |
815 |
|
✗ |
JointModelFreeFlyer test_joint_ff = JointModelFreeFlyer(); |
816 |
|
✗ |
std::vector<bool> cf_limits_ff = test_joint_ff.hasConfigurationLimit(); |
817 |
|
✗ |
std::vector<bool> cf_limits_tangent_ff = test_joint_ff.hasConfigurationLimitInTangent(); |
818 |
|
✗ |
std::vector<bool> expected_cf_limits_ff({true, true, true, false, false, false, false}); |
819 |
|
✗ |
std::vector<bool> expected_cf_limits_tangent_ff({true, true, true, false, false, false}); |
820 |
|
✗ |
BOOST_CHECK(cf_limits_ff == expected_cf_limits_ff); |
821 |
|
✗ |
BOOST_CHECK(cf_limits_tangent_ff == expected_cf_limits_tangent_ff); |
822 |
|
|
|
823 |
|
✗ |
JointModelPlanar test_joint_planar = JointModelPlanar(); |
824 |
|
✗ |
std::vector<bool> cf_limits_planar = test_joint_planar.hasConfigurationLimit(); |
825 |
|
✗ |
std::vector<bool> cf_limits_tangent_planar = test_joint_planar.hasConfigurationLimitInTangent(); |
826 |
|
✗ |
std::vector<bool> expected_cf_limits_planar({true, true, false, false}); |
827 |
|
✗ |
std::vector<bool> expected_cf_limits_tangent_planar({true, true, false}); |
828 |
|
✗ |
BOOST_CHECK(cf_limits_planar == expected_cf_limits_planar); |
829 |
|
✗ |
BOOST_CHECK(cf_limits_tangent_planar == expected_cf_limits_tangent_planar); |
830 |
|
|
|
831 |
|
✗ |
JointModelPX test_joint_p = JointModelPX(); |
832 |
|
✗ |
std::vector<bool> cf_limits_prismatic = test_joint_p.hasConfigurationLimit(); |
833 |
|
✗ |
std::vector<bool> cf_limits_tangent_prismatic = test_joint_p.hasConfigurationLimitInTangent(); |
834 |
|
✗ |
std::vector<bool> expected_cf_limits_prismatic({true}); |
835 |
|
✗ |
std::vector<bool> expected_cf_limits_tangent_prismatic({true}); |
836 |
|
✗ |
BOOST_CHECK(cf_limits_prismatic == expected_cf_limits_prismatic); |
837 |
|
✗ |
BOOST_CHECK(cf_limits_tangent_prismatic == expected_cf_limits_tangent_prismatic); |
838 |
|
|
|
839 |
|
|
// Test model.hasConfigurationLimit() function |
840 |
|
✗ |
Model model; |
841 |
|
✗ |
JointIndex jointId = 0; |
842 |
|
|
|
843 |
|
✗ |
jointId = model.addJoint(jointId, JointModelFreeFlyer(), SE3::Identity(), "Joint0"); |
844 |
|
✗ |
jointId = model.addJoint(jointId, JointModelRZ(), SE3::Identity(), "Joint1"); |
845 |
|
✗ |
jointId = model.addJoint( |
846 |
|
✗ |
jointId, JointModelRUBZ(), SE3(Matrix3d::Identity(), Vector3d(1.0, 0.0, 0.0)), "Joint2"); |
847 |
|
|
|
848 |
|
|
std::vector<bool> expected_cf_limits_model( |
849 |
|
|
{true, true, true, // translation of FF |
850 |
|
|
false, false, false, false, // rotation of FF |
851 |
|
|
true, // roational joint |
852 |
|
✗ |
false, false}); // unbounded rotational joint |
853 |
|
✗ |
std::vector<bool> model_cf_limits = model.hasConfigurationLimit(); |
854 |
|
✗ |
BOOST_CHECK((model_cf_limits == expected_cf_limits_model)); |
855 |
|
|
|
856 |
|
|
// Test model.hasConfigurationLimitInTangent() function |
857 |
|
|
std::vector<bool> expected_cf_limits_tangent_model( |
858 |
|
|
{true, true, true, // translation of FF |
859 |
|
|
false, false, false, // rotation of FF |
860 |
|
|
true, // roational joint |
861 |
|
✗ |
false}); // unbounded rotational joint |
862 |
|
✗ |
std::vector<bool> model_cf_limits_tangent = model.hasConfigurationLimitInTangent(); |
863 |
|
✗ |
BOOST_CHECK((model_cf_limits_tangent == expected_cf_limits_tangent_model)); |
864 |
|
|
} |
865 |
|
|
|
866 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
867 |
|
|
|