| 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 |
✓✗ | 2 |
appendModel (humanoid, manipulator, geomHumanoid, geomManipulator, fid, |
210 |
✓✗ | 2 |
SE3::Identity(), model, geomModel); |
211 |
|||
212 |
{ |
||
213 |
✓✗✓✗ |
4 |
Model model2 = appendModel(humanoid, manipulator, fid, SE3::Identity()); |
214 |
✓✗ | 4 |
Model model3; |
215 |
✓✗✓✗ |
2 |
appendModel(humanoid, manipulator, fid, SE3::Identity(), model3); |
216 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(model == model2); |
217 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(model == model3); |
218 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(model2 == model3); |
219 |
} |
||
220 |
|||
221 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
BOOST_TEST_MESSAGE(model); |
222 |
|||
223 |
✓✗ | 4 |
Data data (model); |
224 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(model.check(data)); |
225 |
|||
226 |
// Check the model |
||
227 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(model.getJointId("humanoid/chest2_joint"), |
228 |
model.parents[model.getJointId("manipulator/shoulder1_joint")]); |
||
229 |
|||
230 |
// check the joint order and the inertias |
||
231 |
✓✗✓✗ |
2 |
JointIndex chest2 = model.getJointId("humanoid/chest2_joint"); |
232 |
✓✓ | 30 |
for (JointIndex jid = 1; jid < chest2; ++jid) { |
233 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
28 |
BOOST_TEST_MESSAGE("Checking joint " << jid << " " << model.names[jid]); |
234 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
28 |
BOOST_CHECK_EQUAL(model.names[jid], humanoid.names[jid]); |
235 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
28 |
BOOST_CHECK_EQUAL(model.inertias[jid], humanoid.inertias[jid]); |
236 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
28 |
BOOST_CHECK_EQUAL(model.jointPlacements[jid], humanoid.jointPlacements[jid]); |
237 |
} |
||
238 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
BOOST_TEST_MESSAGE("Checking joint " << chest2 << " " << model.names[chest2]); |
239 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(model.names[chest2], humanoid.names[chest2]); |
240 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_MESSAGE(model.inertias[chest2].isApprox(manipulator.inertias[0].se3Action(aMb) + humanoid.inertias[chest2]), |
241 |
model.inertias[chest2] << " != " << manipulator.inertias[0].se3Action(aMb) + humanoid.inertias[chest2]); |
||
242 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(model.jointPlacements[chest2], humanoid.jointPlacements[chest2]); |
243 |
|||
244 |
✓✓ | 14 |
for (JointIndex jid = 1; jid < manipulator.joints.size(); ++jid) { |
245 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
12 |
BOOST_TEST_MESSAGE("Checking joint " << chest2+jid << " " << model.names[chest2+jid]); |
246 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
12 |
BOOST_CHECK_EQUAL(model.names[chest2+jid], manipulator.names[jid]); |
247 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
12 |
BOOST_CHECK_EQUAL(model.inertias[chest2+jid], manipulator.inertias[jid]); |
248 |
✓✓ | 12 |
if (jid==1) |
249 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_EQUAL(model.jointPlacements[chest2+jid], aMb*manipulator.jointPlacements[jid]); |
250 |
else |
||
251 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
10 |
BOOST_CHECK_EQUAL(model.jointPlacements[chest2+jid], manipulator.jointPlacements[jid]); |
252 |
} |
||
253 |
✓✓ | 30 |
for (JointIndex jid = chest2+1; jid < humanoid.joints.size(); ++jid) { |
254 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
28 |
BOOST_TEST_MESSAGE("Checking joint " << jid+manipulator.joints.size()-1 << " " << model.names[jid+manipulator.joints.size()-1]); |
255 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
28 |
BOOST_CHECK_EQUAL(model.names[jid+manipulator.joints.size()-1], humanoid.names[jid]); |
256 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
28 |
BOOST_CHECK_EQUAL(model.inertias[jid+manipulator.joints.size()-1], humanoid.inertias[jid]); |
257 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
28 |
BOOST_CHECK_EQUAL(model.jointPlacements[jid+manipulator.joints.size()-1], humanoid.jointPlacements[jid]); |
258 |
} |
||
259 |
|||
260 |
// Check the frames |
||
261 |
✓✓ | 126 |
for (FrameIndex fid = 1; fid < humanoid.frames.size(); ++fid) { |
262 |
124 |
const Frame& frame = humanoid.frames[fid], |
|
263 |
✓✗ | 248 |
parent = humanoid.frames[frame.previousFrame]; |
264 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
124 |
BOOST_CHECK(model.existFrame (frame.name, frame.type)); |
265 |
✓✗ | 124 |
const Frame& nframe = model.frames[model.getFrameId(frame.name, frame.type)], |
266 |
✓✗ | 248 |
nparent = model.frames[nframe.previousFrame]; |
267 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
124 |
BOOST_CHECK_EQUAL(parent.name, nparent.name); |
268 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
124 |
BOOST_CHECK_EQUAL(frame.placement, nframe.placement); |
269 |
} |
||
270 |
✓✓ | 28 |
for (FrameIndex fid = 1; fid < manipulator.frames.size(); ++fid) { |
271 |
26 |
const Frame& frame = manipulator.frames[fid], |
|
272 |
✓✗ | 52 |
parent = manipulator.frames[frame.previousFrame]; |
273 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
26 |
BOOST_CHECK(model.existFrame (frame.name, frame.type)); |
274 |
✓✗ | 26 |
const Frame& nframe = model.frames[model.getFrameId(frame.name, frame.type)], |
275 |
✓✗ | 52 |
nparent = model.frames[nframe.previousFrame]; |
276 |
✓✓ | 26 |
if (frame.previousFrame > 0) { |
277 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
24 |
BOOST_CHECK_EQUAL(parent.name, nparent.name); |
278 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
24 |
BOOST_CHECK_EQUAL(frame.placement, nframe.placement); |
279 |
} |
||
280 |
} |
||
281 |
2 |
} |
|
282 |
#endif |
||
283 |
|||
284 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_buildReducedModel_empty) |
285 |
{ |
||
286 |
✓✗ | 4 |
Model humanoid_model; |
287 |
✓✗ | 2 |
buildModels::humanoid(humanoid_model); |
288 |
|||
289 |
✓✗✓✗ |
2 |
static const std::vector<JointIndex> empty_index_vector; |
290 |
|||
291 |
✓✗✓✗ |
2 |
humanoid_model.lowerPositionLimit.head<3>().fill(-1.); |
292 |
✓✗✓✗ |
2 |
humanoid_model.upperPositionLimit.head<3>().fill( 1.); |
293 |
|||
294 |
✓✗✓✗ ✓✗ |
2 |
humanoid_model.referenceConfigurations.insert(std::pair<std::string,Eigen::VectorXd>("neutral",neutral(humanoid_model))); |
295 |
✓✗ | 4 |
Eigen::VectorXd reference_config_humanoid = randomConfiguration(humanoid_model); |
296 |
✓✗ | 4 |
Model humanoid_copy_model = buildReducedModel(humanoid_model,empty_index_vector,reference_config_humanoid); |
297 |
|||
298 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(humanoid_copy_model.names == humanoid_model.names); |
299 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(humanoid_copy_model.joints == humanoid_model.joints); |
300 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(humanoid_copy_model == humanoid_model); |
301 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(humanoid_copy_model.referenceConfigurations["neutral"].isApprox(neutral(humanoid_copy_model))); |
302 |
|||
303 |
4 |
const std::vector<JointIndex> empty_joints_to_lock; |
|
304 |
|||
305 |
✓✗ | 4 |
const Model reduced_humanoid_model = buildReducedModel(humanoid_model,empty_joints_to_lock,reference_config_humanoid); |
306 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_model.njoints == humanoid_model.njoints); |
307 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_model.frames == humanoid_model.frames); |
308 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_model.jointPlacements == humanoid_model.jointPlacements); |
309 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_model.joints == humanoid_model.joints); |
310 |
|||
311 |
✓✓ | 60 |
for(JointIndex joint_id = 1; joint_id < (JointIndex)reduced_humanoid_model.njoints; ++joint_id) |
312 |
{ |
||
313 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
58 |
BOOST_CHECK(reduced_humanoid_model.inertias[joint_id].isApprox(humanoid_model.inertias[joint_id])); |
314 |
} |
||
315 |
2 |
} |
|
316 |
|||
317 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_buildReducedModel) |
318 |
{ |
||
319 |
✓✗ | 4 |
Model humanoid_model; |
320 |
✓✗ | 2 |
buildModels::humanoid(humanoid_model); |
321 |
|||
322 |
✓✗✓✗ |
2 |
static const std::vector<JointIndex> empty_index_vector; |
323 |
|||
324 |
✓✗✓✗ |
2 |
humanoid_model.lowerPositionLimit.head<3>().fill(-1.); |
325 |
✓✗✓✗ |
2 |
humanoid_model.upperPositionLimit.head<3>().fill( 1.); |
326 |
|||
327 |
✓✗✓✗ ✓✗ |
2 |
humanoid_model.referenceConfigurations.insert(std::pair<std::string,Eigen::VectorXd>("neutral",neutral(humanoid_model))); |
328 |
✓✗ | 4 |
Eigen::VectorXd reference_config_humanoid = randomConfiguration(humanoid_model); |
329 |
✓✗ | 4 |
Model humanoid_copy_model = buildReducedModel(humanoid_model,empty_index_vector,reference_config_humanoid); |
330 |
|||
331 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(humanoid_copy_model.names == humanoid_model.names); |
332 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(humanoid_copy_model.joints == humanoid_model.joints); |
333 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(humanoid_copy_model == humanoid_model); |
334 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(humanoid_copy_model.referenceConfigurations["neutral"].isApprox(neutral(humanoid_copy_model))); |
335 |
|||
336 |
4 |
std::vector<JointIndex> joints_to_lock; |
|
337 |
✓✗ | 4 |
const std::string joint1_to_lock = "rarm_shoulder2_joint"; |
338 |
✓✗✓✗ |
2 |
joints_to_lock.push_back(humanoid_model.getJointId(joint1_to_lock)); |
339 |
✓✗ | 4 |
const std::string joint2_to_lock = "larm_shoulder2_joint"; |
340 |
✓✗✓✗ |
2 |
joints_to_lock.push_back(humanoid_model.getJointId(joint2_to_lock)); |
341 |
|||
342 |
✓✗ | 4 |
Model reduced_humanoid_model = buildReducedModel(humanoid_model,joints_to_lock,reference_config_humanoid); |
343 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_model.njoints == humanoid_model.njoints-(int)joints_to_lock.size()); |
344 |
|||
345 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_model != humanoid_model); |
346 |
|||
347 |
✓✗ | 4 |
Model reduced_humanoid_model_other_signature; |
348 |
✓✗✓✗ |
2 |
buildReducedModel(humanoid_model,joints_to_lock,reference_config_humanoid,reduced_humanoid_model_other_signature); |
349 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_model == reduced_humanoid_model_other_signature); |
350 |
|||
351 |
// Check that forward kinematics give same results |
||
352 |
✓✗✓✗ |
4 |
Data data(humanoid_model), reduced_data(reduced_humanoid_model); |
353 |
✓✗ | 4 |
Eigen::VectorXd q = reference_config_humanoid; |
354 |
✓✗ | 4 |
Eigen::VectorXd reduced_q(reduced_humanoid_model.nq); |
355 |
|||
356 |
✓✓ | 56 |
for(JointIndex joint_id = 1; joint_id < (JointIndex)reduced_humanoid_model.njoints; ++joint_id) |
357 |
{ |
||
358 |
✓✗ | 54 |
const JointIndex reference_joint_id = humanoid_model.getJointId(reduced_humanoid_model.names[joint_id]); |
359 |
|||
360 |
✓✗ | 54 |
reduced_humanoid_model.joints[joint_id].jointConfigSelector(reduced_q) = |
361 |
✓✗✓✗ |
108 |
humanoid_model.joints[reference_joint_id].jointConfigSelector(q); |
362 |
} |
||
363 |
|||
364 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_model.referenceConfigurations["neutral"].isApprox(neutral(reduced_humanoid_model))); |
365 |
|||
366 |
✓✗ | 2 |
framesForwardKinematics(humanoid_model,data,q); |
367 |
✓✗ | 2 |
framesForwardKinematics(reduced_humanoid_model,reduced_data,reduced_q); |
368 |
|||
369 |
✓✓ | 126 |
for(size_t frame_id = 0; frame_id < reduced_humanoid_model.frames.size(); ++frame_id) |
370 |
{ |
||
371 |
124 |
const Frame & reduced_frame = reduced_humanoid_model.frames[frame_id]; |
|
372 |
✓✓ | 124 |
switch(reduced_frame.type) |
373 |
{ |
||
374 |
60 |
case JOINT: |
|
375 |
case FIXED_JOINT: |
||
376 |
{ |
||
377 |
// May not be present in the original model |
||
378 |
✓✗✓✗ |
60 |
if(humanoid_model.existJointName(reduced_frame.name)) |
379 |
{ |
||
380 |
✓✗ | 60 |
const JointIndex joint_id = humanoid_model.getJointId(reduced_frame.name); |
381 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
60 |
BOOST_CHECK(data.oMi[joint_id].isApprox(reduced_data.oMf[frame_id])); |
382 |
} |
||
383 |
else if(humanoid_model.existFrame(reduced_frame.name)) |
||
384 |
{ |
||
385 |
const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name); |
||
386 |
BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id])); |
||
387 |
} |
||
388 |
else |
||
389 |
{ |
||
390 |
BOOST_CHECK_MESSAGE(false, "The frame " << reduced_frame.name << " is not presend in the humanoid_model"); |
||
391 |
} |
||
392 |
60 |
break; |
|
393 |
} |
||
394 |
64 |
default: |
|
395 |
{ |
||
396 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
64 |
BOOST_CHECK(humanoid_model.existFrame(reduced_frame.name)); |
397 |
✓✗ | 64 |
const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name); |
398 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
64 |
BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id])); |
399 |
64 |
break; |
|
400 |
} |
||
401 |
|||
402 |
} |
||
403 |
} |
||
404 |
2 |
} |
|
405 |
|||
406 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_aligned_vector_of_model) |
407 |
{ |
||
408 |
typedef PINOCCHIO_ALIGNED_STD_VECTOR(Model) VectorOfModels; |
||
409 |
|||
410 |
4 |
VectorOfModels models; |
|
411 |
✓✓ | 202 |
for(size_t k = 0; k < 100; ++k) |
412 |
{ |
||
413 |
✓✗✓✗ |
200 |
models.push_back(Model()); |
414 |
✓✗ | 200 |
buildModels::humanoidRandom(models[k]); |
415 |
} |
||
416 |
2 |
} |
|
417 |
|||
418 |
#ifdef PINOCCHIO_WITH_HPP_FCL |
||
419 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_buildReducedModel_with_geom) |
420 |
{ |
||
421 |
✓✗ | 4 |
Model humanoid_model; |
422 |
✓✗ | 2 |
buildModels::humanoid(humanoid_model); |
423 |
|||
424 |
✓✗✓✗ |
2 |
humanoid_model.lowerPositionLimit.head<3>().fill(-1.); |
425 |
✓✗✓✗ |
2 |
humanoid_model.upperPositionLimit.head<3>().fill( 1.); |
426 |
✓✗ | 4 |
const Eigen::VectorXd reference_config_humanoid = randomConfiguration(humanoid_model); |
427 |
|||
428 |
✓✗ | 4 |
GeometryModel humanoid_geometry; |
429 |
✓✗ | 2 |
buildModels::humanoidGeometries(humanoid_model, humanoid_geometry); |
430 |
|||
431 |
✓✗✓✗ |
2 |
static const std::vector<JointIndex> empty_index_vector; |
432 |
|||
433 |
✓✗✓✗ |
4 |
Model humanoid_copy_model; GeometryModel humanoid_copy_geometry; |
434 |
✓✗ | 2 |
buildReducedModel(humanoid_model,humanoid_geometry,empty_index_vector, |
435 |
reference_config_humanoid,humanoid_copy_model,humanoid_copy_geometry); |
||
436 |
|||
437 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(humanoid_copy_model == humanoid_model); |
438 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(humanoid_copy_geometry == humanoid_geometry); |
439 |
|||
440 |
4 |
std::vector<JointIndex> joints_to_lock; |
|
441 |
✓✗ | 4 |
const std::string joint1_to_lock = "rarm_shoulder2_joint"; |
442 |
✓✗✓✗ |
2 |
joints_to_lock.push_back(humanoid_model.getJointId(joint1_to_lock)); |
443 |
✓✗ | 4 |
const std::string joint2_to_lock = "larm_shoulder2_joint"; |
444 |
✓✗✓✗ |
2 |
joints_to_lock.push_back(humanoid_model.getJointId(joint2_to_lock)); |
445 |
|||
446 |
✓✗✓✗ |
4 |
Model reduced_humanoid_model; GeometryModel reduced_humanoid_geometry; |
447 |
✓✗ | 2 |
buildReducedModel(humanoid_model,humanoid_geometry,joints_to_lock, |
448 |
reference_config_humanoid,reduced_humanoid_model,reduced_humanoid_geometry); |
||
449 |
|||
450 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_geometry.ngeoms == humanoid_geometry.ngeoms); |
451 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_geometry.collisionPairs == humanoid_geometry.collisionPairs); |
452 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(reduced_humanoid_geometry.geometryObjects.size() == humanoid_geometry.geometryObjects.size()); |
453 |
|||
454 |
✓✓ | 56 |
for(Index i = 0; i < humanoid_geometry.geometryObjects.size(); ++i) |
455 |
{ |
||
456 |
54 |
const GeometryObject & go1 = humanoid_geometry.geometryObjects[i]; |
|
457 |
54 |
const GeometryObject & go2 = reduced_humanoid_geometry.geometryObjects[i]; |
|
458 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK_EQUAL(go1.name, go2.name); |
459 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK_EQUAL(go1.geometry, go2.geometry); |
460 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK_EQUAL(go1.meshPath, go2.meshPath); |
461 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK_EQUAL(go1.meshScale, go2.meshScale); |
462 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK_EQUAL(go1.overrideMaterial, go2.overrideMaterial); |
463 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK_EQUAL(go1.meshColor, go2.meshColor); |
464 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK_EQUAL(go1.meshTexturePath, go2.meshTexturePath); |
465 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK_EQUAL(humanoid_model.frames[go1.parentFrame].name, |
466 |
reduced_humanoid_model.frames[go2.parentFrame].name); |
||
467 |
} |
||
468 |
|||
469 |
✓✗✓✗ |
4 |
Data data(humanoid_model), reduced_data(reduced_humanoid_model); |
470 |
✓✗ | 4 |
const Eigen::VectorXd q = reference_config_humanoid; |
471 |
✓✗ | 4 |
Eigen::VectorXd reduced_q(reduced_humanoid_model.nq); |
472 |
|||
473 |
✓✓ | 56 |
for(JointIndex joint_id = 1; joint_id < (JointIndex)reduced_humanoid_model.njoints; ++joint_id) |
474 |
{ |
||
475 |
✓✗ | 54 |
const JointIndex reference_joint_id = humanoid_model.getJointId(reduced_humanoid_model.names[joint_id]); |
476 |
|||
477 |
✓✗ | 54 |
reduced_humanoid_model.joints[joint_id].jointConfigSelector(reduced_q) = |
478 |
✓✗✓✗ |
108 |
humanoid_model.joints[reference_joint_id].jointConfigSelector(q); |
479 |
} |
||
480 |
|||
481 |
✓✗ | 2 |
framesForwardKinematics(humanoid_model,data,q); |
482 |
✓✗ | 2 |
framesForwardKinematics(reduced_humanoid_model,reduced_data,reduced_q); |
483 |
|||
484 |
✓✓ | 126 |
for(size_t frame_id = 0; frame_id < reduced_humanoid_model.frames.size(); ++frame_id) |
485 |
{ |
||
486 |
124 |
const Frame & reduced_frame = reduced_humanoid_model.frames[frame_id]; |
|
487 |
✓✓ | 124 |
switch(reduced_frame.type) |
488 |
{ |
||
489 |
60 |
case JOINT: |
|
490 |
case FIXED_JOINT: |
||
491 |
{ |
||
492 |
// May not be present in the original model |
||
493 |
✓✗✓✗ |
60 |
if(humanoid_model.existJointName(reduced_frame.name)) |
494 |
{ |
||
495 |
✓✗ | 60 |
const JointIndex joint_id = humanoid_model.getJointId(reduced_frame.name); |
496 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
60 |
BOOST_CHECK(data.oMi[joint_id].isApprox(reduced_data.oMf[frame_id])); |
497 |
} |
||
498 |
else if(humanoid_model.existFrame(reduced_frame.name)) |
||
499 |
{ |
||
500 |
const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name); |
||
501 |
BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id])); |
||
502 |
} |
||
503 |
else |
||
504 |
{ |
||
505 |
BOOST_CHECK_MESSAGE(false, "The frame " << reduced_frame.name << " is not presend in the humanoid_model"); |
||
506 |
} |
||
507 |
60 |
break; |
|
508 |
} |
||
509 |
64 |
default: |
|
510 |
{ |
||
511 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
64 |
BOOST_CHECK(humanoid_model.existFrame(reduced_frame.name)); |
512 |
✓✗ | 64 |
const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name); |
513 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
64 |
BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id])); |
514 |
64 |
break; |
|
515 |
} |
||
516 |
|||
517 |
} |
||
518 |
} |
||
519 |
|||
520 |
// Test GeometryObject placements |
||
521 |
✓✗✓✗ |
4 |
GeometryData geom_data(humanoid_geometry), reduded_geom_data(reduced_humanoid_geometry); |
522 |
✓✗ | 2 |
updateGeometryPlacements(humanoid_model,data,humanoid_geometry,geom_data); |
523 |
✓✗ | 2 |
updateGeometryPlacements(reduced_humanoid_model,reduced_data,reduced_humanoid_geometry,reduded_geom_data); |
524 |
|||
525 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(geom_data.oMg.size() == reduded_geom_data.oMg.size()); |
526 |
✓✓ | 56 |
for(FrameIndex i = 0; i < geom_data.oMg.size(); ++i) |
527 |
{ |
||
528 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(geom_data.oMg[i].isApprox(reduded_geom_data.oMg[i])); |
529 |
} |
||
530 |
|||
531 |
// Test other signature |
||
532 |
4 |
std::vector<GeometryModel> full_geometry_models; |
|
533 |
✓✗ | 2 |
full_geometry_models.push_back(humanoid_geometry); |
534 |
✓✗ | 2 |
full_geometry_models.push_back(humanoid_geometry); |
535 |
✓✗ | 2 |
full_geometry_models.push_back(humanoid_geometry); |
536 |
|||
537 |
4 |
std::vector<GeometryModel> reduced_geometry_models; |
|
538 |
|||
539 |
✓✗ | 4 |
Model reduced_humanoid_model_other_sig; |
540 |
✓✗ | 2 |
buildReducedModel(humanoid_model,full_geometry_models,joints_to_lock, |
541 |
reference_config_humanoid,reduced_humanoid_model_other_sig,reduced_geometry_models); |
||
542 |
|||
543 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reduced_geometry_models[0] == reduced_humanoid_geometry); |
544 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reduced_geometry_models[1] == reduced_humanoid_geometry); |
545 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(reduced_geometry_models[2] == reduced_humanoid_geometry); |
546 |
2 |
} |
|
547 |
#endif // PINOCCHIO_WITH_HPP_FCL |
||
548 |
|||
549 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_has_configuration_limit) |
550 |
{ |
||
551 |
using namespace Eigen; |
||
552 |
|||
553 |
// Test joint specific function hasConfigurationLimit |
||
554 |
✓✗ | 2 |
JointModelFreeFlyer test_joint_ff = JointModelFreeFlyer(); |
555 |
✓✗ | 4 |
std::vector<bool> cf_limits_ff = test_joint_ff.hasConfigurationLimit(); |
556 |
✓✗ | 4 |
std::vector<bool> cf_limits_tangent_ff = test_joint_ff.hasConfigurationLimitInTangent(); |
557 |
✓✗ | 4 |
std::vector<bool> expected_cf_limits_ff({true, true, true, false, false, false, false}); |
558 |
✓✗ | 4 |
std::vector<bool> expected_cf_limits_tangent_ff({true, true, true, false, false, false}); |
559 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(cf_limits_ff == expected_cf_limits_ff); |
560 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(cf_limits_tangent_ff == expected_cf_limits_tangent_ff); |
561 |
|||
562 |
✓✗ | 2 |
JointModelPlanar test_joint_planar = JointModelPlanar(); |
563 |
✓✗ | 4 |
std::vector<bool> cf_limits_planar = test_joint_planar.hasConfigurationLimit(); |
564 |
✓✗ | 4 |
std::vector<bool> cf_limits_tangent_planar = test_joint_planar.hasConfigurationLimitInTangent(); |
565 |
✓✗ | 4 |
std::vector<bool> expected_cf_limits_planar({true, true, false, false}); |
566 |
✓✗ | 4 |
std::vector<bool> expected_cf_limits_tangent_planar({true, true, false}); |
567 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(cf_limits_planar == expected_cf_limits_planar); |
568 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(cf_limits_tangent_planar == expected_cf_limits_tangent_planar); |
569 |
|||
570 |
✓✗ | 2 |
JointModelPX test_joint_p = JointModelPX(); |
571 |
✓✗ | 4 |
std::vector<bool> cf_limits_prismatic = test_joint_p.hasConfigurationLimit(); |
572 |
✓✗ | 4 |
std::vector<bool> cf_limits_tangent_prismatic = test_joint_p.hasConfigurationLimitInTangent(); |
573 |
✓✗ | 4 |
std::vector<bool> expected_cf_limits_prismatic({true}); |
574 |
✓✗ | 4 |
std::vector<bool> expected_cf_limits_tangent_prismatic({true}); |
575 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(cf_limits_prismatic == expected_cf_limits_prismatic); |
576 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(cf_limits_tangent_prismatic == expected_cf_limits_tangent_prismatic); |
577 |
|||
578 |
// Test model.hasConfigurationLimit() function |
||
579 |
✓✗ | 4 |
Model model; |
580 |
2 |
JointIndex jointId = 0; |
|
581 |
|||
582 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
jointId = model.addJoint(jointId, JointModelFreeFlyer(), SE3::Identity(), "Joint0"); |
583 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
jointId = model.addJoint(jointId, JointModelRZ(), SE3::Identity(), "Joint1"); |
584 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
jointId = model.addJoint(jointId, JointModelRUBZ(), SE3(Matrix3d::Identity(), Vector3d(1.0, 0.0, 0.0)), "Joint2"); |
585 |
|||
586 |
std::vector<bool> expected_cf_limits_model({true, true, true, // translation of FF |
||
587 |
false, false, false, false, // rotation of FF |
||
588 |
true, // roational joint |
||
589 |
✓✗ | 4 |
false, false}); // unbounded rotational joint |
590 |
✓✗ | 4 |
std::vector<bool> model_cf_limits = model.hasConfigurationLimit(); |
591 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK((model_cf_limits == expected_cf_limits_model)); |
592 |
|||
593 |
// Test model.hasConfigurationLimitInTangent() function |
||
594 |
std::vector<bool> expected_cf_limits_tangent_model({true, true, true, // translation of FF |
||
595 |
false, false, false, // rotation of FF |
||
596 |
true, // roational joint |
||
597 |
✓✗ | 4 |
false }); // unbounded rotational joint |
598 |
✓✗ | 4 |
std::vector<bool> model_cf_limits_tangent = model.hasConfigurationLimitInTangent(); |
599 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK((model_cf_limits_tangent == expected_cf_limits_tangent_model)); |
600 |
2 |
} |
|
601 |
|||
602 |
|||
603 |
BOOST_AUTO_TEST_SUITE_END() |
| Generated by: GCOVR (Version 4.2) |