Enable/disable: gravity and static

dartsim/src/FreeGroupFeatures.cc

@@ -146,6 +146,130 @@ 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;
+
+  if (skeleton)
+  {
+    skeleton->setMobile(!_state);
+  }
+
+  // Recursively set static state for all nested models
+  for (const auto &nestedModel : modelInfo->nestedModels)
+  {
+    auto nestedModelIdentity = this->GenerateIdentity(nestedModel);
+    const auto &nestedModelInfo = this->models.at(nestedModel);
+    // Only set static state if the nested model has BodyNodes or
+    // further nested models
+    if (nestedModelInfo->model->getNumBodyNodes() > 0 ||
+        !nestedModelInfo->nestedModels.empty())
+    {
+      this->SetFreeGroupStaticState(nestedModelIdentity, _state);
+    }
+  }
+}
+
+/////////////////////////////////////////////////
+bool FreeGroupFeatures::GetFreeGroupStaticState(
+    const Identity &_groupID) const
+{
+  const auto modelInfo = this->models.at(_groupID);
+  // Verify that the model qualifies as a FreeGroup
+  dart::dynamics::SkeletonPtr &skeleton = modelInfo->model;
+  bool isStatic = false;
+  if (skeleton)
+  {
+    isStatic = !skeleton->isMobile();
+  }
+
+  for (const auto &nestedModel : modelInfo->nestedModels)
+  {
+    auto nestedIdentity = this->GenerateIdentity(nestedModel);
+    const auto &nestedModelInfo = this->models.at(nestedModel);
+    if (nestedModelInfo->model->getNumBodyNodes() > 0 ||
+        !nestedModelInfo->nestedModels.empty())
+    {
+      isStatic = isStatic &&
+        this->GetFreeGroupStaticState(nestedIdentity);
+    }
+  }
+
+  return isStatic;
+}
+
+/////////////////////////////////////////////////
+void FreeGroupFeatures::SetFreeGroupGravityEnabled(
+    const Identity &_groupID,
+    bool _enabled)
+{
+  const FreeGroupInfo &info = GetCanonicalInfo(_groupID);
+  if (!info.model)
+  {
+    if (info.link)
+      info.link->setGravityMode(_enabled);
+    return;
+  }
+  // Set gravity for all BodyNodes in the skeleton (not just roots)
+  for (std::size_t i = 0; i < info.model->getNumBodyNodes(); ++i)
+  {
+    auto *bn = info.model->getBodyNode(i);
+    if (bn)
+      bn->setGravityMode(_enabled);
+  }
+
+  auto modelInfo = this->models.at(_groupID);
+  for (const auto &nestedModel : modelInfo->nestedModels)
+  {
+    auto nestedModelIdentity = this->GenerateIdentity(nestedModel);
+    const FreeGroupInfo &nestedInfo = GetCanonicalInfo(nestedModelIdentity);
+    // If nestedInfo.link is a nullptr, we skip this model because means the
+    // BodyNodes in this skeleton have been moved to another skeleton and their
+    // pose update will be handled there.
+    if (nullptr != nestedInfo.link && nestedInfo.link != info.link)
+    {
+      this->SetFreeGroupGravityEnabled(nestedModelIdentity, _enabled);
+    }
+  }
+}
+
+/////////////////////////////////////////////////
+bool FreeGroupFeatures::GetFreeGroupGravityEnabled(
+    const Identity &_groupID) const
+{
+  const FreeGroupInfo &info = GetCanonicalInfo(_groupID);
+  if (!info.model)
+  {
+    return info.link->getGravityMode();
+  }
+
+  bool gravityMode = true;
+  for (std::size_t i = 0; i < info.model->getNumTrees(); ++i)
+  {
+    auto *bn = info.model->getRootBodyNode(i);
+    gravityMode = gravityMode && bn->getGravityMode();
+  }
+
+  auto modelInfo = this->models.at(_groupID);
+  for (const auto &nestedModel : modelInfo->nestedModels)
+  {
+    auto nestedModelIdentity = this->GenerateIdentity(nestedModel);
+    const FreeGroupInfo &nestedInfo = GetCanonicalInfo(nestedModelIdentity);
+    if (nullptr != nestedInfo.link && nestedInfo.link != info.link)
+    {
+      gravityMode = gravityMode &&
+        this->GetFreeGroupGravityEnabled(nestedModelIdentity);
+    }
+  }
+
+  return gravityMode;
+}
+
 /////////////////////////////////////////////////
 void FreeGroupFeatures::SetFreeGroupWorldPose(
     const Identity &_groupID,

dartsim/src/FreeGroupFeatures.hh

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

include/gz/physics/FreeGroup.hh

@@ -161,6 +161,124 @@ namespace gz
       };
     };
 
