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