Debian Codegraph @

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