Skip to content

Commit

Permalink
Add patches merged upstream
Browse files Browse the repository at this point in the history 
Upstream reviewed and merged the following patches that are important
for the robotics community:
 * Fix grouping of contact constraints
   dartsim/dart#1624
 * Fix issue with removing skeletons without shapes
   dartsim/dart#1625

Add them in d/patches since upstream does not provide patch releases
  • Loading branch information
@j-rivero
j-rivero committed Jan 10, 2022
1 parent 9f3e66d commit 4993330
Show file tree
Hide file tree
Showing 3 changed files with 5,336 additions and 0 deletions.
177 changes: 177 additions & 0 deletions debian/patches/0009-fix-skeleton-wihtout-shapes.patch
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,177 @@
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,
Loading

0 comments on commit 4993330

Please sign in to comment.