From 77f4c00308402f816fb2d253318b5626aea7911a Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 9 Apr 2024 12:08:36 -0700 Subject: [PATCH] Physics: remove *VelocityCmd at each time step (#2228) This implements a suggestion from #1926 to delete all `*VelocityCmd` components after each time step. This also updates the logic for the following two systems to handle this change: * MulticopterMotorModel: handle missing component Since JointVelocityCmd components are deleted after each timestep, don't skip updating forces and moments if the component does not exist, and use the SetComponent API to more cleanly handle the component creation logic. A syntax error in the the quadcopter test worlds was fixed as well. * TrajectoryFollower: don't need to remove commands Now that the physics system is removing AngularVelocityCmd components at every timestep, that logic can be removed from the trajectory follower system. Signed-off-by: Steve Peters --- Migration.md | 8 +++ .../MulticopterMotorModel.cc | 17 ++--- src/systems/physics/Physics.cc | 62 +++++++++++++------ .../trajectory_follower/TrajectoryFollower.cc | 24 +------ test/worlds/quadcopter.sdf | 8 +-- test/worlds/quadcopter_velocity_control.sdf | 8 +-- 6 files changed, 66 insertions(+), 61 deletions(-) diff --git a/Migration.md b/Migration.md index a1df392c5c..2e55298ea9 100644 --- a/Migration.md +++ b/Migration.md @@ -5,6 +5,14 @@ Deprecated code produces compile-time warnings. These warning serve as notification to users that their code should be upgraded. The next major release will remove the deprecated code. +## Gazebo Sim 8.x to 9.0 + + * **Modified**: + + In the Physics system, all `*VelocityCmd` components are now deleted after + each time step, whereas previously the component values were set to `0` + after each time step. Persistent velocity commands should be reapplied at + each time step. + ## Gazebo Sim 7.x to 8.0 * **Deprecated** + `gz::sim::components::Factory::Register(const std::string &_type, ComponentDescriptorBase *_compDesc)` and diff --git a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc index 8e6e421ce9..12dcab4c86 100644 --- a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc +++ b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc @@ -447,14 +447,6 @@ void MulticopterMotorModel::PreUpdate(const UpdateInfo &_info, doUpdateForcesAndMoments = false; } - if (!_ecm.Component( - this->dataPtr->jointEntity)) - { - _ecm.CreateComponent(this->dataPtr->jointEntity, - components::JointVelocityCmd({0})); - doUpdateForcesAndMoments = false; - } - if (!_ecm.Component(this->dataPtr->linkEntity)) { _ecm.CreateComponent(this->dataPtr->linkEntity, components::WorldPose()); @@ -682,11 +674,10 @@ void MulticopterMotorModelPrivate::UpdateForcesAndMoments( refMotorRotVel = this->rotorVelocityFilter->UpdateFilter( this->refMotorInput, this->samplingTime); - const auto jointVelCmd = _ecm.Component( - this->jointEntity); - *jointVelCmd = components::JointVelocityCmd( - {this->turningDirection * refMotorRotVel - / this->rotorVelocitySlowdownSim}); + _ecm.SetComponentData( + this->jointEntity, + {this->turningDirection * refMotorRotVel + / this->rotorVelocitySlowdownSim}); } } } diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 1f6defbf44..6a56cb101c 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -3628,12 +3628,20 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, return true; }); - _ecm.Each( - [&](const Entity &, components::JointVelocityCmd *_vel) -> bool - { - std::fill(_vel->Data().begin(), _vel->Data().end(), 0.0); - return true; - }); + { + std::vector entitiesJointVelocityCmd; + _ecm.Each( + [&](const Entity &_entity, components::JointVelocityCmd *) -> bool + { + entitiesJointVelocityCmd.push_back(_entity); + return true; + }); + + for (const auto entity : entitiesJointVelocityCmd) + { + _ecm.RemoveComponent(entity); + } + } _ecm.Each( [&](const Entity &, components::SlipComplianceCmd *_slip) -> bool @@ -3641,21 +3649,37 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, std::fill(_slip->Data().begin(), _slip->Data().end(), 0.0); return true; }); - GZ_PROFILE_END(); - _ecm.Each( - [&](const Entity &, components::AngularVelocityCmd *_vel) -> bool - { - _vel->Data() = math::Vector3d::Zero; - return true; - }); + { + std::vector entitiesAngularVelocityCmd; + _ecm.Each( + [&](const Entity &_entity, components::AngularVelocityCmd *) -> bool + { + entitiesAngularVelocityCmd.push_back(_entity); + return true; + }); - _ecm.Each( - [&](const Entity &, components::LinearVelocityCmd *_vel) -> bool - { - _vel->Data() = math::Vector3d::Zero; - return true; - }); + for (const auto entity : entitiesAngularVelocityCmd) + { + _ecm.RemoveComponent(entity); + } + } + + { + std::vector entitiesLinearVelocityCmd; + _ecm.Each( + [&](const Entity &_entity, components::LinearVelocityCmd *) -> bool + { + entitiesLinearVelocityCmd.push_back(_entity); + return true; + }); + + for (const auto entity : entitiesLinearVelocityCmd) + { + _ecm.RemoveComponent(entity); + } + } + GZ_PROFILE_END(); // Update joint positions GZ_PROFILE_BEGIN("Joints"); diff --git a/src/systems/trajectory_follower/TrajectoryFollower.cc b/src/systems/trajectory_follower/TrajectoryFollower.cc index 1db5e1c2d0..e295686946 100644 --- a/src/systems/trajectory_follower/TrajectoryFollower.cc +++ b/src/systems/trajectory_follower/TrajectoryFollower.cc @@ -111,9 +111,6 @@ class gz::sim::systems::TrajectoryFollowerPrivate /// \brief Whether the trajectory follower behavior should be paused or not. public: bool paused = false; - /// \brief Angular velocity set to zero - public: bool zeroAngVelSet = false; - /// \brief Force angular velocity to be zero when bearing is reached public: bool forceZeroAngVel = false; }; @@ -390,37 +387,22 @@ void TrajectoryFollower::PreUpdate( // Transform the force and torque to the world frame. // Move commands. The vehicle always move forward (X direction). gz::math::Vector3d forceWorld; + gz::math::Vector3d torqueWorld; if (std::abs(bearing.Degree()) <= this->dataPtr->bearingTolerance) { forceWorld = (*comPose).Rot().RotateVector( gz::math::Vector3d(this->dataPtr->forceToApply, 0, 0)); // force angular velocity to be zero when bearing is reached - if (this->dataPtr->forceZeroAngVel && !this->dataPtr->zeroAngVelSet && + if (this->dataPtr->forceZeroAngVel && math::equal (std::abs(bearing.Degree()), 0.0, this->dataPtr->bearingTolerance * 0.5)) { this->dataPtr->link.SetAngularVelocity(_ecm, math::Vector3d::Zero); - this->dataPtr->zeroAngVelSet = true; } } - gz::math::Vector3d torqueWorld; - if (std::abs(bearing.Degree()) > this->dataPtr->bearingTolerance) + else { - // remove angular velocity component otherwise the physics system will set - // the zero ang vel command every iteration - if (this->dataPtr->forceZeroAngVel && this->dataPtr->zeroAngVelSet) - { - auto angVelCmdComp = _ecm.Component( - this->dataPtr->link.Entity()); - if (angVelCmdComp) - { - _ecm.RemoveComponent( - this->dataPtr->link.Entity()); - this->dataPtr->zeroAngVelSet = false; - } - } - int sign = static_cast(std::abs(bearing.Degree()) / bearing.Degree()); torqueWorld = (*comPose).Rot().RotateVector( gz::math::Vector3d(0, 0, sign * this->dataPtr->torqueToApply)); diff --git a/test/worlds/quadcopter.sdf b/test/worlds/quadcopter.sdf index bc93489ee5..b45f431aa4 100644 --- a/test/worlds/quadcopter.sdf +++ b/test/worlds/quadcopter.sdf @@ -91,7 +91,7 @@ - 0 0 0 1.57 0 0 0 + 0 0 0 1.57 0 0 0.2 @@ -136,7 +136,7 @@ - 0 0 0 1.57 0 0 0 + 0 0 0 1.57 0 0 0.2 @@ -181,7 +181,7 @@ - 0 0 0 1.57 0 0 0 + 0 0 0 1.57 0 0 0.2 @@ -226,7 +226,7 @@ - 0 0 0 1.57 0 0 0 + 0 0 0 1.57 0 0 0.2 diff --git a/test/worlds/quadcopter_velocity_control.sdf b/test/worlds/quadcopter_velocity_control.sdf index 09390c0274..1650b5852c 100644 --- a/test/worlds/quadcopter_velocity_control.sdf +++ b/test/worlds/quadcopter_velocity_control.sdf @@ -91,7 +91,7 @@ - 0 0 0 1.57 0 0 0 + 0 0 0 1.57 0 0 0.2 @@ -136,7 +136,7 @@ - 0 0 0 1.57 0 0 0 + 0 0 0 1.57 0 0 0.2 @@ -181,7 +181,7 @@ - 0 0 0 1.57 0 0 0 + 0 0 0 1.57 0 0 0.2 @@ -226,7 +226,7 @@ - 0 0 0 1.57 0 0 0 + 0 0 0 1.57 0 0 0.2