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 "TestHelpers.hpp"
36
37 #include "dart/dynamics/BodyNode.hpp"
38 #include "dart/dynamics/RevoluteJoint.hpp"
39 #include "dart/dynamics/Skeleton.hpp"
40 #include "dart/math/Geometry.hpp"
41 #include "dart/simulation/World.hpp"
42
43 using namespace dart;
44 using namespace math;
45 using namespace dynamics;
46 using namespace simulation;
47
48 /******************************************************************************/
TEST(BUILDING,BASIC)49 TEST(BUILDING, BASIC)
50 {
51 // Skeletons
52 SkeletonPtr skel1 = Skeleton::create();
53
54 std::pair<RevoluteJoint*, BodyNode*> pair;
55 BodyNode *body1, *body2, *body3;
56 RevoluteJoint *joint1, *joint2, *joint3;
57
58 pair = skel1->createJointAndBodyNodePair<RevoluteJoint>(nullptr);
59 joint1 = pair.first;
60 body1 = pair.second;
61
62 pair = body1->createChildJointAndBodyNodePair<RevoluteJoint>();
63 joint2 = pair.first;
64 body2 = pair.second;
65
66 pair = body2->createChildJointAndBodyNodePair<RevoluteJoint>();
67 joint3 = pair.first;
68 body3 = pair.second;
69
70 // Joints
71 joint1->setTransformFromParentBodyNode(Eigen::Isometry3d::Identity());
72 joint1->setTransformFromChildBodyNode(Eigen::Isometry3d::Identity());
73 joint1->setAxis(Eigen::Vector3d(1.0, 0.0, 0.0));
74
75 joint2->setTransformFromParentBodyNode(Eigen::Isometry3d::Identity());
76 joint2->setTransformFromChildBodyNode(Eigen::Isometry3d::Identity());
77 joint2->setAxis(Eigen::Vector3d(1.0, 0.0, 0.0));
78
79 joint3->setTransformFromParentBodyNode(Eigen::Isometry3d::Identity());
80 joint3->setTransformFromChildBodyNode(Eigen::Isometry3d::Identity());
81 joint3->setAxis(Eigen::Vector3d(1.0, 0.0, 0.0));
82
83 // World
84 WorldPtr world = World::create();
85 world->addSkeleton(skel1);
86
87 //--------------------------------------------------------------------------
88 //
89 //--------------------------------------------------------------------------
90 EXPECT_TRUE(body1->getParentBodyNode() == nullptr);
91 EXPECT_TRUE(body1->getNumChildBodyNodes() == 1);
92 EXPECT_TRUE(body1->getChildBodyNode(0) == body2);
93
94 EXPECT_TRUE(body2->getParentBodyNode() == body1);
95 EXPECT_TRUE(body2->getNumChildBodyNodes() == 1);
96 EXPECT_TRUE(body2->getChildBodyNode(0) == body3);
97
98 EXPECT_TRUE(body3->getParentBodyNode() == body2);
99 EXPECT_TRUE(body3->getNumChildBodyNodes() == 0);
100 // EXPECT_TRUE(body3->getChildBodyNode(0) == nullptr);
101
102 EXPECT_TRUE(skel1->getNumBodyNodes() == 3);
103 EXPECT_TRUE(skel1->getNumDofs() == 3);
104
105 EXPECT_TRUE(world->getNumSkeletons() == 1);
106
107 int nSteps = 20;
108 for (int i = 0; i < nSteps; ++i)
109 world->step();
110 }
111