/* * Copyright (c) 2011-2022, The DART development contributors * All rights reserved. * * The list of contributors can be found at: * https://github.com/dartsim/dart/blob/master/LICENSE * * This file is provided under the following "BSD-style" License: * Redistribution and use in source and binary forms, with or * without modification, are permitted provided that the following * conditions are met: * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include "dart/constraint/ConstraintSolver.hpp" #include "dart/dynamics/BoxShape.hpp" #include "dart/dynamics/FreeJoint.hpp" #include "dart/dynamics/Skeleton.hpp" #include "dart/simulation/World.hpp" class CollisionGroupsTest : public testing::Test, public testing::WithParamInterface { }; TEST_P(CollisionGroupsTest, SkeletonSubscription) { if (!dart::collision::CollisionDetector::getFactory()->canCreate(GetParam())) { std::cout << "Skipping test for [" << GetParam() << "], because it is not " << "available" << std::endl; return; } else { std::cout << "Running CollisionGroups test for [" << GetParam() << "]" << std::endl; } // Note: When skeletons are added to a world, the constraint solver will // subscribe to them. dart::simulation::WorldPtr world = dart::simulation::World::create(); world->getConstraintSolver()->setCollisionDetector( dart::collision::CollisionDetector::getFactory()->create(GetParam())); dart::dynamics::SkeletonPtr skel_A = dart::dynamics::Skeleton::create("A"); dart::dynamics::SkeletonPtr skel_B = dart::dynamics::Skeleton::create("B"); world->addSkeleton(skel_A); world->addSkeleton(skel_B); // There should be no collisions because there are no shapes in the world. EXPECT_FALSE(world->checkCollision()); // We will now add some BodyNodes and collision shapes *after* the Skeletons // have been added to the world, to see that the collision geometries get // updated automatically. Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); tf.translation() = (1.0 + 0.25) * Eigen::Vector3d::UnitX(); auto boxShape = std::make_shared( Eigen::Vector3d::Constant(1.0)); auto pair = skel_A->createJointAndBodyNodePair(); pair.first->setTransform(tf); auto sn1 = pair.second->createShapeNodeWith( boxShape); tf.translation() = (1.0 - 0.25) * Eigen::Vector3d::UnitX(); pair = skel_B->createJointAndBodyNodePair(); pair.first->setTransform(tf); auto sn2 = pair.second->createShapeNodeWith( boxShape); EXPECT_TRUE(world->checkCollision()); // Now we'll change the properties of one the box shape so that there should // no longer be any collisions. boxShape->setSize(Eigen::Vector3d::Constant(0.2)); dart::collision::CollisionResult result2; EXPECT_FALSE(world->checkCollision()); // Now we'll replace one of the boxes with a large one, so that a collision // will occur again. auto largeBox = std::make_shared( Eigen::Vector3d::Constant(0.95)); sn1->setShape(largeBox); EXPECT_TRUE(world->checkCollision()); // After this, both shapes will have been replaced with new shape instances. // If the shape map of the collision detector is empty upon destruction, then // the internal collision shapes are being managed correctly. Otherwise, if // they are not being managed correctly, then an assertion will fail when // testing in debug mode. auto sphereShape = std::make_shared(0.01); sn2->setShape(sphereShape); EXPECT_FALSE(world->checkCollision()); // Resize the sphere so that there is a collision again. sphereShape->setRadius(0.5); EXPECT_TRUE(world->checkCollision()); // Remove the shape node so that there should no longer be a collision sn2->remove(); EXPECT_FALSE(world->checkCollision()); // Create a new shape node so that there should be a collision again pair.second->createShapeNodeWith( sphereShape); EXPECT_TRUE(world->checkCollision()); // Remove the BodyNode so that there should no longer be a collision pair.second->remove(); EXPECT_FALSE(world->checkCollision()); // Add a new BodyNode and Shape Node so that there is a collision again pair = skel_B->createJointAndBodyNodePair(); pair.first->setTransform(tf); pair.second->createShapeNodeWith( sphereShape); EXPECT_TRUE(world->checkCollision()); // Remove a skeleton so that there are no longer collisions world->removeSkeleton(skel_B); EXPECT_FALSE(world->checkCollision()); } TEST_P(CollisionGroupsTest, BodyNodeSubscription) { if (!dart::collision::CollisionDetector::getFactory()->canCreate(GetParam())) { std::cout << "Skipping test for [" << GetParam() << "], because it is not " << "available" << std::endl; return; } else { std::cout << "Running CollisionGroups test for [" << GetParam() << "]" << std::endl; } auto cd = dart::collision::CollisionDetector::getFactory()->create(GetParam()); auto group = cd->createCollisionGroup(); auto skel1 = dart::dynamics::Skeleton::create("skel1"); auto skel2 = dart::dynamics::Skeleton::create("skel2"); auto pair_1a = skel1->createJointAndBodyNodePair(); auto pair_2a = skel2->createJointAndBodyNodePair(); group->subscribeTo(pair_1a.second, pair_2a.second); // The BodyNodes currently have no collision geometries, so we expect there to // be no collisions. EXPECT_FALSE(group->collide()); Eigen::Isometry3d tf{Eigen::Translation3d(0.5, 0.0, 0.0)}; pair_1a.first->setTransform(tf); pair_1a.first->setName("1a"); pair_1a.second->setName("1a"); tf.translation()[0] = -0.5; pair_2a.first->setTransform(tf); pair_2a.first->setName("2a"); pair_2a.second->setName("2a"); auto sphere = std::make_shared(0.75); auto sn_1a = pair_1a.second->createShapeNodeWith( sphere); auto sn_2a = pair_2a.second->createShapeNodeWith( sphere); // The BodyNodes have been given overlapping shapes, so now we expect the // collision information to automatically update and identify a collision. EXPECT_TRUE(group->collide()); tf.translation()[0] = 1.0; pair_1a.first->setTransform(tf); tf.translation()[0] = -1.0; pair_2a.first->setTransform(tf); // The collision geometries have been moved far enough from each other that // there should no longer be any collisions EXPECT_FALSE(group->collide()); sphere->setRadius(1.1); // The shapes of the collision geometries have been expanded enough that they // should collide again. EXPECT_TRUE(group->collide()); auto box = std::make_shared( Eigen::Vector3d::Constant(0.5)); sn_2a->setShape(box); // Now a shape has been replaced with a smaller one, so there should no longer // be a collision. EXPECT_FALSE(group->collide()); auto pair_1b = skel1->createJointAndBodyNodePair(); auto pair_2b = skel2->createJointAndBodyNodePair(); tf.translation() = 0.5 * Eigen::Vector3d::UnitX(); pair_1b.first->setTransform(tf); pair_1b.first->setName("1b"); pair_1b.second->setName("1b"); tf.translation()[0] = -0.5; pair_2b.first->setTransform(tf); pair_2b.first->setName("2b"); pair_2b.second->setName("2b"); pair_1b.second->createShapeNodeWith(sphere); pair_2b.second->createShapeNodeWith(sphere); // The collision group should not be tracking the new BodyNodes, so there // should still be no collisions EXPECT_FALSE(group->collide()); // Change the size of the box so that there are overlapping collision // geometries. The collision information should update automatically, and a // collision should be detected. box->setSize(Eigen::Vector3d::Constant(2.0)); EXPECT_TRUE(group->collide()); sn_1a->remove(); // A shape node has been removed, so there should no longer be a collision EXPECT_FALSE(group->collide()); pair_1a.second->createShapeNodeWith(sphere); // A new shape node has been added with the same geometry as the one that was // removed, so there should be a collision again EXPECT_TRUE(group->collide()); // Remove one of the BodyNodes so that there should no longer be a collision. group->removeShapeFramesOf(pair_1a.second); EXPECT_FALSE(group->collide()); } TEST_P(CollisionGroupsTest, RemovedSkeletonSubscription) { if (!dart::collision::CollisionDetector::getFactory()->canCreate(GetParam())) { std::cout << "Skipping test for [" << GetParam() << "], because it is not " << "available" << std::endl; return; } else { std::cout << "Running CollisionGroups test for [" << GetParam() << "]" << std::endl; } // Note: When skeletons are added to a world, the constraint solver will // subscribe to them. dart::simulation::WorldPtr world = dart::simulation::World::create(); auto cd = dart::collision::CollisionDetector::getFactory()->create(GetParam()); world->getConstraintSolver()->setCollisionDetector(cd); dart::dynamics::SkeletonPtr skel_A = dart::dynamics::Skeleton::create("A"); dart::dynamics::SkeletonPtr skel_B = dart::dynamics::Skeleton::create("B"); auto group = world->getConstraintSolver()->getCollisionGroup(); // Check that there are no subscribtions before adding the skeletons to the // world EXPECT_FALSE(group->isSubscribedTo(skel_A.get())); EXPECT_FALSE(group->isSubscribedTo(skel_B.get())); world->addSkeleton(skel_A); world->addSkeleton(skel_B); // Check that there are subscribtions after adding the skeletons to the // world EXPECT_TRUE(group->isSubscribedTo(skel_A.get())); EXPECT_TRUE(group->isSubscribedTo(skel_B.get())); // Add a shape to one of the skeletons to test that removal works for // skeletons with and without shapes auto boxShape = std::make_shared( Eigen::Vector3d::Constant(1.0)); auto pair = skel_B->createJointAndBodyNodePair(); auto sn = pair.second->createShapeNodeWith( boxShape); // Needed to update subscribtions world->step(); EXPECT_TRUE(group->hasShapeFrame(sn)); const auto* skel_A_ptr = skel_A.get(); const auto* skel_B_ptr = skel_B.get(); // Check that there are no subscribtions after removing the skeletons from the // world world->removeSkeleton(skel_A); world->removeSkeleton(skel_B); world->step(); EXPECT_FALSE(group->hasShapeFrame(sn)); EXPECT_FALSE(group->isSubscribedTo(skel_A_ptr)); EXPECT_FALSE(group->isSubscribedTo(skel_B_ptr)); } INSTANTIATE_TEST_CASE_P( CollisionEngine, CollisionGroupsTest, testing::Values("dart", "fcl", "bullet", "ode"));