[bullet-featherstone] Fix attaching fixed joint (#610)

Signed-off-by: Ian Chen <[email protected]>
gazebosim · Mar 23, 2024 · a86da62 · a86da62
1 parent 7d30e47
commit a86da62
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 7 deletions.
diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc
@@ -368,8 +368,11 @@ void JointFeatures::SetJointTransformFromParent(
 
   if (jointInfo->fixedConstraint)
   {
+      auto tf = convertTf(_pose);
       jointInfo->fixedConstraint->setPivotInA(
-        convertVec(_pose.translation()));
+        tf.getOrigin());
+      jointInfo->fixedConstraint->setFrameInA(
+        tf.getBasis());
   }
 }
 

diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc
@@ -1040,8 +1040,8 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetach)
     auto frameDataModel1Body = model1Body->FrameDataRelativeToWorld();
     auto frameDataModel2Body = model2Body->FrameDataRelativeToWorld();
 
-    const gz::math::Pose3d initialModel1Pose(0, 0, 0.25, 0, 0, 0);
-    const gz::math::Pose3d initialModel2Pose(0, 0, 3.0, 0, 0, 0);
+    const gz::math::Pose3d initialModel1Pose(0, 0, 0.25, 0, 0, 0.1);
+    const gz::math::Pose3d initialModel2Pose(0, 0, 3.0, 0, 0, 0.2);
 
     EXPECT_EQ(initialModel1Pose,
               gz::math::eigen3::convert(frameDataModel1Body.pose));
@@ -1133,14 +1133,14 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetach)
 
     // After a while, body2 should reach the ground and come to a stop
     std::size_t stepCount = 0u;
-    const std::size_t maxNumSteps = 1000u;
+    const std::size_t maxNumSteps = 2000u;
     while (stepCount++ < maxNumSteps)
     {
       world->Step(output, state, input);
       frameDataModel2Body = model2Body->FrameDataRelativeToWorld();
       // Expected Z height of model2 is 0.75 when both boxes are stacked on top
       // of each other since each is 0.5 high.
-      if (fabs(frameDataModel2Body.pose.translation().z() - 0.75) < 2e-2 &&
+      if (fabs(frameDataModel2Body.pose.translation().z() - 0.75) < 1e-2 &&
           fabs(frameDataModel2Body.linearVelocity.z()) < 1e-3)
       {
         break;

diff --git a/test/common_test/worlds/joint_across_models.sdf b/test/common_test/worlds/joint_across_models.sdf
@@ -29,7 +29,7 @@
     </model>
 
     <model name="M1">
-      <pose>0 0 0.25 0 0.0 0</pose>
+      <pose>0 0 0.25 0 0.0 0.1</pose>
       <link name="body">
         <inertial>
           <inertia>
@@ -65,7 +65,7 @@
       </link>
     </model>
     <model name="M2">
-      <pose>0 0 3.0 0 0.0 0</pose>
+      <pose>0 0 3.0 0 0.0 0.2</pose>
       <link name="body">
         <inertial>
           <inertia>