diff --git a/include/gz/sim/Model.hh b/include/gz/sim/Model.hh index ddb0e8130d..dd7a511598 100644 --- a/include/gz/sim/Model.hh +++ b/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. diff --git a/include/gz/sim/components/Gravity.hh b/include/gz/sim/components/Gravity.hh index a4b8ac352d..f159e89ce7 100644 --- a/include/gz/sim/components/Gravity.hh +++ b/include/gz/sim/components/Gravity.hh @@ -41,6 +41,10 @@ namespace components using GravityEnabled = Component; GZ_SIM_REGISTER_COMPONENT( "gz_sim_components.GravityEnabled", GravityEnabled) + + /// \brief Store the gravity acceleration. + using GravityEnabledCmd = Component; + GZ_SIM_REGISTER_COMPONENT("gz_sim_components.GravityEnabledCmd", GravityEnabledCmd) } } } diff --git a/include/gz/sim/components/Static.hh b/include/gz/sim/components/Static.hh index 6bbff98b30..4b2a9f723c 100644 --- a/include/gz/sim/components/Static.hh +++ b/include/gz/sim/components/Static.hh @@ -33,6 +33,13 @@ namespace components /// moveable). using Static = Component; 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) } } } diff --git a/src/Model.cc b/src/Model.cc index 01f4538fb7..02e598bf38 100644 --- a/src/Model.cc +++ b/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( + 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( + 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 { diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index dcce39ef4a..206f807c5c 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -380,6 +380,12 @@ class gz::sim::systems::PhysicsPrivate /// deleted the following iteration. public: std::unordered_set worldPoseCmdsToRemove; + /// \brief Entities whose static commands have been processed and should be + /// deleted the following iteration. + public: std::unordered_set staticStateCmdsToRemove; + + public: std::unordered_set gravityEnabledCmdsToRemove; + /// \brief IDs of the ContactSurfaceHandler callbacks registered for worlds public: std::unordered_map 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( + // [&](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( [&](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( + [&](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(_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(entity); + } + + // update Gravity enabled + auto olderGravityEnabledCmdsToRemove = std::move(this->gravityEnabledCmdsToRemove); + this->gravityEnabledCmdsToRemove.clear(); + + _ecm.Each( + [&](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(_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(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);