Enable/disable: gravity and static

dartsim/src/FreeGroupFeatures.cc

@@ -146,6 +146,52 @@ FreeGroupFeatures::FreeGroupInfo FreeGroupFeatures::GetCanonicalInfo(
   return FreeGroupInfo{this->links.at(_groupID)->link, nullptr};
 }
 
+/////////////////////////////////////////////////
+void FreeGroupFeatures::SetFreeGroupStaticState(
+    const Identity &_groupID,
+    bool _state)
+{
+  const auto modelInfo = this->models.at(_groupID);
+  // Verify that the model qualifies as a FreeGroup
+  dart::dynamics::SkeletonPtr &skeleton = modelInfo->model;
+
+  std::cout << "SetFreeGroupStaticState " << std::endl;
+
+  if (skeleton) {
+    skeleton->setMobile(!_state);
+  } else {
+  }
+}
+
+/////////////////////////////////////////////////
+void FreeGroupFeatures::SetFreeGroupGravityEnabled(
+    const Identity &_groupID,
+    bool _enabled)
+{
+  const FreeGroupInfo &info = GetCanonicalInfo(_groupID);
+  if (!info.model)
+  {
+    info.link->setGravityMode(_enabled);
+    return;
+  }
+
+  for (std::size_t i = 0; i < info.model->getNumTrees(); ++i)
+  {
+    auto *bn = info.model->getRootBodyNode(i);
+    bn->setGravityMode(_enabled);
+  }
+  // const auto modelInfo = this->models.at(_groupID);
+  // // Verify that the model qualifies as a FreeGroup
+  // dart::dynamics::SkeletonPtr &skeleton = modelInfo->model;
+
+  // std::cout << "SetFreeGroupStaticState " << std::endl;
+
+  // if (skeleton) {
+  //   skeleton->setGravity(_gravity);
+  // } else {
+  // }
+}
+
 /////////////////////////////////////////////////
 void FreeGroupFeatures::SetFreeGroupWorldPose(
     const Identity &_groupID,

dartsim/src/FreeGroupFeatures.hh

@@ -29,7 +29,9 @@ namespace dartsim {
 struct FreeGroupFeatureList : FeatureList<
   FindFreeGroupFeature,
   SetFreeGroupWorldPose,
-  SetFreeGroupWorldVelocity
+  SetFreeGroupWorldVelocity,
+  SetFreeGroupStaticState,
+  SetFreeGroupGravityEnabled
   // Note: FreeGroupFrameSemantics is covered in KinematicsFeatures.hh
 > { };
 
@@ -60,6 +62,14 @@ class FreeGroupFeatures
       const Identity &_groupID,
       const PoseType &_pose) override;
 
+  void SetFreeGroupStaticState(
+      const Identity &_groupID,
+      bool _state) override;
+
+  void SetFreeGroupGravityEnabled(
+      const Identity &_groupID,
+      bool _enabled) override;
+
   void SetFreeGroupWorldLinearVelocity(
       const Identity &_groupID,
       const LinearVelocity &_linearVelocity) override;

include/gz/physics/FreeGroup.hh

@@ -161,6 +161,58 @@ namespace gz
       };
     };
 
+    /////////////////////////////////////////////////
+    /// \brief This features sets the FreeGroup static state.
+    class GZ_PHYSICS_VISIBLE SetFreeGroupStaticState
+        : public virtual FeatureWithRequirements<FindFreeGroupFeature>
+    {
+      /// \brief This class defines the FreeGroup concept, which represents a
+      /// group of links that are not connected to the world with any kinematic
+      /// constraints. This class also provides a rough definition of this
+      /// FreeGroup pose in world frame. See FindFreeGroupFeature class
+      /// documentation for more detail.
+      public: template <typename PolicyT, typename FeaturesT>
+      class FreeGroup : public virtual Entity<PolicyT, FeaturesT>
+      {
+        /// \brief Set this FreeGroup pose in world frame.
+        public: void SetStaticState(bool _state);
+      };
+
+      public: template <typename PolicyT>
+      class Implementation : public virtual Feature::Implementation<PolicyT>
+      {
+        public: virtual void SetFreeGroupStaticState(
+            const Identity &_groupID,
+            bool _state) = 0;
+      };
+    };
+
+    /////////////////////////////////////////////////
+    /// \brief This features sets the FreeGroup gravity.
+    class GZ_PHYSICS_VISIBLE SetFreeGroupGravityEnabled
+        : public virtual FeatureWithRequirements<FindFreeGroupFeature>
+    {
+      /// \brief This class defines the FreeGroup concept, which represents a
+      /// group of links that are not connected to the world with any kinematic
+      /// constraints. This class also provides a rough definition of this
+      /// FreeGroup pose in world frame. See FindFreeGroupFeature class
+      /// documentation for more detail.
+      public: template <typename PolicyT, typename FeaturesT>
+      class FreeGroup : public virtual Entity<PolicyT, FeaturesT>
+      {
+        /// \brief Set this FreeGroup pose in world frame.
+        public: void SetGravityEnabled(bool _enabled);
+      };
+
+      public: template <typename PolicyT>
+      class Implementation : public virtual Feature::Implementation<PolicyT>
+      {
+        public: virtual void SetFreeGroupGravityEnabled(
+            const Identity &_groupID,
+            bool _enabled) = 0;
+      };
+    };
+
     /////////////////////////////////////////////////
     /// \brief This features sets the FreeGroup linear and angular velocity in
     /// world frame.

