Use correct link indicies when constructing fixed constraints

Signed-off-by: Michael Carroll <mjcarroll@intrinsic.ai>

bullet-featherstone/src/JointFeatures.cc

@@ -303,11 +303,11 @@ Identity JointFeatures::AttachFixedJoint(
     const BaseLink3dPtr &_parent,
     const std::string &_name)
 {
-  auto linkInfo = this->ReferenceInterface<LinkInfo>(_childID);
-  auto modelInfo = this->ReferenceInterface<ModelInfo>(linkInfo->model);
-  auto parentLinkInfo = this->ReferenceInterface<LinkInfo>(
+  auto *linkInfo = this->ReferenceInterface<LinkInfo>(_childID);
+  auto *modelInfo = this->ReferenceInterface<ModelInfo>(linkInfo->model);
+  auto *parentLinkInfo = this->ReferenceInterface<LinkInfo>(
     _parent->FullIdentity());
-  auto parentModelInfo = this->ReferenceInterface<ModelInfo>(
+  auto *parentModelInfo = this->ReferenceInterface<ModelInfo>(
     parentLinkInfo->model);
   auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);
 
@@ -322,16 +322,18 @@ Identity JointFeatures::AttachFixedJoint(
       linkInfo->model
     });
 
-  auto jointInfo = this->ReferenceInterface<JointInfo>(jointID);
+  auto parentLinkIdx = parentLinkInfo->indexInModel.value_or(-1);
+  auto childLinkIdx = linkInfo->indexInModel.value_or(-1);
+  auto *jointInfo = this->ReferenceInterface<JointInfo>(jointID);
 
   jointInfo->fixedConstraint = std::make_shared<btMultiBodyFixedConstraint>(
-    parentModelInfo->body.get(), -1,
-    modelInfo->body.get(), -1,
+    parentModelInfo->body.get(), parentLinkIdx,
+    modelInfo->body.get(), childLinkIdx,
     btVector3(0, 0, 0), btVector3(0, 0, 0),
     btMatrix3x3::getIdentity(),
     btMatrix3x3::getIdentity());
 
-  if (world && world->world)
+  if (world != nullptr && world->world)
   {
     world->world->addMultiBodyConstraint(jointInfo->fixedConstraint.get());
     return this->GenerateIdentity(jointID, this->joints.at(jointID));