Fix collision issue with detachable joints

Changelog.md

@@ -2,6 +2,9 @@
 
 ### Ignition Physics 1.x.x (20XX-XX-XX)
 
+1. Fix collision issue with detachable joints
+    * [Pull request 31](https://github.com/ignitionrobotics/ign-physics/pull/31)
+
 1. Add PlaneShape feature and implement in dartsim with test.
     * [BitBucket pull request 66](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/66)
 

dartsim/src/JointFeatures.cc

@@ -161,6 +161,9 @@ void JointFeatures::DetachJoint(const Identity &_jointId)
   freeJoint->setSpatialVelocity(spatialVelocity,
           dart::dynamics::Frame::World(),
           dart::dynamics::Frame::World());
+  // TODO(addisu) Remove incrementVersion once DART has been updated to
+  // internally increment the BodyNode's version after moveTo.
+  child->incrementVersion();
 }
 
 /////////////////////////////////////////////////
@@ -200,6 +203,9 @@ Identity JointFeatures::AttachFixedJoint(
 
   const std::size_t jointID = this->AddJoint(
       bn->moveTo<dart::dynamics::WeldJoint>(parentBn, properties));
+  // TODO(addisu) Remove incrementVersion once DART has been updated to
+  // internally increment the BodyNode's version after moveTo.
+  bn->incrementVersion();
   return this->GenerateIdentity(jointID, this->joints.at(jointID));
 }
 
@@ -282,6 +288,9 @@ Identity JointFeatures::AttachRevoluteJoint(
 
   const std::size_t jointID = this->AddJoint(
       bn->moveTo<dart::dynamics::RevoluteJoint>(parentBn, properties));
+  // TODO(addisu) Remove incrementVersion once DART has been updated to
+  // internally increment the BodyNode's version after moveTo.
+  bn->incrementVersion();
   return this->GenerateIdentity(jointID, this->joints.at(jointID));
 }
 
@@ -341,6 +350,9 @@ Identity JointFeatures::AttachPrismaticJoint(
 
   const std::size_t jointID = this->AddJoint(
       bn->moveTo<dart::dynamics::PrismaticJoint>(parentBn, properties));
+  // TODO(addisu) Remove incrementVersion once DART has been updated to
+  // internally increment the BodyNode's version after moveTo.
+  bn->incrementVersion();
   return this->GenerateIdentity(jointID, this->joints.at(jointID));
 }
 

dartsim/src/JointFeatures_TEST.cc

@@ -37,8 +37,10 @@
 #include <ignition/physics/Joint.hh>
 #include <ignition/physics/RevoluteJoint.hh>
 #include <ignition/physics/dartsim/World.hh>
+#include <ignition/physics/sdf/ConstructModel.hh>
 #include <ignition/physics/sdf/ConstructWorld.hh>
 
+#include <sdf/Model.hh>
 #include <sdf/Root.hh>
 #include <sdf/World.hh>
 
@@ -50,12 +52,14 @@ using TestFeatureList = ignition::physics::FeatureList<
   physics::dartsim::RetrieveWorld,
   physics::AttachFixedJointFeature,
   physics::DetachJointFeature,
+  physics::SetJointTransformFromParentFeature,
   physics::ForwardStep,
   physics::FreeJointCast,
   physics::GetBasicJointState,
   physics::GetEntities,
   physics::RevoluteJointCast,
   physics::SetJointVelocityCommandFeature,
+  physics::sdf::ConstructSdfModel,
   physics::sdf::ConstructSdfWorld
 >;
 
@@ -317,8 +321,17 @@ TEST_F(JointFeaturesFixture, JointAttachDetach)
     EXPECT_GT(0.0, body2LinearVelocity.Z());
   }
 
+  const auto poseParent = dartBody1->getTransform();
+  const auto poseChild = dartBody2->getTransform();
+  auto poseParentChild = poseParent.inverse() * poseChild;
+
   auto fixedJoint = model2Body->AttachFixedJoint(model1Body);
 
+  // AttachFixedJoint snaps the child body to the origin of the parent, so we
+  // set a transform on the joint to keep the transform between the two bodies
+  // the same as it was before they were attached
+  fixedJoint->SetTransformFromParent(poseParentChild);
+
   for (std::size_t i = 0; i < numSteps; ++i)
   {
     world->Step(output, state, input);
@@ -350,6 +363,14 @@ TEST_F(JointFeaturesFixture, JointAttachDetach)
     // Negative z velocity
     EXPECT_GT(0.0, body2LinearVelocity.Z());
   }
+
+  // After a while, body2 should reach the ground and come to a stop
+  for (std::size_t i = 0; i < 1000; ++i)
+  {
+    world->Step(output, state, input);
+  }
+
+  EXPECT_NEAR(0.0, dartBody2->getLinearVelocity().z(), 1e-3);
 }
 
 /////////////////////////////////////////////////
@@ -397,6 +418,135 @@ TEST_F(JointFeaturesFixture, LinkCountsInJointAttachDetach)
   EXPECT_EQ(0u, model2->GetLinkCount());
 }
 