+    /////////////////////////////////////////////////
+    /// \brief This features sets the FreeGroup static state.
+    /// If set to true, the model is immovable; i.e., a dynamics engine
+    /// will not update its position.
+    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 static state.
+        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 gets the FreeGroup static state.
+    /// If return true, the model is immovable; i.e., a dynamics engine
+    /// will not update its position.
+    class GZ_PHYSICS_VISIBLE GetFreeGroupStaticState
+        : 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 Get this FreeGroup static state.
+        public: bool GetStaticState() const;
+      };
+
+      /// \brief Implementation interface for the FreeGroup feature.
+      ///
+      /// This class defines the interface that physics engine plugins must
+      /// implement to support the FreeGroup feature functionality.
+      public: template <typename PolicyT>
+      class Implementation : public virtual Feature::Implementation<PolicyT>
+      {
+        public: virtual bool GetFreeGroupStaticState(
+            const Identity &_groupID) const = 0;
+      };
+    };
+
+    /////////////////////////////////////////////////
+    /// \brief This features sets the FreeGroup gravity.
+    /// If set to true, the model is affected by gravity, otherwise the model
+    /// is not affected by the 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 Gravity enabled.
+        public: void SetGravityEnabled(bool _enabled);
+      };
+
+      /// \brief Implementation interface for the FreeGroup feature.
+      ///
+      /// This class defines the interface that physics engine plugins must
+      /// implement to support the FreeGroup feature functionality.
+      public: template <typename PolicyT>
+      class Implementation : public virtual Feature::Implementation<PolicyT>
+      {
+        public: virtual void SetFreeGroupGravityEnabled(
+            const Identity &_groupID,
+            bool _enabled) = 0;
+      };
+    };
+
+    /////////////////////////////////////////////////
+    /// \brief This features gets the FreeGroup gravity.
+    /// If return true, the model is affected by gravity, otherwise the model
+    /// is not affected by the gravity.
+    class GZ_PHYSICS_VISIBLE GetFreeGroupGravityEnabled
+        : 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 Get this FreeGroup Gravity enabled.
+        public: bool GetGravityEnabled() const;
+      };
+
+      public: template <typename PolicyT>
+      class Implementation : public virtual Feature::Implementation<PolicyT>
+      {
+        public: virtual bool GetFreeGroupGravityEnabled(
+            const Identity &_groupID) const = 0;
+      };
+    };
+
     /////////////////////////////////////////////////
     /// \brief This features sets the FreeGroup linear and angular velocity in
     /// world frame.

include/gz/physics/detail/FreeGroup.hh

@@ -109,6 +109,42 @@ 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>
+    bool GetFreeGroupStaticState::FreeGroup<PolicyT, FeaturesT>::
+    GetStaticState() const
+    {
+      return this->template Interface<GetFreeGroupStaticState>()
+        ->GetFreeGroupStaticState(this->identity);
+    }
+
+    /////////////////////////////////////////////////
+    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>
+    bool GetFreeGroupGravityEnabled::FreeGroup<PolicyT, FeaturesT>::
+    GetGravityEnabled() const
+    {
+      return this->template Interface<GetFreeGroupGravityEnabled>()
+        ->GetFreeGroupGravityEnabled(this->identity);
+    }
+
     /////////////////////////////////////////////////
     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");