| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| 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 |
|||
15 |
#include "pinocchio/parsers/sample-models.hpp" |
||
16 |
|||
17 |
#include <boost/test/unit_test.hpp> |
||
18 |
#include <boost/utility/binary.hpp> |
||
19 |
|||
20 |
using namespace pinocchio; |
||
21 |
|||
22 |
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE ) |
||
23 |
|||
24 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_model_subtree) |
25 |
{ |
||
26 |
✓✗ | 4 |
Model model; |
27 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
28 |
|||
29 |
✓✗✓✗ |
2 |
Model::JointIndex idx_larm1 = model.getJointId("larm1_joint"); |
30 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(idx_larm1<(Model::JointIndex)model.njoints); |
31 |
✓✗ | 4 |
Model::IndexVector subtree = model.subtrees[idx_larm1]; |
32 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(subtree.size()==6); |
33 |
|||
34 |
✓✓ | 12 |
for(size_t i=1; i<subtree.size();++i) |
35 |
{ |
||
36 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
10 |
BOOST_CHECK(model.parents[subtree[i]]==subtree[i-1]); |
37 |
} |
||
38 |
|||
39 |
// Check that i starts subtree[i] |
||
40 |
✓✓ | 56 |
for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) |
41 |
{ |
||
42 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
54 |
BOOST_CHECK(model.subtrees[joint_id][0] == joint_id); |
43 |
} |
||
44 |
|||
45 |
// Check that subtree[0] contains all joint ids |
||
46 |
✓✓ | 56 |
for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) |
47 |
{ |
||
48 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
54 |
BOOST_CHECK(model.subtrees[0][joint_id-1] == joint_id); |
49 |
} |
||
50 |
2 |
} |
|
51 |
|||
52 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_model_get_frame_id) |
53 |
{ |
||
54 |
✓✗ | 4 |
Model model; |
55 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
56 |
|||
57 |
✓✓ | 112 |
for(FrameIndex i=0; i<static_cast<FrameIndex>(model.nframes); i++) |
58 |
{ |
||
59 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
110 |
BOOST_CHECK_EQUAL(i, model.getFrameId(model.frames[i].name)); |
60 |
} |
||
61 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(model.nframes, model.getFrameId("NOT_A_FRAME")); |
62 |
2 |
} |
|
63 |
|||
64 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_model_support) |
65 |
{ |
||
66 |
✓✗ | 4 |
Model model; |
67 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
68 |
✓✗ | 4 |
const Model::IndexVector support0_ref(1,0); |
69 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(model.supports[0] == support0_ref); |
70 |
|||
71 |
// Check that i ends supports[i] |
||
72 |
✓✓ | 56 |
for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) |
73 |
{ |
||
74 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
54 |
BOOST_CHECK(model.supports[joint_id].back() == joint_id); |
75 |
54 |
Model::IndexVector & support = model.supports[joint_id]; |
|
76 |
|||
77 |
54 |
size_t current_id = support.size()-2; |
|
78 |
54 |
for(JointIndex parent_id = model.parents[joint_id]; |
|
79 |
✓✓ | 276 |
parent_id > 0; |
80 |
222 |
parent_id = model.parents[parent_id], |
|
81 |
current_id--) |
||
82 |
{ |
||
83 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
222 |
BOOST_CHECK(parent_id == support[current_id]); |
84 |
} |
||
85 |
} |
||
86 |
2 |
} |
|
87 |
|||
88 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_model_subspace_dimensions) |
89 |
{ |
||
90 |
✓✗ | 4 |
Model model; |
91 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
92 |
|||
93 |
// Check that i ends supports[i] |
||
94 |
✓✓ | 56 |
for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) |
95 |
{ |
||
96 |
54 |
const Model::JointModel & jmodel = model.joints[joint_id]; |
|
97 |
|||
98 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(model.nqs[joint_id] == jmodel.nq()); |
99 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(model.idx_qs[joint_id] == jmodel.idx_q()); |
100 |
|||
101 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(model.nvs[joint_id] == jmodel.nv()); |
102 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(model.idx_vs[joint_id] == jmodel.idx_v()); |
103 |
} |
||
104 |
2 |
} |
|
105 |
|||
106 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(comparison) |
107 |
{ |
||
108 |
✓✗ | 4 |
Model model; |
109 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
110 |
|||
111 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(model == model); |
112 |
2 |
} |
|
113 |
|||
114 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(cast) |
115 |
{ |
||
116 |
✓✗ | 4 |
Model model; |
117 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
118 |
|||
119 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(model.cast<double>() == model.cast<double>()); |
120 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(model.cast<double>().cast<long double>() == model.cast<long double>()); |
121 |
2 |
} |
|
122 |
|||
123 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_std_vector_of_Model) |
124 |
{ |
||
125 |
✓✗ | 4 |
Model model; |
126 |
✓✗ | 2 |
buildModels::humanoid(model); |
127 |
|||
128 |
4 |
PINOCCHIO_ALIGNED_STD_VECTOR(Model) models; |
|
129 |
✓✓ | 42 |
for(size_t k = 0; k < 20; ++k) |
130 |
{ |
||
131 |
✓✗✓✗ |
40 |
models.push_back(Model()); |
132 |
✓✗ | 40 |
buildModels::humanoid(models.back()); |
133 |
|||
134 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
40 |
BOOST_CHECK(model == models.back()); |
135 |
} |
||
136 |
2 |
} |
|
137 |
|||
138 |
#ifdef PINOCCHIO_WITH_HPP_FCL |
||
139 |
struct AddPrefix { |
||
140 |
std::string p; |
||
141 |
35 |
std::string operator() (const std::string& n) { return p + n; } |
|
142 |
✓✗ | 74 |
Frame operator() (const Frame& _f) { Frame f (_f); f.name = p + f.name; return f; } |
143 |
✓✗ | 2 |
AddPrefix (const char* _p) : p(_p) {} |
144 |
}; |
||
145 |
|||
146 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(append) |
147 |
{ |
||
148 |
✓✗✓✗ |
4 |
Model manipulator, humanoid; |
149 |
✓✗✓✗ |
4 |
GeometryModel geomManipulator, geomHumanoid; |
150 |
|||
151 |
✓✗ | 2 |
buildModels::manipulator(manipulator); |
152 |
✓✗ | 2 |
buildModels::manipulatorGeometries(manipulator, geomManipulator); |
153 |
✓✗ | 2 |
geomManipulator.addAllCollisionPairs(); |
154 |
// Add prefix to joint and frame names |
||
155 |
✓✗ | 4 |
AddPrefix addManipulatorPrefix ("manipulator/"); |
156 |
2 |
std::transform (++manipulator.names.begin(), manipulator.names.end(), |
|
157 |
✓✗✓✗ |
4 |
++manipulator.names.begin(), addManipulatorPrefix); |
158 |
2 |
std::transform (++manipulator.frames.begin(), manipulator.frames.end(), |
|
159 |
✓✗✓✗ |
4 |
++manipulator.frames.begin(), addManipulatorPrefix); |
160 |
|||
161 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
BOOST_TEST_MESSAGE(manipulator); |
162 |
|||
163 |
✓✗ | 2 |
buildModels::humanoid(humanoid); |
164 |
✓✗ | 2 |
buildModels::humanoidGeometries(humanoid, geomHumanoid); |
165 |
✓✗ | 2 |
geomHumanoid.addAllCollisionPairs(); |
166 |
// Add prefix to joint and frame names |
||
167 |
✓✗ | 4 |
AddPrefix addHumanoidPrefix ("humanoid/"); |
168 |
2 |
std::transform (++humanoid.names.begin(), humanoid.names.end(), |
|
169 |
✓✗✓✗ |
4 |
++humanoid.names.begin(), addHumanoidPrefix); |
170 |
2 |
std::transform (++humanoid.frames.begin(), humanoid.frames.end(), |
|
171 |
✓✗✓✗ |
4 |
++humanoid.frames.begin(), addHumanoidPrefix); |
172 |
|||
173 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
BOOST_TEST_MESSAGE(humanoid); |
174 |
|||
175 |
//TODO fix inertia of the base |
||
176 |
✓✗ | 2 |
manipulator.inertias[0].setRandom(); |
177 |
✓✗ | 2 |
SE3 aMb = SE3::Random(); |
178 |
|||
179 |
// First append a model to the universe frame. |
||
180 |
✓✗ | 4 |
Model model1; |
181 |
✓✗ | 4 |
GeometryModel geomModel1; |
182 |
2 |
FrameIndex fid = 0; |
|
183 |
✓✗ | 2 |
appendModel (humanoid, manipulator, geomHumanoid, geomManipulator, fid, |
184 |
✓✗ | 2 |
SE3::Identity(), model1, geomModel1); |
185 |
|||
186 |
{ |
||
187 |
✓✗✓✗ |
4 |
Model model2 = appendModel(humanoid, manipulator, fid, SE3::Identity()); |
188 |
✓✗ | 4 |
Model model3; |
189 |
✓✗✓✗ |
2 |
appendModel(humanoid, manipulator, fid, SE3::Identity(), model3); |
190 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(model1 == model2); |
191 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(model1 == model3); |
192 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(model2 == model3); |
193 |
} |
||
194 |
|||
195 |
✓✗ | 4 |
Data data1 (model1); |
196 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(model1.check(data1)); |
197 |
|||
198 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
BOOST_TEST_MESSAGE(model1); |
199 |
|||
200 |
// Second, append a model to a moving frame. |
||
201 |
✓✗ | 4 |
Model model; |
202 |
|||
203 |
✓✗ | 4 |
GeometryModel geomModel; |
204 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
fid = humanoid.addFrame (Frame ("humanoid/add_manipulator", |
205 |
humanoid.getJointId("humanoid/chest2_joint"), |
||
206 |
humanoid.getFrameId("humanoid/chest2_joint"), aMb, |
||
207 |
OP_FRAME)); |
||
208 |
|||
209 |
// Append manipulator to chest2_joint of humanoid |
||
210 |
✓✗ | 2 |
appendModel (humanoid, manipulator, geomHumanoid, geomManipulator, fid, |
211 |
✓✗ | 2 |
SE3::Identity(), model, geomModel); |
212 |
|||
213 |
{ |
||
214 |
✓✗✓✗ |
4 |
Model model2 = appendModel(humanoid, manipulator, fid, SE3::Identity()); |
215 |
✓✗ | 4 |
Model model3; |
216 |
✓✗✓✗ |
2 |
appendModel(humanoid, manipulator, fid, SE3::Identity(), model3); |
217 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(model == model2); |
218 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(model == model3); |
219 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(model2 == model3); |
220 |
} |
||
221 |
|||
222 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
BOOST_TEST_MESSAGE(model); |
223 |
|||
224 |
✓✗ | 4 |
Data data (model); |
225 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(model.check(data)); |
226 |
|||
227 |
// Check the model |
||
228 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(model.getJointId("humanoid/chest2_joint"), |
229 |
model.parents[model.getJointId("manipulator/shoulder1_joint")]); |
||
230 |
|||
231 |
// check the joint order and the inertias |
||
232 |
// All the joints of the manipulator should be at the end of the merged model |
||
233 |
2 |
JointIndex hnj = (JointIndex) humanoid.njoints; |
|
234 |
✓✗✓✗ |
2 |
JointIndex chest2 = model.getJointId("humanoid/chest2_joint"); |
235 |
✓✓ | 60 |
for (JointIndex jid = 1; jid < hnj; ++jid) { |
236 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
58 |
BOOST_TEST_MESSAGE("Checking joint " << jid << " " << model.names[jid]); |
237 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
58 |
BOOST_CHECK_EQUAL(model.names[jid], humanoid.names[jid]); |
238 |
✓✓ | 58 |
if (jid != chest2) |
239 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
56 |
BOOST_CHECK_EQUAL(model.inertias[jid], humanoid.inertias[jid]); |
240 |
else |
||
241 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_MESSAGE(model.inertias[jid].isApprox(manipulator.inertias[0].se3Action(aMb) + |
242 |
humanoid.inertias[jid]), |
||
243 |
model.inertias[jid] << " != " << manipulator.inertias[0].se3Action(aMb) + |
||
244 |
humanoid.inertias[jid]); |
||
245 |
|||
246 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
58 |
BOOST_CHECK_EQUAL(model.jointPlacements[jid], humanoid.jointPlacements[jid]); |
247 |
} |
||
248 |
✓✓ | 12 |
for (JointIndex jid = 1; jid < manipulator.joints.size()-1; ++jid) { |
249 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
10 |
BOOST_TEST_MESSAGE("Checking joint " << hnj -1 +jid << " " << model.names[hnj+jid]); |
250 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
10 |
BOOST_CHECK_EQUAL(model.names[hnj-1+jid], manipulator.names[jid]); |
251 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
10 |
BOOST_CHECK_EQUAL(model.inertias[hnj-1+jid], manipulator.inertias[jid]); |
252 |
✓✓ | 10 |
if (jid==1) |
253 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_EQUAL(model.jointPlacements[hnj-1+jid], aMb*manipulator.jointPlacements[jid]); |
254 |
else |
||
255 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
8 |
BOOST_CHECK_EQUAL(model.jointPlacements[hnj-1+jid], manipulator.jointPlacements[jid]); |
256 |
} |
||
257 |
// Check the frames |
||
258 |
✓✓ | 126 |
for (FrameIndex fid = 1; fid < humanoid.frames.size(); ++fid) { |
259 |
124 |
const Frame& frame = humanoid.frames[fid], |
|
260 |
✓✗ | 248 |
parent = humanoid.frames[frame.previousFrame]; |
261 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
124 |
BOOST_CHECK(model.existFrame (frame.name, frame.type)); |
262 |
✓✗ | 124 |
const Frame& nframe = model.frames[model.getFrameId(frame.name, frame.type)], |
263 |
✓✗ | 248 |
nparent = model.frames[nframe.previousFrame]; |
264 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
124 |
BOOST_CHECK_EQUAL(parent.name, nparent.name); |
265 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
124 |
BOOST_CHECK_EQUAL(frame.placement, nframe.placement); |
266 |
} |
||
267 |
✓✓ | 28 |
for (FrameIndex fid = 1; fid < manipulator.frames.size(); ++fid) { |
268 |
26 |
const Frame& frame = manipulator.frames[fid], |
|
269 |
✓✗ | 52 |
parent = manipulator.frames[frame.previousFrame]; |
270 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
26 |
BOOST_CHECK(model.existFrame (frame.name, frame.type)); |
271 |
✓✗ | 26 |
const Frame& nframe = model.frames[model.getFrameId(frame.name, frame.type)], |
272 |
✓✗ | 52 |
nparent = model.frames[nframe.previousFrame]; |
273 |
✓✓ | 26 |
if (frame.previousFrame > 0) { |
274 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
24 |
BOOST_CHECK_EQUAL(parent.name, nparent.name); |
275 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
24 |
BOOST_CHECK_EQUAL(frame.placement, nframe.placement); |
276 |
} |
||
277 |
} |
||
278 |
2 |
} |
|
279 |
#endif |
||
280 |
|||
281 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_buildReducedModel_empty) |
282 |
{ |
||
283 |
✓✗ | 4 |
Model humanoid_model; |
284 |
✓✗ | 2 |
buildModels::humanoid(humanoid_model); |
285 |
|||
286 |
✓✗✓✗ |
2 |
static const std::vector<JointIndex> empty_index_vector; |
287 |
|||
288 |
✓✗✓✗ |
2 |
humanoid_model.lowerPositionLimit.head<3>().fill(-1.); |
289 |
✓✗✓✗ |
2 |
humanoid_model.upperPositionLimit.head<3>().fill( 1.); |
290 |
|||
291 |
✓✗✓✗ ✓✗ |
2 |
humanoid_model.referenceConfigurations.insert(std::pair<std::string,Eigen::VectorXd>("neutral",neutral(humanoid_model))); |
292 |
✓✗ | 4 |
Eigen::VectorXd reference_config_humanoid = randomConfiguration(humanoid_model); |
293 |
✓✗ | 4 |
Model humanoid_copy_model = buildReducedModel(humanoid_model,empty_index_vector,reference_config_humanoid); |
294 |
|||
295 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(humanoid_copy_model.names == humanoid_model.names); |
296 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(humanoid_copy_model.joints == humanoid_model.joints); |
297 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(humanoid_copy_model == humanoid_model); |
298 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(humanoid_copy_model.referenceConfigurations["neutral"].isApprox(neutral(humanoid_copy_model))); |
299 |
|||
300 |
4 |
const std::vector<JointIndex> empty_joints_to_lock; |
|
301 |
|||
302 |
✓✗ | 4 |
const Model reduced_humanoid_model = buildReducedModel(humanoid_model,empty_joints_to_lock,reference_config_humanoid); |
303 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_model.njoints == humanoid_model.njoints); |
304 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_model.frames == humanoid_model.frames); |
305 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_model.jointPlacements == humanoid_model.jointPlacements); |
306 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_model.joints == humanoid_model.joints); |
307 |
|||
308 |
✓✓ | 60 |
for(JointIndex joint_id = 1; joint_id < (JointIndex)reduced_humanoid_model.njoints; ++joint_id) |
309 |
{ |
||
310 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
58 |
BOOST_CHECK(reduced_humanoid_model.inertias[joint_id].isApprox(humanoid_model.inertias[joint_id])); |
311 |
} |
||
312 |
2 |
} |
|
313 |
|||
314 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_buildReducedModel) |
315 |
{ |
||
316 |
✓✗ | 4 |
Model humanoid_model; |
317 |
✓✗ | 2 |
buildModels::humanoid(humanoid_model); |
318 |
|||
319 |
✓✗✓✗ |
2 |
static const std::vector<JointIndex> empty_index_vector; |
320 |
|||
321 |
✓✗✓✗ |
2 |
humanoid_model.lowerPositionLimit.head<3>().fill(-1.); |
322 |
✓✗✓✗ |
2 |
humanoid_model.upperPositionLimit.head<3>().fill( 1.); |
323 |
|||
324 |
✓✗✓✗ ✓✗ |
2 |
humanoid_model.referenceConfigurations.insert(std::pair<std::string,Eigen::VectorXd>("neutral",neutral(humanoid_model))); |
325 |
✓✗ | 4 |
Eigen::VectorXd reference_config_humanoid = randomConfiguration(humanoid_model); |
326 |
✓✗ | 4 |
Model humanoid_copy_model = buildReducedModel(humanoid_model,empty_index_vector,reference_config_humanoid); |
327 |
|||
328 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(humanoid_copy_model.names == humanoid_model.names); |
329 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(humanoid_copy_model.joints == humanoid_model.joints); |
330 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(humanoid_copy_model == humanoid_model); |
331 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(humanoid_copy_model.referenceConfigurations["neutral"].isApprox(neutral(humanoid_copy_model))); |
332 |
|||
333 |
4 |
std::vector<JointIndex> joints_to_lock; |
|
334 |
✓✗ | 4 |
const std::string joint1_to_lock = "rarm_shoulder2_joint"; |
335 |
✓✗✓✗ |
2 |
joints_to_lock.push_back(humanoid_model.getJointId(joint1_to_lock)); |
336 |
✓✗ | 4 |
const std::string joint2_to_lock = "larm_shoulder2_joint"; |
337 |
✓✗✓✗ |
2 |
joints_to_lock.push_back(humanoid_model.getJointId(joint2_to_lock)); |
338 |
|||
339 |
✓✗ | 4 |
Model reduced_humanoid_model = buildReducedModel(humanoid_model,joints_to_lock,reference_config_humanoid); |
340 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_model.njoints == humanoid_model.njoints-(int)joints_to_lock.size()); |
341 |
|||
342 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_model != humanoid_model); |
343 |
|||
344 |
✓✗ | 4 |
Model reduced_humanoid_model_other_signature; |
345 |
✓✗✓✗ |
2 |
buildReducedModel(humanoid_model,joints_to_lock,reference_config_humanoid,reduced_humanoid_model_other_signature); |
346 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_model == reduced_humanoid_model_other_signature); |
347 |
|||
348 |
// Check that forward kinematics give same results |
||
349 |
✓✗✓✗ |
4 |
Data data(humanoid_model), reduced_data(reduced_humanoid_model); |
350 |
✓✗ | 4 |
Eigen::VectorXd q = reference_config_humanoid; |
351 |
✓✗ | 4 |
Eigen::VectorXd reduced_q(reduced_humanoid_model.nq); |
352 |
|||
353 |
✓✓ | 56 |
for(JointIndex joint_id = 1; joint_id < (JointIndex)reduced_humanoid_model.njoints; ++joint_id) |
354 |
{ |
||
355 |
✓✗ | 54 |
const JointIndex reference_joint_id = humanoid_model.getJointId(reduced_humanoid_model.names[joint_id]); |
356 |
|||
357 |
✓✗ | 54 |
reduced_humanoid_model.joints[joint_id].jointConfigSelector(reduced_q) = |
358 |
✓✗✓✗ |
108 |
humanoid_model.joints[reference_joint_id].jointConfigSelector(q); |
359 |
} |
||
360 |
|||
361 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_model.referenceConfigurations["neutral"].isApprox(neutral(reduced_humanoid_model))); |
362 |
|||
363 |
✓✗ | 2 |
framesForwardKinematics(humanoid_model,data,q); |
364 |
✓✗ | 2 |
framesForwardKinematics(reduced_humanoid_model,reduced_data,reduced_q); |
365 |
|||
366 |
✓✓ | 126 |
for(size_t frame_id = 0; frame_id < reduced_humanoid_model.frames.size(); ++frame_id) |
367 |
{ |
||
368 |
124 |
const Frame & reduced_frame = reduced_humanoid_model.frames[frame_id]; |
|
369 |
✓✓ | 124 |
switch(reduced_frame.type) |
370 |
{ |
||
371 |
60 |
case JOINT: |
|
372 |
case FIXED_JOINT: |
||
373 |
{ |
||
374 |
// May not be present in the original model |
||
375 |
✓✗✓✗ |
60 |
if(humanoid_model.existJointName(reduced_frame.name)) |
376 |
{ |
||
377 |
✓✗ | 60 |
const JointIndex joint_id = humanoid_model.getJointId(reduced_frame.name); |
378 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
60 |
BOOST_CHECK(data.oMi[joint_id].isApprox(reduced_data.oMf[frame_id])); |
379 |
} |
||
380 |
else if(humanoid_model.existFrame(reduced_frame.name)) |
||
381 |
{ |
||
382 |
const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name); |
||
383 |
BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id])); |
||
384 |
} |
||
385 |
else |
||
386 |
{ |
||
387 |
BOOST_CHECK_MESSAGE(false, "The frame " << reduced_frame.name << " is not presend in the humanoid_model"); |
||
388 |
} |
||
389 |
60 |
break; |
|
390 |
} |
||
391 |
64 |
default: |
|
392 |
{ |
||
393 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
64 |
BOOST_CHECK(humanoid_model.existFrame(reduced_frame.name)); |
394 |
✓✗ | 64 |
const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name); |
395 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
64 |
BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id])); |
396 |
64 |
break; |
|
397 |
} |
||
398 |
|||
399 |
} |
||
400 |
} |
||
401 |
2 |
} |
|
402 |
|||
403 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_aligned_vector_of_model) |
404 |
{ |
||
405 |
typedef PINOCCHIO_ALIGNED_STD_VECTOR(Model) VectorOfModels; |
||
406 |
|||
407 |
4 |
VectorOfModels models; |
|
408 |
✓✓ | 202 |
for(size_t k = 0; k < 100; ++k) |
409 |
{ |
||
410 |
✓✗✓✗ |
200 |
models.push_back(Model()); |
411 |
✓✗ | 200 |
buildModels::humanoidRandom(models[k]); |
412 |
} |
||
413 |
2 |
} |
|
414 |
|||
415 |
#ifdef PINOCCHIO_WITH_HPP_FCL |
||
416 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_buildReducedModel_with_geom) |
417 |
{ |
||
418 |
✓✗ | 4 |
Model humanoid_model; |
419 |
✓✗ | 2 |
buildModels::humanoid(humanoid_model); |
420 |
|||
421 |
✓✗✓✗ |
2 |
humanoid_model.lowerPositionLimit.head<3>().fill(-1.); |
422 |
✓✗✓✗ |
2 |
humanoid_model.upperPositionLimit.head<3>().fill( 1.); |
423 |
✓✗ | 4 |
const Eigen::VectorXd reference_config_humanoid = randomConfiguration(humanoid_model); |
424 |
|||
425 |
✓✗ | 4 |
GeometryModel humanoid_geometry; |
426 |
✓✗ | 2 |
buildModels::humanoidGeometries(humanoid_model, humanoid_geometry); |
427 |
|||
428 |
✓✗✓✗ |
2 |
static const std::vector<JointIndex> empty_index_vector; |
429 |
|||
430 |
✓✗✓✗ |
4 |
Model humanoid_copy_model; GeometryModel humanoid_copy_geometry; |
431 |
✓✗ | 2 |
buildReducedModel(humanoid_model,humanoid_geometry,empty_index_vector, |
432 |
reference_config_humanoid,humanoid_copy_model,humanoid_copy_geometry); |
||
433 |
|||
434 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(humanoid_copy_model == humanoid_model); |
435 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(humanoid_copy_geometry == humanoid_geometry); |
436 |
|||
437 |
4 |
std::vector<JointIndex> joints_to_lock; |
|
438 |
✓✗ | 4 |
const std::string joint1_to_lock = "rarm_shoulder2_joint"; |
439 |
✓✗✓✗ |
2 |
joints_to_lock.push_back(humanoid_model.getJointId(joint1_to_lock)); |
440 |
✓✗ | 4 |
const std::string joint2_to_lock = "larm_shoulder2_joint"; |
441 |
✓✗✓✗ |
2 |
joints_to_lock.push_back(humanoid_model.getJointId(joint2_to_lock)); |
442 |
|||
443 |
✓✗✓✗ |
4 |
Model reduced_humanoid_model; GeometryModel reduced_humanoid_geometry; |
444 |
✓✗ | 2 |
buildReducedModel(humanoid_model,humanoid_geometry,joints_to_lock, |
445 |
reference_config_humanoid,reduced_humanoid_model,reduced_humanoid_geometry); |
||
446 |
|||
447 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_geometry.ngeoms == humanoid_geometry.ngeoms); |
448 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_geometry.collisionPairs == humanoid_geometry.collisionPairs); |
449 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_geometry.geometryObjects.size() == humanoid_geometry.geometryObjects.size()); |
450 |
|||
451 |
✓✓ | 56 |
for(Index i = 0; i < humanoid_geometry.geometryObjects.size(); ++i) |
452 |
{ |
||
453 |
54 |
const GeometryObject & go1 = humanoid_geometry.geometryObjects[i]; |
|
454 |
54 |
const GeometryObject & go2 = reduced_humanoid_geometry.geometryObjects[i]; |
|
455 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK_EQUAL(go1.name, go2.name); |
456 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK_EQUAL(go1.geometry, go2.geometry); |
457 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK_EQUAL(go1.meshPath, go2.meshPath); |
458 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK_EQUAL(go1.meshScale, go2.meshScale); |
459 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK_EQUAL(go1.overrideMaterial, go2.overrideMaterial); |
460 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK_EQUAL(go1.meshColor, go2.meshColor); |
461 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK_EQUAL(go1.meshTexturePath, go2.meshTexturePath); |
462 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK_EQUAL(go1.parentFrame, go2.parentFrame); |
463 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK_EQUAL(humanoid_model.frames[go1.parentFrame].name, |
464 |
reduced_humanoid_model.frames[go2.parentFrame].name); |
||
465 |
} |
||
466 |
|||
467 |
✓✗✓✗ |
4 |
Data data(humanoid_model), reduced_data(reduced_humanoid_model); |
468 |
✓✗ | 4 |
const Eigen::VectorXd q = reference_config_humanoid; |
469 |
✓✗ | 4 |
Eigen::VectorXd reduced_q(reduced_humanoid_model.nq); |
470 |
|||
471 |
✓✓ | 56 |
for(JointIndex joint_id = 1; joint_id < (JointIndex)reduced_humanoid_model.njoints; ++joint_id) |
472 |
{ |
||
473 |
✓✗ | 54 |
const JointIndex reference_joint_id = humanoid_model.getJointId(reduced_humanoid_model.names[joint_id]); |
474 |
|||
475 |
✓✗ | 54 |
reduced_humanoid_model.joints[joint_id].jointConfigSelector(reduced_q) = |
476 |
✓✗✓✗ |
108 |
humanoid_model.joints[reference_joint_id].jointConfigSelector(q); |
477 |
} |
||
478 |
|||
479 |
✓✗ | 2 |
framesForwardKinematics(humanoid_model,data,q); |
480 |
✓✗ | 2 |
framesForwardKinematics(reduced_humanoid_model,reduced_data,reduced_q); |
481 |
|||
482 |
✓✓ | 126 |
for(size_t frame_id = 0; frame_id < reduced_humanoid_model.frames.size(); ++frame_id) |
483 |
{ |
||
484 |
124 |
const Frame & reduced_frame = reduced_humanoid_model.frames[frame_id]; |
|
485 |
✓✓ | 124 |
switch(reduced_frame.type) |
486 |
{ |
||
487 |
60 |
case JOINT: |
|
488 |
case FIXED_JOINT: |
||
489 |
{ |
||
490 |
// May not be present in the original model |
||
491 |
✓✗✓✗ |
60 |
if(humanoid_model.existJointName(reduced_frame.name)) |
492 |
{ |
||
493 |
✓✗ | 60 |
const JointIndex joint_id = humanoid_model.getJointId(reduced_frame.name); |
494 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
60 |
BOOST_CHECK(data.oMi[joint_id].isApprox(reduced_data.oMf[frame_id])); |
495 |
} |
||
496 |
else if(humanoid_model.existFrame(reduced_frame.name)) |
||
497 |
{ |
||
498 |
const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name); |
||
499 |
BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id])); |
||
500 |
} |
||
501 |
else |
||
502 |
{ |
||
503 |
BOOST_CHECK_MESSAGE(false, "The frame " << reduced_frame.name << " is not presend in the humanoid_model"); |
||
504 |
} |
||
505 |
60 |
break; |
|
506 |
} |
||
507 |
64 |
default: |
|
508 |
{ |
||
509 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
64 |
BOOST_CHECK(humanoid_model.existFrame(reduced_frame.name)); |
510 |
✓✗ | 64 |
const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name); |
511 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
64 |
BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id])); |
512 |
64 |
break; |
|
513 |
} |
||
514 |
|||
515 |
} |
||
516 |
} |
||
517 |
|||
518 |
// Test GeometryObject placements |
||
519 |
✓✗✓✗ |
4 |
GeometryData geom_data(humanoid_geometry), reduded_geom_data(reduced_humanoid_geometry); |
520 |
✓✗ | 2 |
updateGeometryPlacements(humanoid_model,data,humanoid_geometry,geom_data); |
521 |
✓✗ | 2 |
updateGeometryPlacements(reduced_humanoid_model,reduced_data,reduced_humanoid_geometry,reduded_geom_data); |
522 |
|||
523 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(geom_data.oMg.size() == reduded_geom_data.oMg.size()); |
524 |
✓✓ | 56 |
for(FrameIndex i = 0; i < geom_data.oMg.size(); ++i) |
525 |
{ |
||
526 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(geom_data.oMg[i].isApprox(reduded_geom_data.oMg[i])); |
527 |
} |
||
528 |
|||
529 |
// Test other signature |
||
530 |
4 |
std::vector<GeometryModel> full_geometry_models; |
|
531 |
✓✗ | 2 |
full_geometry_models.push_back(humanoid_geometry); |
532 |
✓✗ | 2 |
full_geometry_models.push_back(humanoid_geometry); |
533 |
✓✗ | 2 |
full_geometry_models.push_back(humanoid_geometry); |
534 |
|||
535 |
4 |
std::vector<GeometryModel> reduced_geometry_models; |
|
536 |
|||
537 |
✓✗ | 4 |
Model reduced_humanoid_model_other_sig; |
538 |
✓✗ | 2 |
buildReducedModel(humanoid_model,full_geometry_models,joints_to_lock, |
539 |
reference_config_humanoid,reduced_humanoid_model_other_sig,reduced_geometry_models); |
||
540 |
|||
541 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reduced_geometry_models[0] == reduced_humanoid_geometry); |
542 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reduced_geometry_models[1] == reduced_humanoid_geometry); |
543 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reduced_geometry_models[2] == reduced_humanoid_geometry); |
544 |
2 |
} |
|
545 |
#endif // PINOCCHIO_WITH_HPP_FCL |
||
546 |
|||
547 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_has_configuration_limit) |
548 |
{ |
||
549 |
using namespace Eigen; |
||
550 |
|||
551 |
// Test joint specific function hasConfigurationLimit |
||
552 |
✓✗ | 2 |
JointModelFreeFlyer test_joint_ff = JointModelFreeFlyer(); |
553 |
✓✗ | 4 |
std::vector<bool> cf_limits_ff = test_joint_ff.hasConfigurationLimit(); |
554 |
✓✗ | 4 |
std::vector<bool> cf_limits_tangent_ff = test_joint_ff.hasConfigurationLimitInTangent(); |
555 |
✓✗ | 4 |
std::vector<bool> expected_cf_limits_ff({true, true, true, false, false, false, false}); |
556 |
✓✗ | 4 |
std::vector<bool> expected_cf_limits_tangent_ff({true, true, true, false, false, false}); |
557 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(cf_limits_ff == expected_cf_limits_ff); |
558 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(cf_limits_tangent_ff == expected_cf_limits_tangent_ff); |
559 |
|||
560 |
✓✗ | 2 |
JointModelPlanar test_joint_planar = JointModelPlanar(); |
561 |
✓✗ | 4 |
std::vector<bool> cf_limits_planar = test_joint_planar.hasConfigurationLimit(); |
562 |
✓✗ | 4 |
std::vector<bool> cf_limits_tangent_planar = test_joint_planar.hasConfigurationLimitInTangent(); |
563 |
✓✗ | 4 |
std::vector<bool> expected_cf_limits_planar({true, true, false, false}); |
564 |
✓✗ | 4 |
std::vector<bool> expected_cf_limits_tangent_planar({true, true, false}); |
565 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(cf_limits_planar == expected_cf_limits_planar); |
566 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(cf_limits_tangent_planar == expected_cf_limits_tangent_planar); |
567 |
|||
568 |
✓✗ | 2 |
JointModelPX test_joint_p = JointModelPX(); |
569 |
✓✗ | 4 |
std::vector<bool> cf_limits_prismatic = test_joint_p.hasConfigurationLimit(); |
570 |
✓✗ | 4 |
std::vector<bool> cf_limits_tangent_prismatic = test_joint_p.hasConfigurationLimitInTangent(); |
571 |
✓✗ | 4 |
std::vector<bool> expected_cf_limits_prismatic({true}); |
572 |
✓✗ | 4 |
std::vector<bool> expected_cf_limits_tangent_prismatic({true}); |
573 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(cf_limits_prismatic == expected_cf_limits_prismatic); |
574 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(cf_limits_tangent_prismatic == expected_cf_limits_tangent_prismatic); |
575 |
|||
576 |
// Test model.hasConfigurationLimit() function |
||
577 |
✓✗ | 4 |
Model model; |
578 |
2 |
JointIndex jointId = 0; |
|
579 |
|||
580 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
jointId = model.addJoint(jointId, JointModelFreeFlyer(), SE3::Identity(), "Joint0"); |
581 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
jointId = model.addJoint(jointId, JointModelRZ(), SE3::Identity(), "Joint1"); |
582 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
jointId = model.addJoint(jointId, JointModelRUBZ(), SE3(Matrix3d::Identity(), Vector3d(1.0, 0.0, 0.0)), "Joint2"); |
583 |
|||
584 |
std::vector<bool> expected_cf_limits_model({true, true, true, // translation of FF |
||
585 |
false, false, false, false, // rotation of FF |
||
586 |
true, // roational joint |
||
587 |
✓✗ | 4 |
false, false}); // unbounded rotational joint |
588 |
✓✗ | 4 |
std::vector<bool> model_cf_limits = model.hasConfigurationLimit(); |
589 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK((model_cf_limits == expected_cf_limits_model)); |
590 |
|||
591 |
// Test model.hasConfigurationLimitInTangent() function |
||
592 |
std::vector<bool> expected_cf_limits_tangent_model({true, true, true, // translation of FF |
||
593 |
false, false, false, // rotation of FF |
||
594 |
true, // roational joint |
||
595 |
✓✗ | 4 |
false }); // unbounded rotational joint |
596 |
✓✗ | 4 |
std::vector<bool> model_cf_limits_tangent = model.hasConfigurationLimitInTangent(); |
597 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK((model_cf_limits_tangent == expected_cf_limits_tangent_model)); |
598 |
2 |
} |
|
599 |
|||
600 |
|||
601 |
BOOST_AUTO_TEST_SUITE_END() |
| Generated by: GCOVR (Version 4.2) |