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