From 21a608d08188eaef02b4b9433c9c088d57be5f84 Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Thu, 9 Jun 2022 15:17:41 -0700
Subject: [PATCH] Fix test by resetting constraint transform

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 dartsim/src/Base.hh          |  1 +
 dartsim/src/JointFeatures.cc | 10 +++++++++-
 2 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh
index a82817ec6..b882164a6 100644
--- a/dartsim/src/Base.hh
+++ b/dartsim/src/Base.hh
@@ -372,6 +372,7 @@ class Base : public Implements3d<FeatureList<Feature>>
     // 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;
+    weldedBodyProperties.mIsCollidable = false;
     {
       std::size_t weldedBodyCount = _link->weldedNodes.size();
       weldedBodyProperties.mName =
diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc
index 34f7a4e64..3bc806018 100644
--- a/dartsim/src/JointFeatures.cc
+++ b/dartsim/src/JointFeatures.cc
@@ -462,22 +462,30 @@ Identity JointFeatures::AttachFixedJoint(
   auto *const parentBn = _parent ? this->ReferenceInterface<LinkInfo>(
       _parent->FullIdentity())->link.get() : nullptr;
 
+  std::string childLinkName = linkInfo->name;
   if (bn->getParentJoint()->getType() != "FreeJoint")
   {
     // child already has a parent joint
     // split and weld the child body node, and attach to the new welded node
     bn = SplitAndWeldLink(linkInfo);
+    childLinkName = bn->getName();
   }
 
   {
     auto skeleton = bn->getSkeleton();
     if (skeleton)
     {
-      bn->setName(skeleton->getName() + '/' + linkInfo->name);
+      bn->setName(skeleton->getName() + '/' + childLinkName);
     }
   }
   const std::size_t jointID = this->AddJoint(
       bn->moveTo<dart::dynamics::WeldJoint>(parentBn, properties));
+  if (linkInfo->weldedNodes.size() > 0)
+  {
+    // weld constraint needs to be updated after moving to new skeleton
+    auto constraint = linkInfo->weldedNodes.back().second;
+    constraint->setRelativeTransform(Eigen::Isometry3d::Identity());
+  }
   // TODO(addisu) Remove incrementVersion once DART has been updated to
   // internally increment the BodyNode's version after moveTo.
   bn->incrementVersion();