Allow to enable/disable static and gravity

include/gz/sim/Model.hh

@@ -187,6 +187,18 @@ namespace gz
       public: void SetWorldPoseCmd(EntityComponentManager &_ecm,
           const math::Pose3d &_pose);
 
+      /// \brief Set a new state to change the model's static.
+      /// \param[in] _ecm Entity-component manager.
+      /// \param[in] _state New model static state.
+      public: void SetStaticStateCmd(EntityComponentManager &_ecm,
+          bool _state);
+
+      /// \brief Set a new state to change the model's gravity.
+      /// \param[in] _ecm Entity-component manager.
+      /// \param[in] _gravity New model state.
+      public: void SetGravityEnabledCmd(EntityComponentManager &_ecm,
+          bool _enabled);
+
       /// \brief Get the model's canonical link entity.
       /// \param[in] _ecm Entity-component manager.
       /// \return Link entity.

include/gz/sim/components/Gravity.hh

@@ -41,6 +41,10 @@ namespace components
   using GravityEnabled = Component<bool, class GravityEnabledTag>;
   GZ_SIM_REGISTER_COMPONENT(
       "gz_sim_components.GravityEnabled", GravityEnabled)
+
+      /// \brief Store the gravity acceleration.
+  using GravityEnabledCmd = Component<bool, class GravityEnabledCmdTag>;
+  GZ_SIM_REGISTER_COMPONENT("gz_sim_components.GravityEnabledCmd", GravityEnabledCmd)
 }
 }
 }

include/gz/sim/components/Static.hh

@@ -33,6 +33,13 @@ namespace components
   /// moveable).
   using Static = Component<bool, class StaticTag>;
   GZ_SIM_REGISTER_COMPONENT("gz_sim_components.Static", Static)
+
+  // \brief A component type that contains the commanded static state of an
+  /// entity represented by bool.
+  using StaticStateCmd = Component<
+    bool, class StaticStateCmdTag>;
+  GZ_SIM_REGISTER_COMPONENT(
+      "gz_sim_components.StaticStateCmd", StaticStateCmd)
 }
 }
 }

src/Model.cc

@@ -16,6 +16,7 @@
 */
 
 #include "gz/sim/components/CanonicalLink.hh"
+#include "gz/sim/components/Gravity.hh"
 #include "gz/sim/components/Joint.hh"
 #include "gz/sim/components/Link.hh"
 #include "gz/sim/components/Model.hh"
@@ -219,6 +220,44 @@ void Model::SetWorldPoseCmd(EntityComponentManager &_ecm,
   }
 }
 