+/////////////////////////////////////////////////
+// Attach a fixed joint between links that belong to different models where one
+// of the models is created after a step is called
+TEST_F(JointFeaturesFixture, JointAttachDetachSpawnedModel)
+{
+  std::string model1Str = R"(
+  <sdf version="1.6">
+    <model name="M1">
+      <pose>0 0 0.1 0 0 0</pose>
+      <link name="body">
+        <collision name="coll_box">
+          <geometry>
+            <box>
+              <size>0.2 0.2 0.2</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+    </model>
+  </sdf>)";
+
+  std::string model2Str = R"(
+  <sdf version="1.6">
+    <model name="M2">
+      <pose>1 0 0.1 0 0 0</pose>
+      <link name="chassis">
+        <collision name="coll_sphere">
+          <geometry>
+            <sphere>
+              <radius>0.1</radius>
+            </sphere>
+          </geometry>
+        </collision>
+      </link>
+    </model>
+  </sdf>)";
+
+  physics::ForwardStep::Output output;
+  physics::ForwardStep::State state;
+  physics::ForwardStep::Input input;
+
+  physics::World3dPtr<TestFeatureList> world;
+  {
+    sdf::Root root;
+    const sdf::Errors errors = root.Load(TEST_WORLD_DIR "ground.sdf");
+    ASSERT_TRUE(errors.empty()) << errors.front();
+    world = this->engine->ConstructWorld(*root.WorldByIndex(0));
+    ASSERT_NE(nullptr, world);
+  }
+
+  {
+    sdf::Root root;
+    sdf::Errors errors = root.LoadSdfString(model1Str);
+    ASSERT_TRUE(errors.empty()) << errors.front();
+    ASSERT_NE(nullptr, root.ModelByIndex(0));
+    world->ConstructModel(*root.ModelByIndex(0));
+  }
+
+  world->Step(output, state, input);
+
+  {
+    sdf::Root root;
+    sdf::Errors errors = root.LoadSdfString(model2Str);
+    ASSERT_TRUE(errors.empty()) << errors.front();
+    ASSERT_NE(nullptr, root.ModelByIndex(0));
+    world->ConstructModel(*root.ModelByIndex(0));
+  }
+
+  const std::string modelName1{"M1"};
+  const std::string modelName2{"M2"};
+  const std::string bodyName1{"body"};
+  const std::string bodyName2{"chassis"};
+
+  auto model1 = world->GetModel(modelName1);
+  auto model2 = world->GetModel(modelName2);
+  auto model1Body = model1->GetLink(bodyName1);
+  auto model2Body = model2->GetLink(bodyName2);
+
+  dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld();
+  ASSERT_NE(nullptr, dartWorld);
+
+  const auto skeleton1 = dartWorld->getSkeleton(modelName1);
+  const auto skeleton2 = dartWorld->getSkeleton(modelName2);
+  ASSERT_NE(nullptr, skeleton1);
+  ASSERT_NE(nullptr, skeleton2);
+
+  auto *dartBody1 = skeleton1->getBodyNode(bodyName1);
+  auto *dartBody2 = skeleton2->getBodyNode(bodyName2);
+
+  ASSERT_NE(nullptr, dartBody1);
+  ASSERT_NE(nullptr, dartBody2);
+
+  const auto poseParent = dartBody1->getTransform();
+  const auto poseChild = dartBody2->getTransform();
+
+  // Before ign-physics PR #31, uncommenting the following `step` call makes
+  // this test pass, but commenting it out makes it fail.
+  // world->Step(output, state, input);
+  auto fixedJoint = model2Body->AttachFixedJoint(model1Body);
+
+  // Pose of child relative to parent
+  auto poseParentChild = poseParent.inverse() * poseChild;
+
+  // We let the joint be at the origin of the child link.
+  fixedJoint->SetTransformFromParent(poseParentChild);
+
+  const std::size_t numSteps = 100;
+
+  for (std::size_t i = 0; i < numSteps; ++i)
+  {
+    world->Step(output, state, input);
+  }
+
+  // Expect both bodies to hit the ground and stop
+  EXPECT_NEAR(0.0, dartBody1->getLinearVelocity().z(), 1e-3);
+  EXPECT_NEAR(0.0, dartBody2->getLinearVelocity().z(), 1e-3);
+
+  fixedJoint->Detach();
+
+  for (std::size_t i = 0; i < numSteps; ++i)
+  {
+    world->Step(output, state, input);
+  }
+
+  // Expect both bodies to remain in contact with the ground with zero velocity.
+  EXPECT_NEAR(0.0, dartBody1->getLinearVelocity().z(), 1e-3);
+  EXPECT_NEAR(0.0, dartBody2->getLinearVelocity().z(), 1e-3);
+}
+
 /////////////////////////////////////////////////
 int main(int argc, char *argv[])
 {

dartsim/worlds/ground.sdf

@@ -0,0 +1,35 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="test_world">
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>100</mu>
+                <mu2>50</mu2>
+              </ode>
+            </friction>
+          </surface>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </visual>
+      </link>
+    </model>
+  </world>
+</sdf>
+