include/gz/physics/detail/FreeGroup.hh

@@ -109,6 +109,24 @@ namespace gz
         ->SetFreeGroupWorldPose(this->identity, _pose);
     }
 
+    /////////////////////////////////////////////////
+    template <typename PolicyT, typename FeaturesT>
+    void SetFreeGroupStaticState::FreeGroup<PolicyT, FeaturesT>::SetStaticState(
+        bool _state)
+    {
+      this->template Interface<SetFreeGroupStaticState>()
+        ->SetFreeGroupStaticState(this->identity, _state);
+    }
+
+    /////////////////////////////////////////////////
+    template <typename PolicyT, typename FeaturesT>
+    void SetFreeGroupGravityEnabled::FreeGroup<PolicyT, FeaturesT>::
+    SetGravityEnabled(bool _enabled)
+    {
+      this->template Interface<SetFreeGroupGravityEnabled>()
+        ->SetFreeGroupGravityEnabled(this->identity, _enabled);
+    }
+
     /////////////////////////////////////////////////
     template <typename PolicyT, typename FeaturesT>
     void SetFreeGroupWorldVelocity::FreeGroup<PolicyT, FeaturesT>::

test/common_test/Worlds.hh

@@ -56,6 +56,7 @@ const auto kShapesWorld = CommonTestWorld("shapes.world");
 const auto kShapesBitmaskWorld = CommonTestWorld("shapes_bitmask.sdf");
 const auto kSlipComplianceSdf = CommonTestWorld("slip_compliance.sdf");
 const auto kSphereSdf = CommonTestWorld("sphere.sdf");
+const auto kSphereGravitySdf = CommonTestWorld("sphere_gravity.sdf");
 const auto kStringPendulumSdf = CommonTestWorld("string_pendulum.sdf");
 const auto kTestWorld = CommonTestWorld("test.world");
 const auto kWorldJointTestSdf = CommonTestWorld("world_joint_test.sdf");

test/common_test/simulation_features.cc

@@ -967,6 +967,136 @@ TYPED_TEST(SimulationFeaturesTestFreeGroup, FreeGroup)
   }
 }
 