+void Model::SetStaticStateCmd(EntityComponentManager &_ecm,
+    bool _state)
+{
+  std::cout << "SetStaticState " << std::endl;
+  auto staticComp = _ecm.Component<components::StaticStateCmd>(
+      this->dataPtr->id);
+  if (!staticComp)
+  {
+    _ecm.CreateComponent(this->dataPtr->id, components::StaticStateCmd(_state));
+  }
+  else
+  {
+    staticComp->SetData(_state,
+        [](const bool &, const bool &){return false;});
+    _ecm.SetChanged(this->dataPtr->id,
+        components::StaticStateCmd::typeId, ComponentState::OneTimeChange);
+  }
+}
+
+void Model::SetGravityEnabledCmd(EntityComponentManager &_ecm,
+    bool _enabled)
+{
+  std::cout << "SetGravityEnabledCmd " << std::endl;
+  auto staticComp = _ecm.Component<components::GravityEnabledCmd>(
+      this->dataPtr->id);
+  if (!staticComp)
+  {
+    _ecm.CreateComponent(this->dataPtr->id, components::GravityEnabledCmd(_enabled));
+  }
+  else
+  {
+    staticComp->SetData(_enabled,
+        [](const bool &, const bool &){return false;});
+    _ecm.SetChanged(this->dataPtr->id,
+        components::GravityEnabledCmd::typeId, ComponentState::OneTimeChange);
+  }
+}
+
 //////////////////////////////////////////////////
 Entity Model::CanonicalLink(const EntityComponentManager &_ecm) const
 {

src/systems/physics/Physics.cc

@@ -380,6 +380,12 @@ class gz::sim::systems::PhysicsPrivate
   /// deleted the following iteration.
   public: std::unordered_set<Entity> worldPoseCmdsToRemove;
 
+  /// \brief Entities whose static commands have been processed and should be
+  /// deleted the following iteration.
+  public: std::unordered_set<Entity> staticStateCmdsToRemove;
+
+  public: std::unordered_set<Entity> gravityEnabledCmdsToRemove;
+
   /// \brief IDs of the ContactSurfaceHandler callbacks registered for worlds
   public: std::unordered_map<Entity, std::string> worldContactCallbackIDs;
 
@@ -571,6 +577,19 @@ class gz::sim::systems::PhysicsPrivate
             physics::GetModelBoundingBox>{};
 
   //////////////////////////////////////////////////
+  // static
+  /// \brief Feature list for model static state.
+  public: struct StaticStateFeatureList : physics::FeatureList<
+            MinimumFeatureList,
+            physics::SetFreeGroupStaticState>{};
+
+  //////////////////////////////////////////////////
+  // enabled gravity
+  /// \brief Feature list for model static state.
+  public: struct GravityEnabledFeatureList : physics::FeatureList<
+            MinimumFeatureList,
+            physics::SetFreeGroupGravityEnabled>{};
+
   // Link Bounding box
   /// \brief Feature list for model bounding box.
   public: struct LinkBoundingBoxFeatureList : physics::FeatureList<
@@ -754,7 +773,9 @@ class gz::sim::systems::PhysicsPrivate
   public: using EntityFreeGroupMap = EntityFeatureMap3d<
             physics::FreeGroup,
             MinimumFeatureList,
-            WorldVelocityCommandFeatureList
+            WorldVelocityCommandFeatureList,
+            StaticStateFeatureList,
+            GravityEnabledFeatureList
             >;
 
   /// \brief A map between collision entity ids in the ECM to FreeGroup Entities
@@ -2101,6 +2122,30 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm)
 void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
 {
   GZ_PROFILE("PhysicsPrivate::UpdatePhysics");
+
+  // _ecm.Each<components::Model, components::Name, components::Static>(
+  //     [&](const Entity & _entity,
+  //         const components::Model * _model,
+  //         const components::Name * _name,
+  //         const components::Static *_static) -> bool
+  //     {
+  //       bool is_static = this->staticEntities.find(_entity) !=
+  //           this->staticEntities.end();
+  //       if (is_static != _static->Data()) {
+  //         if (is_static) {
+  //           this->staticEntities.erase(_entity);
+  //           gzwarn << "UpdatePhysics static erase" << std::endl;
+  //         } else {
+  //           this->staticEntities.insert(_entity);
+  //           gzwarn << "UpdatePhysics static insert " << std::endl;
+  //           for (auto & lol : this->staticEntities) {
+  //             gzwarn << "lol " << lol << " " << _name->Data() << std::endl;
+  //           }
+  //         }
+  //       }
+  //       return true;
+  //     });
+
   // Battery state
   _ecm.Each<components::BatterySoC>(
       [&](const Entity & _entity, const components::BatterySoC *_bat)
@@ -2411,6 +2456,141 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
         return true;
       });
 
