Debian Codegraph @

unittests/integration/test_CollisionGroups.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 <dart/dynamics/SphereShape.hpp>
36 #include <gtest/gtest.h>
37
38 #include "dart/constraint/ConstraintSolver.hpp"
39 #include "dart/dynamics/BoxShape.hpp"
40 #include "dart/dynamics/FreeJoint.hpp"
41 #include "dart/dynamics/Skeleton.hpp"
42 #include "dart/simulation/World.hpp"
43
44 class CollisionGroupsTest : public testing::Test,
45 public testing::WithParamInterface<const char*>
46 {
47 };
48
49 TEST_P(CollisionGroupsTest, SkeletonSubscription)
50 {
51 if (!dart::collision::CollisionDetector::getFactory()->canCreate(GetParam()))
52 {
53 std::cout << "Skipping test for [" << GetParam() << "], because it is not "
54 << "available" << std::endl;
55 return;
56 }
57 else
58 {
59 std::cout << "Running CollisionGroups test for [" << GetParam() << "]"
60 << std::endl;
61 }
62
63 // Note: When skeletons are added to a world, the constraint solver will
64 // subscribe to them.
65 dart::simulation::WorldPtr world = dart::simulation::World::create();
66 world->getConstraintSolver()->setCollisionDetector(
67 dart::collision::CollisionDetector::getFactory()->create(GetParam()));
68
69 dart::dynamics::SkeletonPtr skel_A = dart::dynamics::Skeleton::create("A");
70 dart::dynamics::SkeletonPtr skel_B = dart::dynamics::Skeleton::create("B");
71
72 world->addSkeleton(skel_A);
73 world->addSkeleton(skel_B);
74
75 // There should be no collisions because there are no shapes in the world.
76 EXPECT_FALSE(world->checkCollision());
77
78 // We will now add some BodyNodes and collision shapes *after* the Skeletons
79 // have been added to the world, to see that the collision geometries get
80 // updated automatically.
81 Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
82 tf.translation() = (1.0 + 0.25) * Eigen::Vector3d::UnitX();
83
84 auto boxShape = std::make_shared<dart::dynamics::BoxShape>(
85 Eigen::Vector3d::Constant(1.0));
86 auto pair = skel_A->createJointAndBodyNodePair<dart::dynamics::FreeJoint>();
87 pair.first->setTransform(tf);
88 auto sn1 = pair.second->createShapeNodeWith<dart::dynamics::CollisionAspect>(
89 boxShape);
90
91 tf.translation() = (1.0 - 0.25) * Eigen::Vector3d::UnitX();
92
93 pair = skel_B->createJointAndBodyNodePair<dart::dynamics::FreeJoint>();
94 pair.first->setTransform(tf);
95 auto sn2 = pair.second->createShapeNodeWith<dart::dynamics::CollisionAspect>(
96 boxShape);
97
98 EXPECT_TRUE(world->checkCollision());
99
100 // Now we'll change the properties of one the box shape so that there should
101 // no longer be any collisions.
102 boxShape->setSize(Eigen::Vector3d::Constant(0.2));
103 dart::collision::CollisionResult result2;
104 EXPECT_FALSE(world->checkCollision());
105
106 // Now we'll replace one of the boxes with a large one, so that a collision
107 // will occur again.
108 auto largeBox = std::make_shared<dart::dynamics::BoxShape>(
109 Eigen::Vector3d::Constant(0.95));
110 sn1->setShape(largeBox);
111 EXPECT_TRUE(world->checkCollision());
112
113 // After this, both shapes will have been replaced with new shape instances.
114 // If the shape map of the collision detector is empty upon destruction, then
115 // the internal collision shapes are being managed correctly. Otherwise, if
116 // they are not being managed correctly, then an assertion will fail when
117 // testing in debug mode.
118 auto sphereShape = std::make_shared<dart::dynamics::SphereShape>(0.01);
119 sn2->setShape(sphereShape);
120 EXPECT_FALSE(world->checkCollision());
121
122 // Resize the sphere so that there is a collision again.
123 sphereShape->setRadius(0.5);
124 EXPECT_TRUE(world->checkCollision());
125
126 // Remove the shape node so that there should no longer be a collision
127 sn2->remove();
128 EXPECT_FALSE(world->checkCollision());
129
130 // Create a new shape node so that there should be a collision again
131 pair.second->createShapeNodeWith<dart::dynamics::CollisionAspect>(
132 sphereShape);
133 EXPECT_TRUE(world->checkCollision());
134
135 // Remove the BodyNode so that there should no longer be a collision
136 pair.second->remove();
137 EXPECT_FALSE(world->checkCollision());
138
139 // Add a new BodyNode and Shape Node so that there is a collision again
140 pair = skel_B->createJointAndBodyNodePair<dart::dynamics::FreeJoint>();
141 pair.first->setTransform(tf);
142 pair.second->createShapeNodeWith<dart::dynamics::CollisionAspect>(
143 sphereShape);
144 EXPECT_TRUE(world->checkCollision());
145
146 // Remove a skeleton so that there are no longer collisions
147 world->removeSkeleton(skel_B);
148 EXPECT_FALSE(world->checkCollision());
149 }
150
151 TEST_P(CollisionGroupsTest, BodyNodeSubscription)
152 {
153 if (!dart::collision::CollisionDetector::getFactory()->canCreate(GetParam()))
154 {
155 std::cout << "Skipping test for [" << GetParam() << "], because it is not "
156 << "available" << std::endl;
157 return;
158 }
159 else
160 {
161 std::cout << "Running CollisionGroups test for [" << GetParam() << "]"
162 << std::endl;
163 }
164
165 auto cd
166 = dart::collision::CollisionDetector::getFactory()->create(GetParam());
167
168 auto group = cd->createCollisionGroup();
169
170 auto skel1 = dart::dynamics::Skeleton::create("skel1");
171 auto skel2 = dart::dynamics::Skeleton::create("skel2");
172
173 auto pair_1a = skel1->createJointAndBodyNodePair<dart::dynamics::FreeJoint>();
174 auto pair_2a = skel2->createJointAndBodyNodePair<dart::dynamics::FreeJoint>();
175
176 group->subscribeTo(pair_1a.second, pair_2a.second);
177 // The BodyNodes currently have no collision geometries, so we expect there to
178 // be no collisions.
179 EXPECT_FALSE(group->collide());
180
181 Eigen::Isometry3d tf{Eigen::Translation3d(0.5, 0.0, 0.0)};
182 pair_1a.first->setTransform(tf);
183 pair_1a.first->setName("1a");
184 pair_1a.second->setName("1a");
185
186 tf.translation()[0] = -0.5;
187 pair_2a.first->setTransform(tf);
188 pair_2a.first->setName("2a");
189 pair_2a.second->setName("2a");
190
191 auto sphere = std::make_shared<dart::dynamics::SphereShape>(0.75);
192
193 auto sn_1a
194 = pair_1a.second->createShapeNodeWith<dart::dynamics::CollisionAspect>(
195 sphere);
196
197 auto sn_2a
198 = pair_2a.second->createShapeNodeWith<dart::dynamics::CollisionAspect>(
199 sphere);
200
201 // The BodyNodes have been given overlapping shapes, so now we expect the
202 // collision information to automatically update and identify a collision.
203 EXPECT_TRUE(group->collide());
204
205 tf.translation()[0] = 1.0;
206 pair_1a.first->setTransform(tf);
207 tf.translation()[0] = -1.0;
208 pair_2a.first->setTransform(tf);
209
210 // The collision geometries have been moved far enough from each other that
211 // there should no longer be any collisions
212 EXPECT_FALSE(group->collide());
213
214 sphere->setRadius(1.1);
215 // The shapes of the collision geometries have been expanded enough that they
216 // should collide again.
217 EXPECT_TRUE(group->collide());
218
219 auto box = std::make_shared<dart::dynamics::BoxShape>(
220 Eigen::Vector3d::Constant(0.5));
221 sn_2a->setShape(box);
222 // Now a shape has been replaced with a smaller one, so there should no longer
223 // be a collision.
224 EXPECT_FALSE(group->collide());
225
226 auto pair_1b = skel1->createJointAndBodyNodePair<dart::dynamics::FreeJoint>();
227 auto pair_2b = skel2->createJointAndBodyNodePair<dart::dynamics::FreeJoint>();
228
229 tf.translation() = 0.5 * Eigen::Vector3d::UnitX();
230 pair_1b.first->setTransform(tf);
231 pair_1b.first->setName("1b");
232 pair_1b.second->setName("1b");
233
234 tf.translation()[0] = -0.5;
235 pair_2b.first->setTransform(tf);
236 pair_2b.first->setName("2b");
237 pair_2b.second->setName("2b");
238
239 pair_1b.second->createShapeNodeWith<dart::dynamics::CollisionAspect>(sphere);
240 pair_2b.second->createShapeNodeWith<dart::dynamics::CollisionAspect>(sphere);
241
242 // The collision group should not be tracking the new BodyNodes, so there
243 // should still be no collisions
244 EXPECT_FALSE(group->collide());
245
246 // Change the size of the box so that there are overlapping collision
247 // geometries. The collision information should update automatically, and a
248 // collision should be detected.
249 box->setSize(Eigen::Vector3d::Constant(2.0));
250 EXPECT_TRUE(group->collide());
251
252 sn_1a->remove();
253 // A shape node has been removed, so there should no longer be a collision
254 EXPECT_FALSE(group->collide());
255
256 pair_1a.second->createShapeNodeWith<dart::dynamics::CollisionAspect>(sphere);
257 // A new shape node has been added with the same geometry as the one that was
258 // removed, so there should be a collision again
259 EXPECT_TRUE(group->collide());
260
261 // Remove one of the BodyNodes so that there should no longer be a collision.
262 group->removeShapeFramesOf(pair_1a.second);
263 EXPECT_FALSE(group->collide());
264 }
265
266 TEST_P(CollisionGroupsTest, RemovedSkeletonSubscription)
267 {
268 if (!dart::collision::CollisionDetector::getFactory()->canCreate(GetParam()))
269 {
270 std::cout << "Skipping test for [" << GetParam() << "], because it is not "
271 << "available" << std::endl;
272 return;
273 }
274 else
275 {
276 std::cout << "Running CollisionGroups test for [" << GetParam() << "]"
277 << std::endl;
278 }
279 // Note: When skeletons are added to a world, the constraint solver will
280 // subscribe to them.
281 dart::simulation::WorldPtr world = dart::simulation::World::create();
282 auto cd
283 = dart::collision::CollisionDetector::getFactory()->create(GetParam());
284
285 world->getConstraintSolver()->setCollisionDetector(cd);
286
287 dart::dynamics::SkeletonPtr skel_A = dart::dynamics::Skeleton::create("A");
288 dart::dynamics::SkeletonPtr skel_B = dart::dynamics::Skeleton::create("B");
289
290 auto group = world->getConstraintSolver()->getCollisionGroup();
291
292 // Check that there are no subscribtions before adding the skeletons to the
293 // world
294 EXPECT_FALSE(group->isSubscribedTo(skel_A.get()));
295 EXPECT_FALSE(group->isSubscribedTo(skel_B.get()));
296
297 world->addSkeleton(skel_A);
298 world->addSkeleton(skel_B);
299
300 // Check that there are subscribtions after adding the skeletons to the
301 // world
302 EXPECT_TRUE(group->isSubscribedTo(skel_A.get()));
303 EXPECT_TRUE(group->isSubscribedTo(skel_B.get()));
304
305 // Add a shape to one of the skeletons to test that removal works for
306 // skeletons with and without shapes
307 auto boxShape = std::make_shared<dart::dynamics::BoxShape>(
308 Eigen::Vector3d::Constant(1.0));
309
310 auto pair = skel_B->createJointAndBodyNodePair<dart::dynamics::FreeJoint>();
311 auto sn = pair.second->createShapeNodeWith<dart::dynamics::CollisionAspect>(
312 boxShape);
313
314 // Needed to update subscribtions
315 world->step();
316
317 EXPECT_TRUE(group->hasShapeFrame(sn));
318 const auto* skel_A_ptr = skel_A.get();
319 const auto* skel_B_ptr = skel_B.get();
320 // Check that there are no subscribtions after removing the skeletons from the
321 // world
322 world->removeSkeleton(skel_A);
323 world->removeSkeleton(skel_B);
324
325 world->step();
326
327 EXPECT_FALSE(group->hasShapeFrame(sn));
328 EXPECT_FALSE(group->isSubscribedTo(skel_A_ptr));
329 EXPECT_FALSE(group->isSubscribedTo(skel_B_ptr));
330 }
331
332 INSTANTIATE_TEST_CASE_P(
333 CollisionEngine,
334 CollisionGroupsTest,
335 testing::Values("dart", "fcl", "bullet", "ode"));