From fc4325045fec6c822f82866db09223e78e31b426 Mon Sep 17 00:00:00 2001
From: youhy <haoyuan2019@outlook.com>
Date: Tue, 7 May 2024 23:44:45 -0500
Subject: [PATCH] add no gravity link support and test for DartSim

---
 dartsim/src/SDFFeatures.cc        |  5 +++++
 dartsim/src/WorldFeatures_TEST.cc | 12 ++++++++++++
 dartsim/worlds/falling.world      | 12 ++++++++++++
 3 files changed, 29 insertions(+)

diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc
index 62137286e..da0f01d11 100644
--- 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
index 0126e2621..25ef17243 100644
--- 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
index 3bae3f366..881daa5b9 100644
--- 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>