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 <TestHelpers.hpp>
34 #include <gtest/gtest.h>
35
36 #include <dart/dart.hpp>
37 #include <dart/utils/sdf/sdf.hpp>
38
39 // This test is adapted from @azeey's work here:
40 // https://github.com/ignitionrobotics/ign-physics/pull/31
TEST(Issue1445,Collision)41 TEST(Issue1445, Collision)
42 {
43 std::string model1Str = R"(
44 <sdf version="1.6">
45 <model name="M1">
46 <pose>0 0 0.1 0 0 0</pose>
47 <link name="body">
48 <collision name="coll_box">
49 <geometry>
50 <box>
51 <size>0.2 0.2 0.2</size>
52 </box>
53 </geometry>
54 </collision>
55 </link>
56 </model>
57 </sdf>)";
58
59 std::string model2Str = R"(
60 <sdf version="1.6">
61 <model name="M2">
62 <pose>1 0 0.1 0 0 0</pose>
63 <link name="chassis">
64 <collision name="coll_sphere">
65 <geometry>
66 <sphere>
67 <radius>0.1</radius>
68 </sphere>
69 </geometry>
70 </collision>
71 </link>
72 </model>
73 </sdf>)";
74
75 auto world = std::make_shared<dart::simulation::World>();
76
77 auto ground = dart::dynamics::Skeleton::create("ground");
78 {
79 auto* bn = ground->createJointAndBodyNodePair<dart::dynamics::WeldJoint>()
80 .second;
81 bn->createShapeNodeWith<
82 dart::dynamics::CollisionAspect,
83 dart::dynamics::DynamicsAspect>(
84 std::make_shared<dart::dynamics::BoxShape>(
85 Eigen::Vector3d(10, 10, 0.2)));
86 auto sn = bn->getShapeNode(0);
87 ASSERT_TRUE(sn);
88 sn->setRelativeTranslation({0, 0, -0.1});
89 world->addSkeleton(ground);
90 }
91
92 auto model1 = dart::dynamics::Skeleton::create("M1");
93 {
94 auto pair = model1->createJointAndBodyNodePair<dart::dynamics::FreeJoint>();
95 auto* joint = pair.first;
96 auto* bn = pair.second;
97 joint->setName("joint1");
98 bn->setName("body1");
99
100 bn->createShapeNodeWith<
101 dart::dynamics::CollisionAspect,
102 dart::dynamics::DynamicsAspect>(
103 std::make_shared<dart::dynamics::BoxShape>(
104 Eigen::Vector3d(0.2, 0.2, 0.2)));
105
106 auto tf = Eigen::Isometry3d::Identity();
107 tf.translation()[2] = 0.1;
108 joint->setTransform(tf);
109
110 world->addSkeleton(model1);
111 }
112
113 world->step();
114
115 auto model2 = dart::dynamics::Skeleton::create("M2");
116 {
117 auto pair = model2->createJointAndBodyNodePair<dart::dynamics::FreeJoint>();
118 auto* joint = pair.first;
119 auto* bn = pair.second;
120 joint->setName("joint2");
121 bn->setName("body2");
122
123 bn->createShapeNodeWith<
124 dart::dynamics::CollisionAspect,
125 dart::dynamics::DynamicsAspect>(
126 std::make_shared<dart::dynamics::SphereShape>(0.1));
127
128 auto tf = Eigen::Isometry3d::Identity();
129 tf.translation() = Eigen::Vector3d(1.0, 0.0, 0.1);
130 joint->setTransform(tf);
131 }
132
133 auto* model1Body = model1->getRootBodyNode();
134 auto* model2Body = model2->getRootBodyNode();
135
136 const auto poseParent = model1Body->getTransform();
137 const auto poseChild = model2Body->getTransform();
138
139 // Commenting out the following `step` call makes this test fail
140 // world->Step(output, state, input);
141 auto fixedJoint = model2Body->moveTo<dart::dynamics::WeldJoint>(model1Body);
142
143 // Pose of child relative to parent
144 auto poseParentChild = poseParent.inverse() * poseChild;
145
146 // We let the joint be at the origin of the child link.
147 fixedJoint->setTransformFromParentBodyNode(poseParentChild);
148
149 const std::size_t numSteps = 100;
150
151 for (std::size_t i = 0; i < numSteps; ++i)
152 world->step();
153
154 // Expect both bodies to hit the ground and stop
155 EXPECT_NEAR(0.0, model1Body->getLinearVelocity().z(), 1e-3);
156 EXPECT_NEAR(0.0, model2Body->getLinearVelocity().z(), 1e-3);
157
158 auto temp1 = dart::dynamics::Skeleton::create("temp1");
159 world->addSkeleton(temp1);
160 model2Body->moveTo<dart::dynamics::FreeJoint>(temp1, nullptr);
161
162 for (std::size_t i = 0; i < numSteps; ++i)
163 world->step();
164
165 // Expect both bodies to remain in contact with the ground with zero velocity.
166 EXPECT_NEAR(0.0, model1Body->getLinearVelocity().z(), 1e-3);
167 EXPECT_NEAR(0.0, model2Body->getLinearVelocity().z(), 1e-3);
168
169 auto* groundBody = ground->getRootBodyNode();
170 auto temp2 = groundBody->remove();
171
172 for (std::size_t i = 0; i < numSteps; ++i)
173 world->step();
174
175 // Expect both bodies to be falling after the BodyNode ofthe the ground is
176 // removed
177 EXPECT_LE(model1Body->getLinearVelocity().z(), -1e-2);
178 EXPECT_LE(model2Body->getLinearVelocity().z(), -1e-2);
179 }
180