Try creating a loop joint

This PR supercedes #345 with a more general solution that would also help address #25 and solve a lot of issues related to casting joints. Open to feedback..

# TODO
- [ ] clean up all the excess joints and links  created during the process of loop joint creation.
- [ ] fix SetJointTransformChild to use only the joint.

Signed-off-by: Arjo Chakravarty <[email protected]>

Author: arjo129

dartsim/src/Base.hh

@@ -74,6 +74,7 @@ struct JointInfo
 {
   dart::dynamics::JointPtr joint;
   dart::dynamics::SimpleFramePtr frame;
+  dart::dynamics::BodyNode *phantomBody{nullptr};
 
   enum JointType
   {
@@ -364,7 +365,6 @@ 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",
@@ -376,23 +376,12 @@ class Base : public Implements3d<FeatureList<Feature>>
     return id;
   }
 
-  public: inline std::size_t AddJointConstraint(DartWeldJointConsPtr _joint)
+  public: inline void AddJointWeldConstraint(const std::size_t _id,
+    DartWeldJointConsPtr _joint, dart::dynamics::BodyNode* _phantomBody)
   {
-    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;
+    this->joints.idToObject[_id]->constraint = _joint;
+    this->joints.idToObject[_id]->type = JointInfo::JointType::CONSTRAINT;
+    this->joints.idToObject[_id]->phantomBody = _phantomBody;
   }
 
   public: inline std::size_t AddShape(

dartsim/src/JointFeatures.cc

@@ -331,29 +331,21 @@ void JointFeatures::SetJointMaxEffort(
 /////////////////////////////////////////////////
 std::size_t JointFeatures::GetJointDegreesOfFreedom(const Identity &_id) const
 {
-  auto jointInfo = this->ReferenceInterface<JointInfo>(_id);
-  if (jointInfo->type == JointInfo::JointType::CONSTRAINT)
-    return 0;
-  return jointInfo->joint->getNumDofs();
+  return this->ReferenceInterface<JointInfo>(_id)->joint->getNumDofs();
 }
 
 /////////////////////////////////////////////////
 Pose3d JointFeatures::GetJointTransformFromParent(const Identity &_id) const
 {
-  auto jointInfo = this->ReferenceInterface<JointInfo>(_id);
-  if (jointInfo->type == JointInfo::JointType::CONSTRAINT)
-    return jointInfo->constraint->getRelativeTransform().inverse();
-  return jointInfo->joint->getTransformFromParentBodyNode();
+  return this->ReferenceInterface<JointInfo>(_id)
+    ->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();
+    ->joint->getTransformFromChildBodyNode().inverse();
 }
 
 /////////////////////////////////////////////////
@@ -467,11 +459,6 @@ 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 *>(
           jointInfo->joint.get());
@@ -499,15 +486,25 @@ Identity JointFeatures::AttachFixedJoint(
   if (bn->getParentJoint()->getType() != "FreeJoint")
   {
     // child already has a parent joint
-    // use a WeldJointConstraint between the two bodies
+    // Create a phantom link and a joint to attach the child to the phantom
+    const auto &[joint, phantomBody] =
+      bn->getSkeleton()->createJointAndBodyNodePair<
+        dart::dynamics::WeldJoint,
+        dart::dynamics::BodyNode>(
+          bn, properties, dart::dynamics::BodyNode::AspectProperties());
+    const std::size_t jointID = this->AddJoint(joint);
+    phantomBody->setCollidable(false);
+
+    // use a WeldJointConstraint between the phantom joint and the link
     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);
+      std::make_shared<dart::constraint::WeldJointConstraint>(
+        parentBn, phantomBody);
     dartWorld->getConstraintSolver()->addConstraint(constraint);
-    auto jointID = this->AddJointConstraint(constraint);
+    this->AddJointWeldConstraint(jointID, constraint, phantomBody);
     return this->GenerateIdentity(jointID, this->joints.at(jointID));
   }
   {
@@ -530,11 +527,6 @@ 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 *>(
         jointInfo->joint.get());
@@ -559,11 +551,6 @@ 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 *>(
           jointInfo->joint.get());
@@ -609,9 +596,26 @@ Identity JointFeatures::AttachRevoluteJoint(
 
   if (bn->getParentJoint()->getType() != "FreeJoint")
   {
-    // child already has a parent joint
-    // TODO(scpeters): use a WeldJointConstraint between the two bodies
-    return this->GenerateInvalidId();
+    // Create a phantom link and a joint to attach the child to the phantom
+    const auto &[joint, phantomBody] =
+      bn->getSkeleton()->createJointAndBodyNodePair<
+        dart::dynamics::RevoluteJoint,
+        dart::dynamics::BodyNode>(
+          bn, properties, dart::dynamics::BodyNode::AspectProperties());
+    const std::size_t jointID = this->AddJoint(joint);
+    phantomBody->setCollidable(false);
+
+    // use a WeldJointConstraint between the phantom joint and the link
+    auto worldId = this->GetWorldOfModelImpl(
+        this->models.IdentityOf(bn->getSkeleton()));
+    auto dartWorld = this->worlds.at(worldId);
+
+    auto constraint =
+      std::make_shared<dart::constraint::WeldJointConstraint>(
+        parentBn, phantomBody);
+    dartWorld->getConstraintSolver()->addConstraint(constraint);
+    this->AddJointWeldConstraint(jointID, constraint, phantomBody);
+    return this->GenerateIdentity(jointID, this->joints.at(jointID));
   }
 
   {
@@ -634,11 +638,6 @@ 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*>(jointInfo->joint.get());
 
@@ -683,9 +682,26 @@ Identity JointFeatures::AttachPrismaticJoint(
 
   if (bn->getParentJoint()->getType() != "FreeJoint")
   {
-    // child already has a parent joint
-    // TODO(scpeters): use a WeldJointConstraint between the two bodies
-    return this->GenerateInvalidId();
+    // Create a phantom link and a joint to attach the child to the phantom
+    const auto &[joint, phantomBody] =
+      bn->getSkeleton()->createJointAndBodyNodePair<
+        dart::dynamics::PrismaticJoint,
+        dart::dynamics::BodyNode>(
+          bn, properties, dart::dynamics::BodyNode::AspectProperties());
+    const std::size_t jointID = this->AddJoint(joint);
+    phantomBody->setCollidable(false);
+
+    // use a WeldJointConstraint between the phantom joint and the link
+    auto worldId = this->GetWorldOfModelImpl(
+        this->models.IdentityOf(bn->getSkeleton()));
+    auto dartWorld = this->worlds.at(worldId);
+
+    auto constraint =
+      std::make_shared<dart::constraint::WeldJointConstraint>(
+        parentBn, phantomBody);
+    dartWorld->getConstraintSolver()->addConstraint(constraint);
+    this->AddJointWeldConstraint(jointID, constraint, phantomBody);
+    return this->GenerateIdentity(jointID, this->joints.at(jointID));
   }
 
   {

dartsim/src/JointFeatures_TEST.cc

@@ -978,6 +978,7 @@ TEST_F(JointFeaturesFixture, JointAttachMultiple)
   EXPECT_EQ(initialModel3Pose,
             math::eigen3::convert(dartBody3->getWorldTransform()));
 
+
   const std::size_t numSteps = 100;
   for (std::size_t i = 0; i < numSteps; ++i)
   {
@@ -995,12 +996,11 @@ TEST_F(JointFeaturesFixture, JointAttachMultiple)
     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);