+struct FreeGroupPhysicsFeatures : gz::physics::FeatureList<
+  gz::physics::FindFreeGroupFeature,
+  gz::physics::SetFreeGroupWorldPose,
+  gz::physics::SetFreeGroupWorldVelocity,
+  gz::physics::SetFreeGroupStaticState,
+  gz::physics::SetFreeGroupGravityEnabled,
+
+  gz::physics::GetModelFromWorld,
+  gz::physics::GetLinkFromModel,
+
+  gz::physics::FreeGroupFrameSemantics,
+  gz::physics::LinkFrameSemantics,
+
+  gz::physics::sdf::ConstructSdfWorld,
+
+  gz::physics::ForwardStep
+> {};
+
+template <class T>
+class SimulationFeaturesTestFreeGroupPhysics :
+  public SimulationFeaturesTest<T>{};
+using SimulationFeaturesTestFreeGroupPhysicsTypes =
+  ::testing::Types<FreeGroupFeatures>;
+TYPED_TEST_SUITE(SimulationFeaturesTestFreeGroupPhysics,
+                 SimulationFeaturesTestFreeGroupPhysicsTypes);
+
+TYPED_TEST(SimulationFeaturesTestFreeGroupPhysics, FreeGroup)
+{
+  for (const std::string &name : this->pluginNames)
+  {
+    CHECK_UNSUPPORTED_ENGINE(name, "tpe")
+    CHECK_UNSUPPORTED_ENGINE(name, "bullet-featherstone")
+
+    auto world = LoadPluginAndWorld<FreeGroupPhysicsFeatures>(
+      this->loader,
+      name,
+      common_test::worlds::kSphereGravitySdf);
+
+    // model free group test
+    auto model = world->GetModel("sphere");
+    auto freeGroup = model->FindFreeGroup();
+    ASSERT_NE(nullptr, freeGroup);
+    GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
+    ASSERT_NE(nullptr, freeGroup->CanonicalLink());
+    GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
+    ASSERT_NE(nullptr, freeGroup->RootLink());
+
+    auto link = model->GetLink("sphere_link");
+    auto freeGroupLink = link->FindFreeGroup();
+    ASSERT_NE(nullptr, freeGroupLink);
+
+    // Disable gravity.
+    freeGroup->SetGravityEnabled(false);
+
+    StepWorld<FreeGroupFeatures>(world, true);
+
+    // Set initial pose.
+    const gz::math::Pose3d initialPose{0, 0, 2, 0, 0, 0};
+    freeGroup->SetWorldPose(
+      gz::math::eigen3::convert(initialPose));
+
+    auto freeGroupFrameData = freeGroup->FrameDataRelativeToWorld();
+    auto linkFrameData = model->GetLink(0)->FrameDataRelativeToWorld();
+
+    // Before stepping, check that pose matches what was set.
+    EXPECT_EQ(initialPose,
+              gz::math::eigen3::convert(freeGroupFrameData.pose));
+    EXPECT_EQ(initialPose,
+              gz::math::eigen3::convert(linkFrameData.pose));
+
+    // Step the world
+    StepWorld<FreeGroupFeatures>(world, false, 1000);
+
+    // The sphere is in the same position gravity is not working
+    freeGroupFrameData = freeGroup->FrameDataRelativeToWorld();
+    linkFrameData = model->GetLink(0)->FrameDataRelativeToWorld();
+
+    EXPECT_EQ(initialPose,
+              gz::math::eigen3::convert(freeGroupFrameData.pose));
+    EXPECT_EQ(initialPose,
+              gz::math::eigen3::convert(linkFrameData.pose));
+
+    // Enable gravity
+    freeGroup->SetGravityEnabled(true);
+
+    // Step the world
+    StepWorld<FreeGroupFeatures>(world, false, 1000);
+
+    // The sphere is not in the same position gravity is working
+    freeGroupFrameData = freeGroup->FrameDataRelativeToWorld();
+    linkFrameData = model->GetLink(0)->FrameDataRelativeToWorld();
+
+    EXPECT_NE(initialPose,
+              gz::math::eigen3::convert(freeGroupFrameData.pose));
+    EXPECT_NE(initialPose,
+              gz::math::eigen3::convert(linkFrameData.pose));
+
+    // Now the model is static
+    freeGroup->SetWorldPose(
+      gz::math::eigen3::convert(initialPose));
+    freeGroup->SetStaticState(true);
+
+    freeGroupFrameData = freeGroup->FrameDataRelativeToWorld();
+    linkFrameData = model->GetLink(0)->FrameDataRelativeToWorld();
+
+    // Gravity is enabled and we also set some velocities.
+    const Eigen::Vector3d initialLinearVelocity{0.1, 0.2, 0.3};
+    const Eigen::Vector3d initialAngularVelocity{0.4, 0.5, 0.6};
+    freeGroup->SetWorldLinearVelocity(initialLinearVelocity);
+    freeGroup->SetWorldAngularVelocity(initialAngularVelocity);
+
+    EXPECT_EQ(initialPose,
+              gz::math::eigen3::convert(freeGroupFrameData.pose));
+    EXPECT_EQ(initialPose,
+              gz::math::eigen3::convert(linkFrameData.pose));
+
+    // Step the world
+    StepWorld<FreeGroupFeatures>(world, false, 1000);
+
+    // sphere should be in the same position
+    freeGroupFrameData = freeGroup->FrameDataRelativeToWorld();
+    linkFrameData = model->GetLink(0)->FrameDataRelativeToWorld();
+
+    EXPECT_EQ(initialPose,
+              gz::math::eigen3::convert(freeGroupFrameData.pose));
+    EXPECT_EQ(initialPose,
+              gz::math::eigen3::convert(linkFrameData.pose));
+  }
+}
+
 template <class T>
 class SimulationFeaturesTestBasic :
   public SimulationFeaturesTest<T>{};

test/common_test/worlds/sphere_gravity.sdf

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <world name="empty">
+    <gravity>0 0 -10</gravity>
+    <model name="sphere">
+      <pose>0 0 2 0 0 3.1416</pose>
+      <link name="sphere_link">
+        <inertial>
+          <inertia>
+            <ixx>0.4</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.4</iyy>
+            <iyz>0</iyz>
+            <izz>0.4</izz>
+          </inertia>
+          <mass>1.0</mass>
+        </inertial>
+        <collision name="sphere_collision">
+          <geometry>
+            <sphere>
+              <radius>1</radius>
+            </sphere>
+          </geometry>
+        </collision>
+        <visual name="sphere_visual">
+          <geometry>
+            <sphere>
+              <radius>1</radius>
+            </sphere>
+          </geometry>
+        </visual>
+      </link>
+    </model>
+  </world>
+</sdf>