add no gravity link support and test for DartSim

gazebosim · May 8, 2024 · fc43250 · fc43250
1 parent 7ff508d
commit fc43250
Show file tree

Hide file tree

Showing 3 changed files with 29 additions and 0 deletions.
diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc
@@ -600,6 +600,11 @@ Identity SDFFeatures::ConstructSdfLink(
 
   bodyProperties.mInertia.setLocalCOM(localCom);
 
+  // ignmsg << "SDF gravity status = " << _sdfLink.EnableGravity() << std::endl;
+
+  bodyProperties.mGravityMode = _sdfLink.EnableGravity();
+  // ignmsg << "gravity in dart set to: " << bodyProperties.mGravityMode << std::endl;
+
   dart::dynamics::FreeJoint::Properties jointProperties;
   jointProperties.mName = bodyProperties.mName + "_FreeJoint";
   // TODO(MXG): Consider adding a UUID to this joint name in order to avoid any

diff --git a/dartsim/src/WorldFeatures_TEST.cc b/dartsim/src/WorldFeatures_TEST.cc
@@ -140,6 +140,12 @@ TEST_F(WorldFeaturesFixture, Gravity)
   auto link = model->GetLink(0);
   ASSERT_NE(nullptr, link);
 
+  auto modelNoGravity = world->GetModel("sphere_no_gravity");
+  ASSERT_NE(nullptr, modelNoGravity);
+
+  auto linkNoGravity = modelNoGravity->GetLink(0);
+  ASSERT_NE(nullptr, linkNoGravity);
+
   // initial link pose
   const Eigen::Vector3d initialLinkPosition(0, 0, 2);
   {
@@ -190,6 +196,12 @@ TEST_F(WorldFeaturesFixture, Gravity)
     EXPECT_PRED_FORMAT2(vectorPredicate3,
                         Eigen::Vector3d(0.5, 0, 2.5),
                         pos);
+    // pose for link without gravity should not change
+    Eigen::Vector3d posNoGravity = linkNoGravity->FrameDataRelativeToWorld()
+                        .pose.translation();
+    EXPECT_PRED_FORMAT2(vectorPredicate3,
+                        Eigen::Vector3d(10, 10, 10),
+                        pos);
   }
 }
 

diff --git a/dartsim/worlds/falling.world b/dartsim/worlds/falling.world
@@ -71,5 +71,17 @@
         </visual>
       </link>
     </model>
+    <model name='sphere_no_gravity'>
+      <link name='link'>
+        <gravity>false</gravity>
+        <pose>10 10 10 0 0 0</pose>
+        <visual name='visual'>
+          <geometry><sphere><radius>1</radius></sphere></geometry>
+        </visual>
+        <collision name='collision'>
+          <geometry><sphere><radius>1</radius></sphere></geometry>
+        </collision>
+      </link>
+    </model>
   </world>
 </sdf>