Add WeldJointConstraint for fixed joints if the parent is not a FreeJoint

dartsim/src/Base.hh

@@ -21,6 +21,7 @@
 #include <dart/dynamics/BodyNode.hpp>
 #include <dart/dynamics/SimpleFrame.hpp>
 #include <dart/dynamics/Skeleton.hpp>
+#include <dart/constraint/WeldJointConstraint.hpp>
 #include <dart/simulation/World.hpp>
 
 #include <memory>
@@ -73,6 +74,15 @@ struct JointInfo
 {
   dart::dynamics::JointPtr joint;
   dart::dynamics::SimpleFramePtr frame;
+
+  enum JointType
+  {
+    JOINT,
+    CONSTRAINT
+  };
+
+  JointType type{JOINT};
+  dart::constraint::WeldJointConstraintPtr constraint;
 };
 
 struct ShapeInfo
@@ -221,6 +231,7 @@ class Base : public Implements3d<FeatureList<Feature>>
   public: using DartBodyNodePtr = dart::dynamics::BodyNodePtr;
   public: using DartJoint = dart::dynamics::Joint;
   public: using DartJointPtr = dart::dynamics::JointPtr;
+  public: using DartWeldJointConsPtr = dart::constraint::WeldJointConstraintPtr;
   public: using DartShapeNode = dart::dynamics::ShapeNode;
   public: using DartShapeNodePtr = std::shared_ptr<DartShapeNode>;
   public: using ModelInfoPtr = std::shared_ptr<ModelInfo>;
@@ -353,6 +364,7 @@ class Base : public Implements3d<FeatureList<Feature>>
     this->joints.idToObject[id] = std::make_shared<JointInfo>();
     this->joints.idToObject[id]->joint = _joint;
     this->joints.objectToID[_joint] = id;
+    this->joints.idToObject[id]->type = JointInfo::JointType::JOINT;
     dart::dynamics::SimpleFramePtr jointFrame =
         dart::dynamics::SimpleFrame::createShared(
             _joint->getChildBodyNode(), _joint->getName() + "_frame",
@@ -364,6 +376,25 @@ class Base : public Implements3d<FeatureList<Feature>>
     return id;
   }
 