+  // update Static State
+  auto olderStaticStateCmdsToRemove = std::move(this->staticStateCmdsToRemove);
+  this->staticStateCmdsToRemove.clear();
+
+  _ecm.Each<components::Model,
+    components::StaticStateCmd,
+    components::Name>(
+      [&](const Entity &_entity, const components::Model *,
+          const components::StaticStateCmd *_staticSateCmd,
+          const components::Name *_name)->bool
+      {
+        this->staticStateCmdsToRemove.insert(_entity);
+
+        auto modelPtrPhys = this->entityModelMap.Get(_entity);
+        if (nullptr == modelPtrPhys)
+          return true;
+
+        auto freeGroup = modelPtrPhys->FindFreeGroup();
+        if (!freeGroup)
+          return true;
+
+        this->entityFreeGroupMap.AddEntity(_entity, freeGroup);
+
+        auto ssModel =
+            this->entityFreeGroupMap.EntityCast<StaticStateFeatureList>(_entity);
+
+        if (!ssModel)
+        {
+          static bool informed{false};
+          if (!informed)
+          {
+            gzdbg << "Attempting to set a static state, but the physics "
+                   << "engine doesn't support feature "
+                   << "[SetStaticState]. static state won't be populated."
+                   << " " << _name->Data()
+                   << std::endl;
+            informed = true;
+          }
+
+          // Break Each call since no Static state'es can be processed
+          return false;
+        }
+        gzwarn << "_staticSateCmd->Data() " << _staticSateCmd->Data() << std::endl;
+
+        bool is_static = this->staticEntities.find(_entity) !=
+            this->staticEntities.end();
+        if (is_static != _staticSateCmd->Data()) {
+          if (is_static) {
+            this->staticEntities.erase(_entity);
+            gzwarn << "UpdatePhysics static erase " << _name->Data() << std::endl;
+          } else {
+            this->staticEntities.insert(_entity);
+            gzwarn << "UpdatePhysics static insert " << _name->Data() << std::endl;
+            // for (auto & lol : this->staticEntities) {
+            //   gzwarn << "lol " << lol << " " << _name->Data() << std::endl;
+            // }
+          }
+        }
+
+        // auto modelPtrPhys = this->entityModelMap.Get(_entity);
+        // if (nullptr == modelPtrPhys)
+        //   return true;
+
+        // // TODO(addisu) Store the free group instead of searching for it at
+        // // every iteration
+        // auto freeGroup = modelPtrPhys->FindFreeGroup();
+        // if (!freeGroup)
+        //   return true;
+
+        ssModel->SetStaticState(_staticSateCmd->Data());
+        return true;
+      });
+
+  // Remove world commands from previous iteration. We let them rotate one
+  // iteration so other systems have a chance to react to them too.
+  for (const Entity &entity : olderStaticStateCmdsToRemove)
+  {
+    _ecm.RemoveComponent<components::StaticStateCmd>(entity);
+  }
+
+  // update Gravity enabled
+  auto olderGravityEnabledCmdsToRemove = std::move(this->gravityEnabledCmdsToRemove);
+  this->gravityEnabledCmdsToRemove.clear();
+
+  _ecm.Each<components::Model,
+    components::GravityEnabledCmd,
+    components::Name>(
+      [&](const Entity &_entity, const components::Model *,
+          const components::GravityEnabledCmd *_gravityEnabledCmd,
+          const components::Name *_name)->bool
+      {
+        this->gravityEnabledCmdsToRemove.insert(_entity);
+
+        auto modelPtrPhys = this->entityModelMap.Get(_entity);
+        if (nullptr == modelPtrPhys)
+          return true;
+
+        auto freeGroup = modelPtrPhys->FindFreeGroup();
+        if (!freeGroup)
+          return true;
+
+        this->entityFreeGroupMap.AddEntity(_entity, freeGroup);
+
+        auto ssModel =
+            this->entityFreeGroupMap.EntityCast<GravityEnabledFeatureList>(_entity);
+
+        if (!ssModel)
+        {
+          static bool informed{false};
+          if (!informed)
+          {
+            gzdbg << "Attempting to set a static state, but the physics "
+                   << "engine doesn't support feature "
+                   << "[SetGravityEnabled]. static state won't be populated."
+                   << " " << _name->Data()
+                   << std::endl;
+            informed = true;
+          }
+
+          // Break Each call since no Static state'es can be processed
+          return false;
+        }
+        gzwarn << "_gravityEnabledCmd->Data() " << _gravityEnabledCmd->Data() << std::endl;
+
+        ssModel->SetGravityEnabled(_gravityEnabledCmd->Data());
+        return true;
+      });
+
+  // Remove world commands from previous iteration. We let them rotate one
+  // iteration so other systems have a chance to react to them too.
+  for (const Entity &entity : olderGravityEnabledCmdsToRemove)
+  {
+    _ecm.RemoveComponent<components::GravityEnabledCmd>(entity);
+  }
+
   // Update model pose
   auto olderWorldPoseCmdsToRemove = std::move(this->worldPoseCmdsToRemove);
   this->worldPoseCmdsToRemove.clear();
@@ -2743,6 +2923,7 @@ void PhysicsPrivate::ResetPhysics(EntityComponentManager &_ecm)
   this->canonicalLinkModelTracker = CanonicalLinkModelTracker();
   this->modelWorldPoses.clear();
   this->worldPoseCmdsToRemove.clear();
+  this->staticStateCmdsToRemove.clear();
 
   this->RemovePhysicsEntities(_ecm);
   this->CreatePhysicsEntities(_ecm, false);