1 /*
2 * Copyright (c) 2011-2021, 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 #include <gtest/gtest.h>
35 #include "dart/dynamics/SoftBodyNode.hpp"
36 #include "TestHelpers.hpp"
37
38 using namespace dart;
39 using namespace math;
40 using namespace dynamics;
41
42 //==============================================================================
TEST(NameManagement,Skeleton)43 TEST(NameManagement, Skeleton)
44 {
45 SkeletonPtr skel = Skeleton::create();
46
47 std::pair<Joint*, BodyNode*> pair;
48 pair = skel->createJointAndBodyNodePair<RevoluteJoint>(
49 nullptr, GenericJoint<R1Space>::Properties(std::string("joint")));
50 Joint* joint1 = pair.first;
51 BodyNode* body1 = pair.second;
52
53 pair = skel->createJointAndBodyNodePair<TranslationalJoint>(
54 body1, GenericJoint<R3Space>::Properties(std::string("joint")));
55 Joint* joint2 = pair.first;
56 BodyNode* body2 = pair.second;
57
58 pair = skel->createJointAndBodyNodePair<FreeJoint>(
59 body2, GenericJoint<SE3Space>::Properties(std::string("joint")));
60 Joint* joint3 = pair.first;
61 BodyNode* body3 = pair.second;
62
63 // Testing whether the repeated names of BodyNodes and Joints get resolved
64 // correctly as BodyNodes get added to the Skeleton
65 EXPECT_FALSE(body1->getName() == body2->getName());
66 EXPECT_FALSE(body2->getName() == body3->getName());
67 EXPECT_FALSE(body3->getName() == body1->getName());
68
69 EXPECT_TRUE(body1->getName() == "BodyNode");
70 EXPECT_TRUE(body2->getName() == "BodyNode(1)");
71 EXPECT_TRUE(body3->getName() == "BodyNode(2)");
72
73 EXPECT_FALSE(joint1->getName() == joint2->getName());
74 EXPECT_FALSE(joint2->getName() == joint3->getName());
75 EXPECT_FALSE(joint3->getName() == joint1->getName());
76
77 EXPECT_TRUE(joint1->getDof(0)->getName() == "joint");
78
79 EXPECT_TRUE(joint2->getDof(0)->getName() == "joint(1)_x");
80 EXPECT_TRUE(joint2->getDof(1)->getName() == "joint(1)_y");
81 EXPECT_TRUE(joint2->getDof(2)->getName() == "joint(1)_z");
82
83 EXPECT_TRUE(joint3->getDof(0)->getName() == "joint(2)_rot_x");
84 EXPECT_TRUE(joint3->getDof(1)->getName() == "joint(2)_rot_y");
85 EXPECT_TRUE(joint3->getDof(2)->getName() == "joint(2)_rot_z");
86 EXPECT_TRUE(joint3->getDof(3)->getName() == "joint(2)_pos_x");
87 EXPECT_TRUE(joint3->getDof(4)->getName() == "joint(2)_pos_y");
88 EXPECT_TRUE(joint3->getDof(5)->getName() == "joint(2)_pos_z");
89
90 // Testing whether the repeated names of BodyNodes get resolved correctly
91 // as they are changed with BodyNode::setName(~)
92 std::string newname1 = body1->setName("same_name");
93 std::string newname2 = body2->setName("same_name");
94 std::string newname3 = body3->setName("same_name");
95
96 EXPECT_FALSE(body1->getName() == body2->getName());
97 EXPECT_FALSE(body2->getName() == body3->getName());
98 EXPECT_FALSE(body3->getName() == body1->getName());
99
100 EXPECT_TRUE(body1->getName() == newname1);
101 EXPECT_TRUE(body2->getName() == newname2);
102 EXPECT_TRUE(body3->getName() == newname3);
103
104 EXPECT_TRUE(skel->getBodyNode(newname1) == body1);
105 EXPECT_TRUE(skel->getBodyNode(newname2) == body2);
106 EXPECT_TRUE(skel->getBodyNode(newname3) == body3);
107
108 // Testing whether the repeated names of Joints get resolved correctly
109 // as they are changed with Joint::setName(~)
110 newname1 = joint1->setName("another_name");
111 newname2 = joint2->setName("another_name");
112 newname3 = joint3->setName("another_name");
113
114 EXPECT_FALSE(joint1->getName() == joint2->getName());
115 EXPECT_FALSE(joint2->getName() == joint3->getName());
116 EXPECT_FALSE(joint3->getName() == joint1->getName());
117
118 EXPECT_TRUE(joint1->getName() == newname1);
119 EXPECT_TRUE(joint2->getName() == newname2);
120 EXPECT_TRUE(joint3->getName() == newname3);
121
122 EXPECT_TRUE(skel->getJoint(newname1) == joint1);
123 EXPECT_TRUE(skel->getJoint(newname2) == joint2);
124 EXPECT_TRUE(skel->getJoint(newname3) == joint3);
125
126 // Testing whether unique names get accidentally changed by the NameManager
127 std::string unique_name = body2->setName("a_unique_name");
128 EXPECT_TRUE(body2->getName() == unique_name);
129 EXPECT_TRUE(body2->getName() == "a_unique_name");
130 EXPECT_TRUE(skel->getBodyNode("a_unique_name") == body2);
131
132 EXPECT_FALSE(body1->getName() == body2->getName());
133 EXPECT_FALSE(body2->getName() == body3->getName());
134 EXPECT_FALSE(body3->getName() == body1->getName());
135
136 unique_name = joint3->setName("a_unique_name");
137 EXPECT_TRUE(joint3->getName() == unique_name);
138 EXPECT_TRUE(joint3->getName() == "a_unique_name");
139 EXPECT_TRUE(skel->getJoint("a_unique_name") == joint3);
140
141 // Testing whether the DegreeOfFreedom names get updated correctly upon their
142 // joint's name change
143 EXPECT_TRUE(joint3->getDof(0)->getName() == "a_unique_name_rot_x");
144 EXPECT_TRUE(joint3->getDof(1)->getName() == "a_unique_name_rot_y");
145 EXPECT_TRUE(joint3->getDof(2)->getName() == "a_unique_name_rot_z");
146 EXPECT_TRUE(joint3->getDof(3)->getName() == "a_unique_name_pos_x");
147 EXPECT_TRUE(joint3->getDof(4)->getName() == "a_unique_name_pos_y");
148 EXPECT_TRUE(joint3->getDof(5)->getName() == "a_unique_name_pos_z");
149
150 EXPECT_TRUE(joint3->getDof(0) == skel->getDof("a_unique_name_rot_x"));
151 EXPECT_TRUE(joint3->getDof(3) == skel->getDof("a_unique_name_pos_x"));
152
153 // Note: The following assumes the joint order in the Skeleton is:
154 // RevoluteJoint -> TranslationalJoint -> FreeJoint
155 EXPECT_TRUE(joint1->getDof(0) == skel->getDof(0));
156
157 EXPECT_TRUE(joint2->getDof(0) == skel->getDof(1));
158 EXPECT_TRUE(joint2->getDof(1) == skel->getDof(2));
159 EXPECT_TRUE(joint2->getDof(2) == skel->getDof(3));
160
161 EXPECT_TRUE(joint3->getDof(0) == skel->getDof(4));
162 EXPECT_TRUE(joint3->getDof(1) == skel->getDof(5));
163 EXPECT_TRUE(joint3->getDof(2) == skel->getDof(6));
164 EXPECT_TRUE(joint3->getDof(3) == skel->getDof(7));
165 EXPECT_TRUE(joint3->getDof(4) == skel->getDof(8));
166 EXPECT_TRUE(joint3->getDof(5) == skel->getDof(9));
167
168 // Test whether the return of getIndexInSkeleton() and the index of the
169 // corresponding DegreeOfFreedom in the Skeleton are same
170 for (std::size_t i = 0; i < skel->getNumDofs(); ++i)
171 EXPECT_TRUE(skel->getDof(i)->getIndexInSkeleton() == i);
172
173 // Test whether all the joint names are still unique
174 EXPECT_FALSE(joint1->getName() == joint2->getName());
175 EXPECT_FALSE(joint2->getName() == joint3->getName());
176 EXPECT_FALSE(joint3->getName() == joint1->getName());
177
178 // Make sure that the Skeleton gives back nullptr for non existent names
179 EXPECT_TRUE(skel->getBodyNode("nonexistent_name") == nullptr);
180 EXPECT_TRUE(skel->getJoint("nonexistent_name") == nullptr);
181 EXPECT_TRUE(skel->getSoftBodyNode("nonexistent_name") == nullptr);
182
183 // Test Node Names
184 EndEffector* ee1 = body1->createEndEffector("ee");
185 EndEffector* ee2 = body1->createEndEffector("ee");
186
187 EXPECT_TRUE(skel->getEndEffector("ee") == ee1);
188 EXPECT_TRUE(skel->getEndEffector("ee(1)") == ee2);
189 }
190
191 //==============================================================================
TEST(NameManagement,SetPattern)192 TEST(NameManagement, SetPattern)
193 {
194 dart::common::NameManager<std::shared_ptr<Entity> > test_mgr("test", "name");
195
196 std::shared_ptr<Entity> entity0(new SimpleFrame(Frame::World(), "name"));
197 std::shared_ptr<Entity> entity1(new SimpleFrame(Frame::World(), "name"));
198 std::shared_ptr<Entity> entity2(new SimpleFrame(Frame::World(), "name"));
199
200 test_mgr.setPattern("%s(%d)");
201
202 test_mgr.issueNewNameAndAdd(entity0->getName(), entity0);
203 test_mgr.issueNewNameAndAdd(entity1->getName(), entity1);
204 test_mgr.issueNewNameAndAdd(entity2->getName(), entity2);
205
206 EXPECT_TRUE(test_mgr.getObject("name") == entity0);
207 EXPECT_TRUE(test_mgr.getObject("name(1)") == entity1);
208 EXPECT_TRUE(test_mgr.getObject("name(2)") == entity2);
209
210 test_mgr.clear();
211
212 entity0->setName("Entity");
213 entity1->setName("Entity");
214 entity2->setName("Entity");
215
216 test_mgr.setPattern("(%d)-%s");
217 test_mgr.issueNewNameAndAdd(entity0->getName(), entity0);
218 test_mgr.issueNewNameAndAdd(entity1->getName(), entity1);
219 test_mgr.issueNewNameAndAdd(entity2->getName(), entity2);
220
221 EXPECT_TRUE(test_mgr.getObject("Entity") == entity0);
222 EXPECT_TRUE(test_mgr.getObject("(1)-Entity") == entity1);
223 EXPECT_TRUE(test_mgr.getObject("(2)-Entity") == entity2);
224 }
225
226 //==============================================================================
TEST(NameManagement,Regression554)227 TEST(NameManagement, Regression554)
228 {
229 dart::common::NameManager<std::shared_ptr<int> > test_mgr("test", "name");
230
231 std::shared_ptr<int> int0(new int(0));
232 std::shared_ptr<int> int1(new int(1));
233 std::shared_ptr<int> int2(new int(2));
234 std::shared_ptr<int> int_another0(new int(0));
235
236 test_mgr.issueNewNameAndAdd(std::to_string(*int0), int0);
237 test_mgr.issueNewNameAndAdd(std::to_string(*int1), int1);
238 test_mgr.issueNewNameAndAdd(std::to_string(*int2), int2);
239
240 EXPECT_TRUE(test_mgr.getObject("0") == int0);
241 EXPECT_TRUE(test_mgr.getObject("1") == int1);
242 EXPECT_TRUE(test_mgr.getObject("2") == int2);
243
244 bool res1 = test_mgr.addName(std::to_string(*int_another0), int_another0);
245 EXPECT_FALSE(res1);
246
247 test_mgr.removeEntries(std::to_string(*int_another0), int_another0);
248 bool res2 = test_mgr.addName(std::to_string(*int_another0), int_another0);
249 EXPECT_TRUE(res2);
250
251 EXPECT_TRUE(test_mgr.getObject("0") == int_another0);
252 EXPECT_TRUE(test_mgr.getObject("1") == int1);
253 EXPECT_TRUE(test_mgr.getObject("2") == int2);
254 }
255
256 //==============================================================================
TEST(NameManagement,WorldSkeletons)257 TEST(NameManagement, WorldSkeletons)
258 {
259 dart::simulation::WorldPtr world1(new dart::simulation::World);
260 world1->setName("world1");
261
262 dart::dynamics::SkeletonPtr skel0 = dart::dynamics::Skeleton::create();
263 dart::dynamics::SkeletonPtr skel1 = dart::dynamics::Skeleton::create();
264 dart::dynamics::SkeletonPtr skel2 = dart::dynamics::Skeleton::create();
265
266 world1->addSkeleton(skel0);
267 world1->addSkeleton(skel1);
268 world1->addSkeleton(skel2);
269
270 EXPECT_TRUE(skel0->getName() == "Skeleton");
271 EXPECT_TRUE(skel1->getName() == "Skeleton(1)");
272 EXPECT_TRUE(skel2->getName() == "Skeleton(2)");
273
274 skel1->setName("Skeleton");
275 EXPECT_TRUE(skel1->getName() == "Skeleton(1)");
276 skel1->setName("OtherName");
277 EXPECT_TRUE(skel1->getName() == "OtherName");
278
279 skel2->setName("Skeleton");
280 EXPECT_TRUE(skel2->getName() == "Skeleton(1)");
281 skel2->setName("OtherName");
282 EXPECT_TRUE(skel2->getName() == "OtherName(1)");
283
284 EXPECT_TRUE(skel0 == world1->getSkeleton(skel0->getName()));
285 EXPECT_TRUE(skel1 == world1->getSkeleton(skel1->getName()));
286 EXPECT_TRUE(skel2 == world1->getSkeleton(skel2->getName()));
287
288 dart::simulation::WorldPtr world2(new dart::simulation::World);
289 world2->setName("world2");
290
291 dart::dynamics::SkeletonPtr skel3
292 = dart::dynamics::Skeleton::create("OtherName");
293 world2->addSkeleton(skel3);
294 world2->addSkeleton(skel2);
295 world2->addSkeleton(skel1);
296
297 EXPECT_TRUE(skel3->getName() == "OtherName");
298 EXPECT_TRUE(skel1->getName() == "OtherName(2)");
299 EXPECT_TRUE(skel2->getName() == "OtherName(1)");
300
301 skel3->setName("Skeleton(1)");
302 skel1->setName("Skeleton");
303
304 EXPECT_TRUE(skel3->getName() == "Skeleton(1)");
305 EXPECT_TRUE(skel1->getName() == "Skeleton(1)(1)");
306
307 EXPECT_TRUE(skel0 == world1->getSkeleton(skel0->getName()));
308 EXPECT_TRUE(skel1 == world1->getSkeleton(skel1->getName()));
309 EXPECT_TRUE(skel2 == world1->getSkeleton(skel2->getName()));
310
311 EXPECT_TRUE(skel3 == world2->getSkeleton(skel3->getName()));
312 EXPECT_TRUE(skel1 == world2->getSkeleton(skel1->getName()));
313 EXPECT_TRUE(skel2 == world2->getSkeleton(skel2->getName()));
314
315 world2->removeSkeleton(skel1);
316
317 skel1->setName("Skeleton");
318 EXPECT_TRUE(skel1->getName() == "Skeleton(1)");
319 }
320
321 //==============================================================================
TEST(NameManagement,WorldSimpleFrames)322 TEST(NameManagement, WorldSimpleFrames)
323 {
324 dart::simulation::WorldPtr world1(new dart::simulation::World);
325
326 dart::dynamics::SimpleFramePtr frame0(
327 new dart::dynamics::SimpleFrame(Frame::World(), "Frame"));
328 dart::dynamics::SimpleFramePtr frame1(
329 new dart::dynamics::SimpleFrame(Frame::World(), "Frame"));
330 dart::dynamics::SimpleFramePtr frame2(
331 new dart::dynamics::SimpleFrame(Frame::World(), "Frame"));
332
333 world1->addSimpleFrame(frame0);
334 world1->addSimpleFrame(frame1);
335 world1->addSimpleFrame(frame2);
336
337 EXPECT_TRUE(frame0->getName() == "Frame");
338 EXPECT_TRUE(frame1->getName() == "Frame(1)");
339 EXPECT_TRUE(frame2->getName() == "Frame(2)");
340
341 frame1->setName("Frame");
342 EXPECT_TRUE(frame1->getName() == "Frame(1)");
343 frame1->setName("OtherName");
344 EXPECT_TRUE(frame1->getName() == "OtherName");
345
346 frame2->setName("Frame");
347 EXPECT_TRUE(frame2->getName() == "Frame(1)");
348 frame2->setName("OtherName");
349 EXPECT_TRUE(frame2->getName() == "OtherName(1)");
350
351 EXPECT_TRUE(frame0 == world1->getSimpleFrame(frame0->getName()));
352 EXPECT_TRUE(frame1 == world1->getSimpleFrame(frame1->getName()));
353 EXPECT_TRUE(frame2 == world1->getSimpleFrame(frame2->getName()));
354
355 dart::simulation::WorldPtr world2(new dart::simulation::World);
356 world2->setName("world2");
357
358 dart::dynamics::SimpleFramePtr frame3(
359 new dart::dynamics::SimpleFrame(Frame::World(), "OtherName"));
360 world2->addSimpleFrame(frame3);
361 world2->addSimpleFrame(frame2);
362 world2->addSimpleFrame(frame1);
363
364 EXPECT_TRUE(frame3->getName() == "OtherName");
365 EXPECT_TRUE(frame1->getName() == "OtherName(2)");
366 EXPECT_TRUE(frame2->getName() == "OtherName(1)");
367
368 frame3->setName("Frame(1)");
369 frame1->setName("Frame");
370
371 EXPECT_TRUE(frame3->getName() == "Frame(1)");
372 EXPECT_TRUE(frame1->getName() == "Frame(1)(1)");
373
374 EXPECT_TRUE(frame0 == world1->getSimpleFrame(frame0->getName()));
375 EXPECT_TRUE(frame1 == world1->getSimpleFrame(frame1->getName()));
376 EXPECT_TRUE(frame2 == world1->getSimpleFrame(frame2->getName()));
377
378 EXPECT_TRUE(frame3 == world2->getSimpleFrame(frame3->getName()));
379 EXPECT_TRUE(frame1 == world2->getSimpleFrame(frame1->getName()));
380 EXPECT_TRUE(frame2 == world2->getSimpleFrame(frame2->getName()));
381
382 world2->removeSimpleFrame(frame1);
383
384 frame1->setName("Frame");
385 EXPECT_TRUE(frame1->getName() == "Frame(1)");
386 }
387
388 //==============================================================================
TEST(NameManagement,JointDegreeOfFreedom)389 TEST(NameManagement, JointDegreeOfFreedom)
390 {
391 SkeletonPtr subtree = Skeleton::create("subtree");
392 EulerJoint::Properties jointProperties;
393 jointProperties.mName = "j_bicep_right";
394 JointPtr joint
395 = subtree
396 ->createJointAndBodyNodePair<EulerJoint>(nullptr, jointProperties)
397 .first;
398 std::string newName = "j_bicep_right_inverse";
399
400 EXPECT_TRUE(joint->getDof(0)->getName() == "j_bicep_right_x");
401 EXPECT_TRUE(joint->getDof(1)->getName() == "j_bicep_right_y");
402 EXPECT_TRUE(joint->getDof(2)->getName() == "j_bicep_right_z");
403
404 joint->setName(newName, false);
405
406 EXPECT_TRUE(joint->getDof(0)->getName() == "j_bicep_right_x");
407 EXPECT_TRUE(joint->getDof(1)->getName() == "j_bicep_right_y");
408 EXPECT_TRUE(joint->getDof(2)->getName() == "j_bicep_right_z");
409
410 joint->setName(newName, true);
411
412 EXPECT_TRUE(joint->getDof(0)->getName() == "j_bicep_right_inverse_x");
413 EXPECT_TRUE(joint->getDof(1)->getName() == "j_bicep_right_inverse_y");
414 EXPECT_TRUE(joint->getDof(2)->getName() == "j_bicep_right_inverse_z");
415
416 newName = "j_bicep_left";
417 joint->setName(newName, true);
418
419 EXPECT_TRUE(joint->getDof(0)->getName() == "j_bicep_left_x");
420 EXPECT_TRUE(joint->getDof(1)->getName() == "j_bicep_left_y");
421 EXPECT_TRUE(joint->getDof(2)->getName() == "j_bicep_left_z");
422 }
423