+  public: inline std::size_t AddJointConstraint(DartWeldJointConsPtr _joint)
+  {
+    const std::size_t id = this->GetNextEntity();
+    this->joints.idToObject[id] = std::make_shared<JointInfo>();
+    this->joints.idToObject[id]->constraint = _joint;
+    this->joints.idToObject[id]->type = JointInfo::JointType::CONSTRAINT;
+    // TODO(arjo): Refactor the joints.objectToId to support constraints.
+    // this->joints.objectToID[_joint] = id;
+    // dart::dynamics::SimpleFramePtr jointFrame =
+    //     dart::dynamics::SimpleFrame::createShared(
+    //         _joint->getChildBodyNode(), _joint->getName() + "_frame",
+    //         _joint->getTransformFromChildBodyNode());
+
+    // this->joints.idToObject[id]->frame = jointFrame;
+    // this->frames[id] = this->joints.idToObject[id]->frame.get();
+
+    return id;
+  }
+
   public: inline std::size_t AddShape(
       const ShapeInfo &_info)
   {

dartsim/src/JointFeatures.cc

@@ -22,6 +22,10 @@
 #include <dart/dynamics/RevoluteJoint.hpp>
 #include <dart/dynamics/WeldJoint.hpp>
 
+#include <dart/constraint/ConstraintSolver.hpp>
+
+#include <memory>
+
 #include "JointFeatures.hh"
 
 namespace ignition {
@@ -327,19 +331,27 @@ void JointFeatures::SetJointMaxEffort(
 /////////////////////////////////////////////////
 std::size_t JointFeatures::GetJointDegreesOfFreedom(const Identity &_id) const
 {
-  return this->ReferenceInterface<JointInfo>(_id)->joint->getNumDofs();
+  auto jointInfo = this->ReferenceInterface<JointInfo>(_id);
+  if (jointInfo->type == JointInfo::JointType::CONSTRAINT)
+    return 0;
+  return jointInfo->joint->getNumDofs();
 }
 
 /////////////////////////////////////////////////
 Pose3d JointFeatures::GetJointTransformFromParent(const Identity &_id) const
 {
-  return this->ReferenceInterface<JointInfo>(_id)
-      ->joint->getTransformFromParentBodyNode();
+  auto jointInfo = this->ReferenceInterface<JointInfo>(_id);
+  if (jointInfo->type == JointInfo::JointType::CONSTRAINT)
+    return jointInfo->constraint->getRelativeTransform().inverse();
+  return jointInfo->joint->getTransformFromParentBodyNode();
 }
 
 /////////////////////////////////////////////////
 Pose3d JointFeatures::GetJointTransformToChild(const Identity &_id) const
 {
+  auto jointInfo = this->ReferenceInterface<JointInfo>(_id);
+  if (jointInfo->type == JointInfo::JointType::CONSTRAINT)
+    return jointInfo->constraint->getRelativeTransform();
   return this->ReferenceInterface<JointInfo>(_id)
       ->joint->getTransformFromChildBodyNode().inverse();
 }
@@ -348,22 +360,38 @@ Pose3d JointFeatures::GetJointTransformToChild(const Identity &_id) const
 void JointFeatures::SetJointTransformFromParent(
     const Identity &_id, const Pose3d &_pose)
 {
-  this->ReferenceInterface<JointInfo>(_id)
-      ->joint->setTransformFromParentBodyNode(_pose);
+  auto jointInfo = this->ReferenceInterface<JointInfo>(_id);
+  if (jointInfo->type == JointInfo::JointType::CONSTRAINT)
+    jointInfo->constraint->setRelativeTransform(_pose.inverse());
+  else
+    jointInfo->joint->setTransformFromParentBodyNode(_pose);
 }
 
 /////////////////////////////////////////////////
 void JointFeatures::SetJointTransformToChild(
     const Identity &_id, const Pose3d &_pose)
 {
-  this->ReferenceInterface<JointInfo>(_id)
-      ->joint->setTransformFromChildBodyNode(_pose.inverse());
+  auto jointInfo = this->ReferenceInterface<JointInfo>(_id);
+  if (jointInfo->type == JointInfo::JointType::CONSTRAINT)
+    jointInfo->constraint->setRelativeTransform(_pose);
+  else
+    jointInfo->joint->setTransformFromChildBodyNode(_pose.inverse());
 }
 
 /////////////////////////////////////////////////
 void JointFeatures::DetachJoint(const Identity &_jointId)
 {
-  auto joint = this->ReferenceInterface<JointInfo>(_jointId)->joint;
+  auto jointInfo = this->ReferenceInterface<JointInfo>(_jointId);
+  if (jointInfo->type == JointInfo::JointType::CONSTRAINT)
+  {
+    auto constraint = jointInfo->constraint;
+    auto worldId = this->GetWorldOfModelImpl(
+        this->models.IdentityOf(constraint->getBodyNode1()->getSkeleton()));
+    auto dartWorld = this->worlds.at(worldId);
+    dartWorld->getConstraintSolver()->removeConstraint(constraint);
+    return;
+  }
+  auto &joint = jointInfo->joint;
   if (joint->getType() == "FreeJoint")
   {
     // don't need to do anything, joint is already a FreeJoint
@@ -438,9 +466,15 @@ void JointFeatures::DetachJoint(const Identity &_jointId)
 Identity JointFeatures::CastToFixedJoint(
     const Identity &_jointID) const
 {
+  auto jointInfo = this->ReferenceInterface<JointInfo>(_jointID);
+  if (jointInfo->type == JointInfo::JointType::CONSTRAINT)
+  {
+    // TODO(arjo): Handle constraint casts.
+    return this->GenerateInvalidId();
+  }
   dart::dynamics::WeldJoint *const weld =
       dynamic_cast<dart::dynamics::WeldJoint *>(
-          this->ReferenceInterface<JointInfo>(_jointID)->joint.get());
+          jointInfo->joint.get());
 
   if (weld)
     return this->GenerateIdentity(_jointID, this->Reference(_jointID));
@@ -465,10 +499,17 @@ Identity JointFeatures::AttachFixedJoint(
   if (bn->getParentJoint()->getType() != "FreeJoint")
   {
     // child already has a parent joint
-    // TODO(scpeters): use a WeldJointConstraint between the two bodies
-    return this->GenerateInvalidId();
+    // use a WeldJointConstraint between the two bodies
+    auto worldId = this->GetWorldOfModelImpl(
+        this->models.IdentityOf(bn->getSkeleton()));
+    auto dartWorld = this->worlds.at(worldId);
+
+    auto constraint =
+      std::make_shared<dart::constraint::WeldJointConstraint>(parentBn, bn);
+    dartWorld->getConstraintSolver()->addConstraint(constraint);
+    auto jointID = this->AddJointConstraint(constraint);
+    return this->GenerateIdentity(jointID, this->joints.at(jointID));
   }
-
   {
     auto skeleton = bn->getSkeleton();
     if (skeleton)
@@ -488,9 +529,15 @@ Identity JointFeatures::AttachFixedJoint(
 Identity JointFeatures::CastToFreeJoint(
     const Identity &_jointID) const
 {
+  auto jointInfo = this->ReferenceInterface<JointInfo>(_jointID);
+  if (jointInfo->type == JointInfo::JointType::CONSTRAINT)
+  {
+    // TODO(arjo): Handle constraint casts.
+    return this->GenerateInvalidId();
+  }
   auto *const freeJoint =
       dynamic_cast<dart::dynamics::FreeJoint *>(
-          this->ReferenceInterface<JointInfo>(_jointID)->joint.get());
+        jointInfo->joint.get());
 
   if (freeJoint)
     return this->GenerateIdentity(_jointID, this->Reference(_jointID));
@@ -511,9 +558,15 @@ void JointFeatures::SetFreeJointRelativeTransform(
 Identity JointFeatures::CastToRevoluteJoint(
     const Identity &_jointID) const
 {
+  auto jointInfo = this->ReferenceInterface<JointInfo>(_jointID);
+  if (jointInfo->type == JointInfo::JointType::CONSTRAINT)
+  {
+    // TODO(arjo): Handle constraint casts.
+    return this->GenerateInvalidId();
+  }
   dart::dynamics::RevoluteJoint *const revolute =
       dynamic_cast<dart::dynamics::RevoluteJoint *>(
-          this->ReferenceInterface<JointInfo>(_jointID)->joint.get());
+          jointInfo->joint.get());
 
   if (revolute)
     return this->GenerateIdentity(_jointID, this->Reference(_jointID));
@@ -580,9 +633,14 @@ Identity JointFeatures::AttachRevoluteJoint(
 Identity JointFeatures::CastToPrismaticJoint(
     const Identity &_jointID) const
 {
+  auto jointInfo = this->ReferenceInterface<JointInfo>(_jointID);
+  if (jointInfo->type == JointInfo::JointType::CONSTRAINT)
+  {
+    // TODO(arjo): Handle constraint casts.
+    return this->GenerateInvalidId();
+  }
   dart::dynamics::PrismaticJoint *prismatic =
-      dynamic_cast<dart::dynamics::PrismaticJoint*>(
-        this->ReferenceInterface<JointInfo>(_jointID)->joint.get());
+      dynamic_cast<dart::dynamics::PrismaticJoint*>(jointInfo->joint.get());
 
   if (prismatic)
     return this->GenerateIdentity(_jointID, this->Reference(_jointID));

dartsim/src/JointFeatures_TEST.cc

@@ -883,6 +883,142 @@ TEST_F(JointFeaturesFixture, JointAttachDetach)
   EXPECT_NEAR(0.0, dartBody2->getLinearVelocity().z(), 1e-3);
 }
 
+/////////////////////////////////////////////////
+// Essentially what happens is there are two floating boxes and a box in the
+// middle that's resting. We start the system out by creating the two
+// fixed joints between the boxes resting on the big box. The middle box will
+// now have two parents. However there should be no movement as the middle box
+// will be holding the other two boxes that are floating in mid air. We run
+// this for 100 steps to make sure that there is no movement. This is because
+// the middle box is holding on to the two side boxes. Then we release the
+// joints the two boxes should fall away.
+TEST_F(JointFeaturesFixture, JointAttachMultiple)
+{
+  sdf::Root root;
+  const sdf::Errors errors =
+      root.Load(TEST_WORLD_DIR "joint_constraint.sdf");
+  ASSERT_TRUE(errors.empty()) << errors.front();
+
+  auto world = this->engine->ConstructWorld(*root.WorldByIndex(0));
+  dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld();
+  ASSERT_NE(nullptr, dartWorld);
+
+  // M1 and M3 are floating boxes
+  const std::string modelName1{"M1"};
+  const std::string modelName2{"M2"};
+  const std::string modelName3{"M3"};
+  const std::string bodyName{"link"};
+
+  auto model1 = world->GetModel(modelName1);
+  auto model2 = world->GetModel(modelName2);
+  auto model3 = world->GetModel(modelName3);
+
+  auto model1Body = model1->GetLink(bodyName);
+  auto model2Body = model2->GetLink(bodyName);
+  auto model3Body = model3->GetLink(bodyName);
+
+  const dart::dynamics::SkeletonPtr skeleton1 =
+      dartWorld->getSkeleton(modelName1);
+  const dart::dynamics::SkeletonPtr skeleton2 =
+      dartWorld->getSkeleton(modelName2);
+  const dart::dynamics::SkeletonPtr skeleton3 =
+      dartWorld->getSkeleton(modelName3);
+  ASSERT_NE(nullptr, skeleton1);
+  ASSERT_NE(nullptr, skeleton2);
+  ASSERT_NE(nullptr, skeleton3);
+
+  auto *dartBody1 = skeleton1->getBodyNode(bodyName);
+  auto *dartBody2 = skeleton2->getBodyNode(bodyName);
+  auto *dartBody3 = skeleton3->getBodyNode(bodyName);
+
+  ASSERT_NE(nullptr, dartBody1);
+  ASSERT_NE(nullptr, dartBody2);
+  ASSERT_NE(nullptr, dartBody3);
+
+  const math::Pose3d initialModel1Pose(0, -0.2, 0.45, 0, 0, 0);
+  const math::Pose3d initialModel2Pose(0, 0.2, 0.45, 0, 0, 0);
+  const math::Pose3d initialModel3Pose(0, 0.6, 0.45, 0, 0, 0);
+
+  EXPECT_EQ(initialModel1Pose,
+            math::eigen3::convert(dartBody1->getWorldTransform()));
+  EXPECT_EQ(initialModel2Pose,
+            math::eigen3::convert(dartBody2->getWorldTransform()));
+  EXPECT_EQ(initialModel3Pose,
+            math::eigen3::convert(dartBody3->getWorldTransform()));
+
+  physics::ForwardStep::Output output;
+  physics::ForwardStep::State state;
+  physics::ForwardStep::Input input;
+
+  // Create the first joint. This should be a normal fixed joint.
+  const auto poseParent1 = dartBody1->getTransform();
+  const auto poseChild1 = dartBody2->getTransform();
+  auto poseParentChild1 = poseParent1.inverse() * poseChild1;
+  auto fixedJoint1 = model2Body->AttachFixedJoint(model1Body);
+  fixedJoint1->SetTransformFromParent(poseParentChild1);
+
+  EXPECT_EQ(initialModel1Pose,
+            math::eigen3::convert(dartBody1->getWorldTransform()));
+  EXPECT_EQ(initialModel2Pose,
+            math::eigen3::convert(dartBody2->getWorldTransform()));
+  EXPECT_EQ(initialModel3Pose,
+            math::eigen3::convert(dartBody3->getWorldTransform()));
+
+  // Create the second joint. This should be a WeldJoint constraint
+  const auto poseParent2 = dartBody3->getTransform();
+  const auto poseChild2 = dartBody2->getTransform();
+  auto poseParentChild2 = poseParent2.inverse() * poseChild2;
+  auto fixedJoint2 = model2Body->AttachFixedJoint(model3Body);
+  fixedJoint2->SetTransformFromParent(poseParentChild2);
+
+  EXPECT_EQ(initialModel1Pose,
+            math::eigen3::convert(dartBody1->getWorldTransform()));
+  EXPECT_EQ(initialModel2Pose,
+            math::eigen3::convert(dartBody2->getWorldTransform()));
+  EXPECT_EQ(initialModel3Pose,
+            math::eigen3::convert(dartBody3->getWorldTransform()));
+
+  const std::size_t numSteps = 100;
+  for (std::size_t i = 0; i < numSteps; ++i)
+  {
+    world->Step(output, state, input);
+
+    // Expect all the bodies to be at rest.
+    // (since they're held in place by the joints)
+    math::Vector3d body1LinearVelocity =
+        math::eigen3::convert(dartBody1->getLinearVelocity());
+    math::Vector3d body2LinearVelocity =
+        math::eigen3::convert(dartBody2->getLinearVelocity());
+    math::Vector3d body3LinearVelocity =
+        math::eigen3::convert(dartBody3->getLinearVelocity());
+    EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-7);
+    EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-7);
+    EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-7);
+  }
+
+  // Detach the joints. M1 and M3 should fall as there is now nothing stopping
+  // them from falling.
+  fixedJoint1->Detach();
+  fixedJoint2->Detach();
+
+  for (std::size_t i = 0; i < numSteps; ++i)
+  {
+    world->Step(output, state, input);
+
+    // Expect the middle box to be still as it is already at rest.
+    // Expect the two side boxes to fall away.
+    math::Vector3d body1LinearVelocity =
+        math::eigen3::convert(dartBody1->getLinearVelocity());
+    math::Vector3d body2LinearVelocity =
+        math::eigen3::convert(dartBody2->getLinearVelocity());
+    math::Vector3d body3LinearVelocity =
+        math::eigen3::convert(dartBody3->getLinearVelocity());
+    EXPECT_GT(0, body1LinearVelocity.Z());
+    EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-7);
+    EXPECT_GT(0, body3LinearVelocity.Z());
+  }
+}
+
 /////////////////////////////////////////////////
 // Expectations on number of links before/after attach/detach
 TEST_F(JointFeaturesFixture, LinkCountsInJointAttachDetach)