Debian Codegraph @

unittests/integration/test_NameManagement.cpp

raw
1 /*
2 * Copyright (c) 2011-2022, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include <iostream>
34
35 #include <gtest/gtest.h>
36
37 #include "dart/dynamics/SoftBodyNode.hpp"
38
39 #include "TestHelpers.hpp"
40
41 using namespace dart;
42 using namespace math;
43 using namespace dynamics;
44
45 //==============================================================================
46 TEST(NameManagement, Skeleton)
47 {
48 SkeletonPtr skel = Skeleton::create();
49
50 std::pair<Joint*, BodyNode*> pair;
51 pair = skel->createJointAndBodyNodePair<RevoluteJoint>(
52 nullptr, GenericJoint<R1Space>::Properties(std::string("joint")));
53 Joint* joint1 = pair.first;
54 BodyNode* body1 = pair.second;
55
56 pair = skel->createJointAndBodyNodePair<TranslationalJoint>(
57 body1, GenericJoint<R3Space>::Properties(std::string("joint")));
58 Joint* joint2 = pair.first;
59 BodyNode* body2 = pair.second;
60
61 pair = skel->createJointAndBodyNodePair<FreeJoint>(
62 body2, GenericJoint<SE3Space>::Properties(std::string("joint")));
63 Joint* joint3 = pair.first;
64 BodyNode* body3 = pair.second;
65
66 // Testing whether the repeated names of BodyNodes and Joints get resolved
67 // correctly as BodyNodes get added to the Skeleton
68 EXPECT_FALSE(body1->getName() == body2->getName());
69 EXPECT_FALSE(body2->getName() == body3->getName());
70 EXPECT_FALSE(body3->getName() == body1->getName());
71
72 EXPECT_TRUE(body1->getName() == "BodyNode");
73 EXPECT_TRUE(body2->getName() == "BodyNode(1)");
74 EXPECT_TRUE(body3->getName() == "BodyNode(2)");
75
76 EXPECT_FALSE(joint1->getName() == joint2->getName());
77 EXPECT_FALSE(joint2->getName() == joint3->getName());
78 EXPECT_FALSE(joint3->getName() == joint1->getName());
79
80 EXPECT_TRUE(joint1->getDof(0)->getName() == "joint");
81
82 EXPECT_TRUE(joint2->getDof(0)->getName() == "joint(1)_x");
83 EXPECT_TRUE(joint2->getDof(1)->getName() == "joint(1)_y");
84 EXPECT_TRUE(joint2->getDof(2)->getName() == "joint(1)_z");
85
86 EXPECT_TRUE(joint3->getDof(0)->getName() == "joint(2)_rot_x");
87 EXPECT_TRUE(joint3->getDof(1)->getName() == "joint(2)_rot_y");
88 EXPECT_TRUE(joint3->getDof(2)->getName() == "joint(2)_rot_z");
89 EXPECT_TRUE(joint3->getDof(3)->getName() == "joint(2)_pos_x");
90 EXPECT_TRUE(joint3->getDof(4)->getName() == "joint(2)_pos_y");
91 EXPECT_TRUE(joint3->getDof(5)->getName() == "joint(2)_pos_z");
92
93 // Testing whether the repeated names of BodyNodes get resolved correctly
94 // as they are changed with BodyNode::setName(~)
95 std::string newname1 = body1->setName("same_name");
96 std::string newname2 = body2->setName("same_name");
97 std::string newname3 = body3->setName("same_name");
98
99 EXPECT_FALSE(body1->getName() == body2->getName());
100 EXPECT_FALSE(body2->getName() == body3->getName());
101 EXPECT_FALSE(body3->getName() == body1->getName());
102
103 EXPECT_TRUE(body1->getName() == newname1);
104 EXPECT_TRUE(body2->getName() == newname2);
105 EXPECT_TRUE(body3->getName() == newname3);
106
107 EXPECT_TRUE(skel->getBodyNode(newname1) == body1);
108 EXPECT_TRUE(skel->getBodyNode(newname2) == body2);
109 EXPECT_TRUE(skel->getBodyNode(newname3) == body3);
110
111 // Testing whether the repeated names of Joints get resolved correctly
112 // as they are changed with Joint::setName(~)
113 newname1 = joint1->setName("another_name");
114 newname2 = joint2->setName("another_name");
115 newname3 = joint3->setName("another_name");
116
117 EXPECT_FALSE(joint1->getName() == joint2->getName());
118 EXPECT_FALSE(joint2->getName() == joint3->getName());
119 EXPECT_FALSE(joint3->getName() == joint1->getName());
120
121 EXPECT_TRUE(joint1->getName() == newname1);
122 EXPECT_TRUE(joint2->getName() == newname2);
123 EXPECT_TRUE(joint3->getName() == newname3);
124
125 EXPECT_TRUE(skel->getJoint(newname1) == joint1);
126 EXPECT_TRUE(skel->getJoint(newname2) == joint2);
127 EXPECT_TRUE(skel->getJoint(newname3) == joint3);
128
129 // Testing whether unique names get accidentally changed by the NameManager
130 std::string unique_name = body2->setName("a_unique_name");
131 EXPECT_TRUE(body2->getName() == unique_name);
132 EXPECT_TRUE(body2->getName() == "a_unique_name");
133 EXPECT_TRUE(skel->getBodyNode("a_unique_name") == body2);
134
135 EXPECT_FALSE(body1->getName() == body2->getName());
136 EXPECT_FALSE(body2->getName() == body3->getName());
137 EXPECT_FALSE(body3->getName() == body1->getName());
138
139 unique_name = joint3->setName("a_unique_name");
140 EXPECT_TRUE(joint3->getName() == unique_name);
141 EXPECT_TRUE(joint3->getName() == "a_unique_name");
142 EXPECT_TRUE(skel->getJoint("a_unique_name") == joint3);
143
144 // Testing whether the DegreeOfFreedom names get updated correctly upon their
145 // joint's name change
146 EXPECT_TRUE(joint3->getDof(0)->getName() == "a_unique_name_rot_x");
147 EXPECT_TRUE(joint3->getDof(1)->getName() == "a_unique_name_rot_y");
148 EXPECT_TRUE(joint3->getDof(2)->getName() == "a_unique_name_rot_z");
149 EXPECT_TRUE(joint3->getDof(3)->getName() == "a_unique_name_pos_x");
150 EXPECT_TRUE(joint3->getDof(4)->getName() == "a_unique_name_pos_y");
151 EXPECT_TRUE(joint3->getDof(5)->getName() == "a_unique_name_pos_z");
152
153 EXPECT_TRUE(joint3->getDof(0) == skel->getDof("a_unique_name_rot_x"));
154 EXPECT_TRUE(joint3->getDof(3) == skel->getDof("a_unique_name_pos_x"));
155
156 // Note: The following assumes the joint order in the Skeleton is:
157 // RevoluteJoint -> TranslationalJoint -> FreeJoint
158 EXPECT_TRUE(joint1->getDof(0) == skel->getDof(0));
159
160 EXPECT_TRUE(joint2->getDof(0) == skel->getDof(1));
161 EXPECT_TRUE(joint2->getDof(1) == skel->getDof(2));
162 EXPECT_TRUE(joint2->getDof(2) == skel->getDof(3));
163
164 EXPECT_TRUE(joint3->getDof(0) == skel->getDof(4));
165 EXPECT_TRUE(joint3->getDof(1) == skel->getDof(5));
166 EXPECT_TRUE(joint3->getDof(2) == skel->getDof(6));
167 EXPECT_TRUE(joint3->getDof(3) == skel->getDof(7));
168 EXPECT_TRUE(joint3->getDof(4) == skel->getDof(8));
169 EXPECT_TRUE(joint3->getDof(5) == skel->getDof(9));
170
171 // Test whether the return of getIndexInSkeleton() and the index of the
172 // corresponding DegreeOfFreedom in the Skeleton are same
173 for (std::size_t i = 0; i < skel->getNumDofs(); ++i)
174 EXPECT_TRUE(skel->getDof(i)->getIndexInSkeleton() == i);
175
176 // Test whether all the joint names are still unique
177 EXPECT_FALSE(joint1->getName() == joint2->getName());
178 EXPECT_FALSE(joint2->getName() == joint3->getName());
179 EXPECT_FALSE(joint3->getName() == joint1->getName());
180
181 // Make sure that the Skeleton gives back nullptr for non existent names
182 EXPECT_TRUE(skel->getBodyNode("nonexistent_name") == nullptr);
183 EXPECT_TRUE(skel->getJoint("nonexistent_name") == nullptr);
184 EXPECT_TRUE(skel->getSoftBodyNode("nonexistent_name") == nullptr);
185
186 // Test Node Names
187 EndEffector* ee1 = body1->createEndEffector("ee");
188 EndEffector* ee2 = body1->createEndEffector("ee");
189
190 EXPECT_TRUE(skel->getEndEffector("ee") == ee1);
191 EXPECT_TRUE(skel->getEndEffector("ee(1)") == ee2);
192 }
193
194 //==============================================================================
195 TEST(NameManagement, SetPattern)
196 {
197 dart::common::NameManager<std::shared_ptr<Entity> > test_mgr("test", "name");
198
199 std::shared_ptr<Entity> entity0(new SimpleFrame(Frame::World(), "name"));
200 std::shared_ptr<Entity> entity1(new SimpleFrame(Frame::World(), "name"));
201 std::shared_ptr<Entity> entity2(new SimpleFrame(Frame::World(), "name"));
202
203 test_mgr.setPattern("%s(%d)");
204
205 test_mgr.issueNewNameAndAdd(entity0->getName(), entity0);
206 test_mgr.issueNewNameAndAdd(entity1->getName(), entity1);
207 test_mgr.issueNewNameAndAdd(entity2->getName(), entity2);
208
209 EXPECT_TRUE(test_mgr.getObject("name") == entity0);
210 EXPECT_TRUE(test_mgr.getObject("name(1)") == entity1);
211 EXPECT_TRUE(test_mgr.getObject("name(2)") == entity2);
212
213 test_mgr.clear();
214
215 entity0->setName("Entity");
216 entity1->setName("Entity");
217 entity2->setName("Entity");
218
219 test_mgr.setPattern("(%d)-%s");
220 test_mgr.issueNewNameAndAdd(entity0->getName(), entity0);
221 test_mgr.issueNewNameAndAdd(entity1->getName(), entity1);
222 test_mgr.issueNewNameAndAdd(entity2->getName(), entity2);
223
224 EXPECT_TRUE(test_mgr.getObject("Entity") == entity0);
225 EXPECT_TRUE(test_mgr.getObject("(1)-Entity") == entity1);
226 EXPECT_TRUE(test_mgr.getObject("(2)-Entity") == entity2);
227 }
228
229 //==============================================================================
230 TEST(NameManagement, Regression554)
231 {
232 dart::common::NameManager<std::shared_ptr<int> > test_mgr("test", "name");
233
234 std::shared_ptr<int> int0(new int(0));
235 std::shared_ptr<int> int1(new int(1));
236 std::shared_ptr<int> int2(new int(2));
237 std::shared_ptr<int> int_another0(new int(0));
238
239 test_mgr.issueNewNameAndAdd(std::to_string(*int0), int0);
240 test_mgr.issueNewNameAndAdd(std::to_string(*int1), int1);
241 test_mgr.issueNewNameAndAdd(std::to_string(*int2), int2);
242
243 EXPECT_TRUE(test_mgr.getObject("0") == int0);
244 EXPECT_TRUE(test_mgr.getObject("1") == int1);
245 EXPECT_TRUE(test_mgr.getObject("2") == int2);
246
247 bool res1 = test_mgr.addName(std::to_string(*int_another0), int_another0);
248 EXPECT_FALSE(res1);
249
250 test_mgr.removeEntries(std::to_string(*int_another0), int_another0);
251 bool res2 = test_mgr.addName(std::to_string(*int_another0), int_another0);
252 EXPECT_TRUE(res2);
253
254 EXPECT_TRUE(test_mgr.getObject("0") == int_another0);
255 EXPECT_TRUE(test_mgr.getObject("1") == int1);
256 EXPECT_TRUE(test_mgr.getObject("2") == int2);
257 }
258
259 //==============================================================================
260 TEST(NameManagement, WorldSkeletons)
261 {
262 dart::simulation::WorldPtr world1(new dart::simulation::World);
263 world1->setName("world1");
264
265 dart::dynamics::SkeletonPtr skel0 = dart::dynamics::Skeleton::create();
266 dart::dynamics::SkeletonPtr skel1 = dart::dynamics::Skeleton::create();
267 dart::dynamics::SkeletonPtr skel2 = dart::dynamics::Skeleton::create();
268
269 world1->addSkeleton(skel0);
270 world1->addSkeleton(skel1);
271 world1->addSkeleton(skel2);
272
273 EXPECT_TRUE(skel0->getName() == "Skeleton");
274 EXPECT_TRUE(skel1->getName() == "Skeleton(1)");
275 EXPECT_TRUE(skel2->getName() == "Skeleton(2)");
276
277 skel1->setName("Skeleton");
278 EXPECT_TRUE(skel1->getName() == "Skeleton(1)");
279 skel1->setName("OtherName");
280 EXPECT_TRUE(skel1->getName() == "OtherName");
281
282 skel2->setName("Skeleton");
283 EXPECT_TRUE(skel2->getName() == "Skeleton(1)");
284 skel2->setName("OtherName");
285 EXPECT_TRUE(skel2->getName() == "OtherName(1)");
286
287 EXPECT_TRUE(skel0 == world1->getSkeleton(skel0->getName()));
288 EXPECT_TRUE(skel1 == world1->getSkeleton(skel1->getName()));
289 EXPECT_TRUE(skel2 == world1->getSkeleton(skel2->getName()));
290
291 dart::simulation::WorldPtr world2(new dart::simulation::World);
292 world2->setName("world2");
293
294 dart::dynamics::SkeletonPtr skel3
295 = dart::dynamics::Skeleton::create("OtherName");
296 world2->addSkeleton(skel3);
297 world2->addSkeleton(skel2);
298 world2->addSkeleton(skel1);
299
300 EXPECT_TRUE(skel3->getName() == "OtherName");
301 EXPECT_TRUE(skel1->getName() == "OtherName(2)");
302 EXPECT_TRUE(skel2->getName() == "OtherName(1)");
303
304 skel3->setName("Skeleton(1)");
305 skel1->setName("Skeleton");
306
307 EXPECT_TRUE(skel3->getName() == "Skeleton(1)");
308 EXPECT_TRUE(skel1->getName() == "Skeleton(1)(1)");
309
310 EXPECT_TRUE(skel0 == world1->getSkeleton(skel0->getName()));
311 EXPECT_TRUE(skel1 == world1->getSkeleton(skel1->getName()));
312 EXPECT_TRUE(skel2 == world1->getSkeleton(skel2->getName()));
313
314 EXPECT_TRUE(skel3 == world2->getSkeleton(skel3->getName()));
315 EXPECT_TRUE(skel1 == world2->getSkeleton(skel1->getName()));
316 EXPECT_TRUE(skel2 == world2->getSkeleton(skel2->getName()));
317
318 world2->removeSkeleton(skel1);
319
320 skel1->setName("Skeleton");
321 EXPECT_TRUE(skel1->getName() == "Skeleton(1)");
322 }
323
324 //==============================================================================
325 TEST(NameManagement, WorldSimpleFrames)
326 {
327 dart::simulation::WorldPtr world1(new dart::simulation::World);
328
329 dart::dynamics::SimpleFramePtr frame0(
330 new dart::dynamics::SimpleFrame(Frame::World(), "Frame"));
331 dart::dynamics::SimpleFramePtr frame1(
332 new dart::dynamics::SimpleFrame(Frame::World(), "Frame"));
333 dart::dynamics::SimpleFramePtr frame2(
334 new dart::dynamics::SimpleFrame(Frame::World(), "Frame"));
335
336 world1->addSimpleFrame(frame0);
337 world1->addSimpleFrame(frame1);
338 world1->addSimpleFrame(frame2);
339
340 EXPECT_TRUE(frame0->getName() == "Frame");
341 EXPECT_TRUE(frame1->getName() == "Frame(1)");
342 EXPECT_TRUE(frame2->getName() == "Frame(2)");
343
344 frame1->setName("Frame");
345 EXPECT_TRUE(frame1->getName() == "Frame(1)");
346 frame1->setName("OtherName");
347 EXPECT_TRUE(frame1->getName() == "OtherName");
348
349 frame2->setName("Frame");
350 EXPECT_TRUE(frame2->getName() == "Frame(1)");
351 frame2->setName("OtherName");
352 EXPECT_TRUE(frame2->getName() == "OtherName(1)");
353
354 EXPECT_TRUE(frame0 == world1->getSimpleFrame(frame0->getName()));
355 EXPECT_TRUE(frame1 == world1->getSimpleFrame(frame1->getName()));
356 EXPECT_TRUE(frame2 == world1->getSimpleFrame(frame2->getName()));
357
358 dart::simulation::WorldPtr world2(new dart::simulation::World);
359 world2->setName("world2");
360
361 dart::dynamics::SimpleFramePtr frame3(
362 new dart::dynamics::SimpleFrame(Frame::World(), "OtherName"));
363 world2->addSimpleFrame(frame3);
364 world2->addSimpleFrame(frame2);
365 world2->addSimpleFrame(frame1);
366
367 EXPECT_TRUE(frame3->getName() == "OtherName");
368 EXPECT_TRUE(frame1->getName() == "OtherName(2)");
369 EXPECT_TRUE(frame2->getName() == "OtherName(1)");
370
371 frame3->setName("Frame(1)");
372 frame1->setName("Frame");
373
374 EXPECT_TRUE(frame3->getName() == "Frame(1)");
375 EXPECT_TRUE(frame1->getName() == "Frame(1)(1)");
376
377 EXPECT_TRUE(frame0 == world1->getSimpleFrame(frame0->getName()));
378 EXPECT_TRUE(frame1 == world1->getSimpleFrame(frame1->getName()));
379 EXPECT_TRUE(frame2 == world1->getSimpleFrame(frame2->getName()));
380
381 EXPECT_TRUE(frame3 == world2->getSimpleFrame(frame3->getName()));
382 EXPECT_TRUE(frame1 == world2->getSimpleFrame(frame1->getName()));
383 EXPECT_TRUE(frame2 == world2->getSimpleFrame(frame2->getName()));
384
385 world2->removeSimpleFrame(frame1);
386
387 frame1->setName("Frame");
388 EXPECT_TRUE(frame1->getName() == "Frame(1)");
389 }
390
391 //==============================================================================
392 TEST(NameManagement, JointDegreeOfFreedom)
393 {
394 SkeletonPtr subtree = Skeleton::create("subtree");
395 EulerJoint::Properties jointProperties;
396 jointProperties.mName = "j_bicep_right";
397 JointPtr joint
398 = subtree
399 ->createJointAndBodyNodePair<EulerJoint>(nullptr, jointProperties)
400 .first;
401 std::string newName = "j_bicep_right_inverse";
402
403 EXPECT_TRUE(joint->getDof(0)->getName() == "j_bicep_right_x");
404 EXPECT_TRUE(joint->getDof(1)->getName() == "j_bicep_right_y");
405 EXPECT_TRUE(joint->getDof(2)->getName() == "j_bicep_right_z");
406
407 joint->setName(newName, false);
408
409 EXPECT_TRUE(joint->getDof(0)->getName() == "j_bicep_right_x");
410 EXPECT_TRUE(joint->getDof(1)->getName() == "j_bicep_right_y");
411 EXPECT_TRUE(joint->getDof(2)->getName() == "j_bicep_right_z");
412
413 joint->setName(newName, true);
414
415 EXPECT_TRUE(joint->getDof(0)->getName() == "j_bicep_right_inverse_x");
416 EXPECT_TRUE(joint->getDof(1)->getName() == "j_bicep_right_inverse_y");
417 EXPECT_TRUE(joint->getDof(2)->getName() == "j_bicep_right_inverse_z");
418
419 newName = "j_bicep_left";
420 joint->setName(newName, true);
421
422 EXPECT_TRUE(joint->getDof(0)->getName() == "j_bicep_left_x");
423 EXPECT_TRUE(joint->getDof(1)->getName() == "j_bicep_left_y");
424 EXPECT_TRUE(joint->getDof(2)->getName() == "j_bicep_left_z");
425 }