dartsim: support non-tree kinematics in AttachFixedJoint

dartsim/src/Base.hh

@@ -18,19 +18,25 @@
 #ifndef IGNITION_PHYSICS_DARTSIM_BASE_HH_
 #define IGNITION_PHYSICS_DARTSIM_BASE_HH_
 
+#include <dart/constraint/ConstraintSolver.hpp>
+#include <dart/constraint/WeldJointConstraint.hpp>
 #include <dart/dynamics/BodyNode.hpp>
+#include <dart/dynamics/FreeJoint.hpp>
 #include <dart/dynamics/SimpleFrame.hpp>
 #include <dart/dynamics/Skeleton.hpp>
 #include <dart/simulation/World.hpp>
 
 #include <memory>
+#include <optional>
 #include <string>
 #include <tuple>
 #include <unordered_map>
 #include <utility>
 #include <vector>
 
 #include <ignition/common/Console.hh>
+#include <ignition/math/eigen3/Conversions.hh>
+#include <ignition/math/Inertial.hh>
 #include <ignition/physics/Implements.hh>
 
 #include <sdf/Types.hh>
@@ -57,6 +63,16 @@ struct LinkInfo
   /// moving the BodyNode to a new skeleton), so we store the Gazebo-specified
   /// name of the Link here.
   std::string name;
+  /// \brief To close kinematic loops, a body node may be divided into separate
+  /// nodes that are welded together using a WeldJointConstraint. The inertia
+  /// is divided evenly between these body nodes. This matches the approach
+  /// used in gazebo-classic.
+  std::vector< std::pair<
+      dart::dynamics::BodyNode *,
+      dart::constraint::WeldJointConstraintPtr> > weldedNodes;
+  /// \brief The total link inertia, which may be split between the `link` and
+  /// `weldedNodes` body nodes.
+  std::optional<math::Inertiald> inertial;
 };
 
 struct ModelInfo
@@ -319,7 +335,8 @@ class Base : public Implements3d<FeatureList<Feature>>
   }
 
   public: inline std::size_t AddLink(DartBodyNode *_bn,
-        const std::string &_fullName, std::size_t _modelID)
+        const std::string &_fullName, std::size_t _modelID,
+        std::optional<math::Inertiald> _inertial = std::nullopt)
   {
     const std::size_t id = this->GetNextEntity();
     auto linkInfo = std::make_shared<LinkInfo>();
@@ -328,6 +345,9 @@ class Base : public Implements3d<FeatureList<Feature>>
     // The name of the BodyNode during creation is assumed to be the
     // Gazebo-specified name.
     linkInfo->name = _bn->getName();
+    // Inertial properties (if available) used when splitting nodes to close
+    // kinematic loops.
+    linkInfo->inertial = _inertial;
     this->links.objectToID[_bn] = id;
     this->frames[id] = _bn;
 
@@ -347,6 +367,58 @@ class Base : public Implements3d<FeatureList<Feature>>
     return id;
   }
 
+  public: inline DartBodyNode* SplitAndWeldLink(LinkInfo *_link)
+  {
+    // First create a new body node with FreeJoint and a unique name based
+    // on the number of welded miror nodes.
+    dart::dynamics::BodyNode::Properties weldedBodyProperties;
+    {
+      std::size_t weldedBodyCount = _link->weldedNodes.size();
+      weldedBodyProperties.mName =
+          _link->name + "_welded_mirror_" + std::to_string(weldedBodyCount);
+    }
+    dart::dynamics::FreeJoint::Properties jointProperties;
+    jointProperties.mName = weldedBodyProperties.mName + "_FreeJoint";
+    DartSkeletonPtr skeleton = _link->link->getSkeleton();
+    auto pairJointBodyNode =
+      skeleton->createJointAndBodyNodePair<dart::dynamics::FreeJoint>(
+        nullptr, jointProperties, weldedBodyProperties);
+
+    // Weld the new body node to the original
+    auto weld = std::make_shared<dart::constraint::WeldJointConstraint>(
+        _link->link, pairJointBodyNode.second);
+    _link->weldedNodes.emplace_back(pairJointBodyNode.second, weld);
+    auto worldId = this->GetWorldOfModelImpl(models.objectToID[skeleton]);
+    auto dartWorld = this->worlds.at(worldId);
+    dartWorld->getConstraintSolver()->addConstraint(weld);
+
+    // Rebalance the link inertia between the original body node and its
+    // welded mirror nodes if inertial data is available.
+    if (_link->inertial)
+    {
+      std::size_t nodeCount = 1 + _link->weldedNodes.size();
+      const double mass = _link->inertial->MassMatrix().Mass() / nodeCount;
+      const math::Matrix3d moi = _link->inertial->Moi() * (1 / nodeCount);
+      const math::Vector3d cog = _link->inertial->Pose().Pos();
+
+      _link->link->setMass(mass);
+      _link->link->setMomentOfInertia(
+          moi(0, 0), moi(1, 1), moi(2, 2),
+          moi(0, 1), moi(0, 2), moi(1, 2));
+      _link->link->setLocalCOM(math::eigen3::convert(cog));
+      for (auto weldedNodePair : _link->weldedNodes)
+      {
+        weldedNodePair.first->setMass(mass);
+        weldedNodePair.first->setMomentOfInertia(
+            moi(0, 0), moi(1, 1), moi(2, 2),
+            moi(0, 1), moi(0, 2), moi(1, 2));
+        weldedNodePair.first->setLocalCOM(math::eigen3::convert(cog));
+      }
+    }
+
+    return pairJointBodyNode.second;
+  }
+
   public: inline std::size_t AddJoint(DartJoint *_joint)
   {
     const std::size_t id = this->GetNextEntity();

dartsim/src/JointFeatures.cc

@@ -455,7 +455,7 @@ Identity JointFeatures::AttachFixedJoint(
     const std::string &_name)
 {
   auto linkInfo = this->ReferenceInterface<LinkInfo>(_childID);
-  DartBodyNode *const bn = linkInfo->link.get();
+  DartBodyNode *bn = linkInfo->link.get();
   dart::dynamics::WeldJoint::Properties properties;
   properties.mName = _name;
 
@@ -465,8 +465,8 @@ 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();
+    // split and weld the child body node, and attach to the new welded node
+    bn = SplitAndWeldLink(linkInfo);
   }
 
   {

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)

dartsim/src/SDFFeatures.cc

@@ -640,7 +640,7 @@ Identity SDFFeatures::ConstructSdfLink(
   const std::string fullName = ::sdf::JoinName(
       world->getName(),
       ::sdf::JoinName(modelInfo.model->getName(), bn->getName()));
-  const std::size_t linkID = this->AddLink(bn, fullName, _modelID);
+  const std::size_t linkID = this->AddLink(bn, fullName, _modelID, sdfInertia);
   this->AddJoint(joint);
 
   auto linkIdentity = this->GenerateIdentity(linkID, this->links.at(linkID));