Description: Fix issue with removing skeletons without shapes
Origin: https://github.com/dartsim/dart/pull/1625
Forwarded: https://github.com/dartsim/dart/pull/1625
Applied-Upstream: https://github.com/dartsim/dart/pull/1625
Last-Update: 2022-01-10
---
diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp
index 9a3d87b6322b..73fea0a4eca9 100644
--- a/dart/collision/CollisionGroup.cpp
+++ b/dart/collision/CollisionGroup.cpp
@@ -142,6 +142,18 @@ void CollisionGroup::removeShapeFramesOf()
   // Do nothing
 }
 
+//==============================================================================
+void CollisionGroup::unsubscribeFrom()
+{
+  // Do nothing
+}
+
+//==============================================================================
+bool CollisionGroup::isSubscribedTo()
+{
+  return true;
+}
+
 //==============================================================================
 void CollisionGroup::removeAllShapeFrames()
 {
diff --git a/dart/collision/CollisionGroup.hpp b/dart/collision/CollisionGroup.hpp
index 79da55f6ac3f..61e964bbf2cf 100644
--- a/dart/collision/CollisionGroup.hpp
+++ b/dart/collision/CollisionGroup.hpp
@@ -214,6 +214,24 @@ class CollisionGroup
   void unsubscribeFrom(
       const dynamics::Skeleton* skeleton, const Others*... others);
 
+  /// Do nothing. This function is for terminating the recursive variadic
+  /// template.
+  void unsubscribeFrom();
+
+  /// Check if this is subscribed to bodyNode and the other sources
+  template <typename... Others>
+  bool isSubscribedTo(
+      const dynamics::BodyNode* bodyNode, const Others*... others);
+
+  /// Check if this is subscribed to skeleton and the other sources
+  template <typename... Others>
+  bool isSubscribedTo(
+      const dynamics::Skeleton* skeleton, const Others*... others);
+
+  /// Return true. This function is for terminating the recursive variadic
+  /// template
+  bool isSubscribedTo();
+
   /// Return true if this CollisionGroup contains shapeFrame
   bool hasShapeFrame(const dynamics::ShapeFrame* shapeFrame) const;
 
diff --git a/dart/collision/detail/CollisionGroup.hpp b/dart/collision/detail/CollisionGroup.hpp
index 7f0355f5a269..e6e28f45c4f0 100644
--- a/dart/collision/detail/CollisionGroup.hpp
+++ b/dart/collision/detail/CollisionGroup.hpp
@@ -280,6 +280,24 @@ void CollisionGroup::unsubscribeFrom(
   unsubscribeFrom(others...);
 }
 
+//==============================================================================
+template <typename... Others>
+bool CollisionGroup::isSubscribedTo(
+    const dynamics::BodyNode* bodyNode, const Others*... others)
+{
+  auto it = mBodyNodeSources.find(bodyNode);
+  return (it != mBodyNodeSources.end()) && isSubscribedTo(others...);
+}
+
+//==============================================================================
+template <typename... Others>
+bool CollisionGroup::isSubscribedTo(
+    const dynamics::Skeleton* skeleton, const Others*... others)
+{
+  auto it = mSkeletonSources.find(skeleton);
+  return (it != mSkeletonSources.end()) && isSubscribedTo(others...);
+}
+
 } // namespace collision
 } // namespace dart
 
diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp
index 49c25a6c2ca8..fdc451896b70 100644
--- a/dart/constraint/ConstraintSolver.cpp
+++ b/dart/constraint/ConstraintSolver.cpp
@@ -142,7 +142,7 @@ void ConstraintSolver::removeSkeleton(const SkeletonPtr& skeleton)
            << "', which doesn't exist in the ConstraintSolver.\n";
   }
 
-  mCollisionGroup->removeShapeFramesOf(skeleton.get());
+  mCollisionGroup->unsubscribeFrom(skeleton.get());
   mSkeletons.erase(
       remove(mSkeletons.begin(), mSkeletons.end(), skeleton), mSkeletons.end());
   mConstrainedGroups.reserve(mSkeletons.size());
diff --git a/unittests/unit/test_CollisionGroups.cpp b/unittests/unit/test_CollisionGroups.cpp
index 6a4227fe59a5..855cb21d6c22 100644
--- a/unittests/unit/test_CollisionGroups.cpp
+++ b/unittests/unit/test_CollisionGroups.cpp
@@ -263,6 +263,72 @@ TEST_P(CollisionGroupsTest, BodyNodeSubscription)
   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<dart::dynamics::BoxShape>(
+      Eigen::Vector3d::Constant(1.0));
+
+  auto pair = skel_B->createJointAndBodyNodePair<dart::dynamics::FreeJoint>();
+  auto sn = pair.second->createShapeNodeWith<dart::dynamics::CollisionAspect>(
+      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,