From 4070cac0a5f2027a9dff45f8aba9a3deebbee724 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sat, 4 Nov 2023 01:13:23 -0700 Subject: [PATCH 1/9] Physics: remove *VelocityCmd at each time step Part of #1926. Signed-off-by: Steve Peters --- Migration.md | 7 ++++ src/systems/physics/Physics.cc | 62 +++++++++++++++++++++++----------- 2 files changed, 50 insertions(+), 19 deletions(-) diff --git a/Migration.md b/Migration.md index a1df392c5c..43d0e8f210 100644 --- a/Migration.md +++ b/Migration.md @@ -5,6 +5,13 @@ 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 deleted 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/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"); From 740071a248b03f0f13a01f5bfde3b419dc2dae0f Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 5 Nov 2023 01:05:48 -0700 Subject: [PATCH 2/9] Fix pose syntax error in quadcopter test worlds Signed-off-by: Steve Peters --- test/worlds/quadcopter.sdf | 8 ++++---- test/worlds/quadcopter_velocity_control.sdf | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) 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 From ac5802271b5c45e23daf70c6226712a21391cf9d Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 5 Nov 2023 01:06:41 -0700 Subject: [PATCH 3/9] 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. Signed-off-by: Steve Peters --- .../MulticopterMotorModel.cc | 17 ++++------------- 1 file changed, 4 insertions(+), 13 deletions(-) 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}); } } } From c963956e60bb121061cf5f03abb7b1531e264a8b Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 5 Nov 2023 08:14:25 -0800 Subject: [PATCH 4/9] wheel_slip: use SetComponentData Signed-off-by: Steve Peters --- test/integration/wheel_slip.cc | 25 ++++++++----------------- 1 file changed, 8 insertions(+), 17 deletions(-) diff --git a/test/integration/wheel_slip.cc b/test/integration/wheel_slip.cc index 08453c8933..fef0017caf 100644 --- a/test/integration/wheel_slip.cc +++ b/test/integration/wheel_slip.cc @@ -526,23 +526,14 @@ TEST_F(WheelSlipTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(TricyclesUphill)) testSlipSystem.OnPreUpdate([&](const UpdateInfo &, EntityComponentManager &) { - auto wheelRearLeftVelocity0Cmd = - ecm->Component(wheelRearLeftSpin0Entity); - auto wheelRearRightVelocity0Cmd = - ecm->Component(wheelRearRightSpin0Entity); - auto wheelRearLeftVelocity1Cmd = - ecm->Component(wheelRearLeftSpin1Entity); - auto wheelRearRightVelocity1Cmd = - ecm->Component(wheelRearRightSpin1Entity); - - *wheelRearLeftVelocity0Cmd = - components::JointVelocityCmd({angularSpeed}); - *wheelRearRightVelocity0Cmd = - components::JointVelocityCmd({angularSpeed}); - *wheelRearLeftVelocity1Cmd = - components::JointVelocityCmd({angularSpeed}); - *wheelRearRightVelocity1Cmd = - components::JointVelocityCmd({angularSpeed}); + ecm->SetComponentData( + wheelRearLeftSpin0Entity, {angularSpeed}); + ecm->SetComponentData( + wheelRearLeftSpin1Entity, {angularSpeed}); + ecm->SetComponentData( + wheelRearRightSpin0Entity, {angularSpeed}); + ecm->SetComponentData( + wheelRearRightSpin1Entity, {angularSpeed}); }); server.AddSystem(testSlipSystem.systemPtr); server.Run(true, 2000, false); From f6ecc6161f4beab09112212ee3b9e0f4a1acde56 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 5 Nov 2023 08:14:43 -0800 Subject: [PATCH 5/9] Fix gz-sensors branch in custom sensor example Signed-off-by: Steve Peters --- examples/plugin/custom_sensor_system/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/plugin/custom_sensor_system/CMakeLists.txt b/examples/plugin/custom_sensor_system/CMakeLists.txt index 60bc0d3c63..6682fed462 100644 --- a/examples/plugin/custom_sensor_system/CMakeLists.txt +++ b/examples/plugin/custom_sensor_system/CMakeLists.txt @@ -20,7 +20,7 @@ include(FetchContent) FetchContent_Declare( sensors_clone GIT_REPOSITORY https://github.com/gazebosim/gz-sensors - GIT_TAG gz-sensors9 + GIT_TAG main ) FetchContent_Populate(sensors_clone) add_subdirectory(${sensors_clone_SOURCE_DIR}/examples/custom_sensor ${sensors_clone_BINARY_DIR}) From b145c05aed9f537191e795354f3a8d124c80ed46 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 5 Nov 2023 21:09:59 -0800 Subject: [PATCH 6/9] Use SetComponentData to improve coverage, simplify Signed-off-by: Steve Peters --- .../ackermann_steering/AckermannSteering.cc | 57 +++--------------- src/systems/diff_drive/DiffDrive.cc | 26 ++------ .../joint_controller/JointController.cc | 12 +--- .../JointPositionController.cc | 12 +--- src/systems/thruster/Thruster.cc | 14 +---- .../velocity_control/VelocityControl.cc | 60 +++---------------- 6 files changed, 24 insertions(+), 157 deletions(-) diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc index 3fe8a5f60d..6a5a8b9e6c 100644 --- a/src/systems/ackermann_steering/AckermannSteering.cc +++ b/src/systems/ackermann_steering/AckermannSteering.cc @@ -608,70 +608,29 @@ void AckermannSteering::PreUpdate(const UpdateInfo &_info, for (Entity joint : this->dataPtr->leftJoints) { // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent( - joint, components::JointVelocityCmd( - {this->dataPtr->leftJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->leftJointSpeed}); - } + _ecm.SetComponentData( + joint, {this->dataPtr->leftJointSpeed}); } for (Entity joint : this->dataPtr->rightJoints) { // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd({this->dataPtr->rightJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->rightJointSpeed}); - } + _ecm.SetComponentData( + joint, {this->dataPtr->rightJointSpeed}); } } // Update steering for (Entity joint : this->dataPtr->leftSteeringJoints) { - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent( - joint, components::JointVelocityCmd( - {this->dataPtr->leftSteeringJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd( - {this->dataPtr->leftSteeringJointSpeed}); - } + _ecm.SetComponentData( + joint, {this->dataPtr->leftSteeringJointSpeed}); } for (Entity joint : this->dataPtr->rightSteeringJoints) { - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd( - {this->dataPtr->rightSteeringJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd( - {this->dataPtr->rightSteeringJointSpeed}); - } + _ecm.SetComponentData( + joint, {this->dataPtr->rightSteeringJointSpeed}); } if (!this->dataPtr->steeringOnly) { diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index 7da57a118c..b40558a0b6 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -463,17 +463,8 @@ void DiffDrive::PreUpdate(const UpdateInfo &_info, continue; // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent( - joint, components::JointVelocityCmd({this->dataPtr->leftJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->leftJointSpeed}); - } + _ecm.SetComponentData(joint, + {this->dataPtr->leftJointSpeed}); } for (Entity joint : this->dataPtr->rightJoints) @@ -483,17 +474,8 @@ void DiffDrive::PreUpdate(const UpdateInfo &_info, continue; // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd({this->dataPtr->rightJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->rightJointSpeed}); - } + _ecm.SetComponentData(joint, + {this->dataPtr->rightJointSpeed}); } // Create the left and right side joint position components if they diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc index 4c18ac7bdf..40f5caac33 100644 --- a/src/systems/joint_controller/JointController.cc +++ b/src/systems/joint_controller/JointController.cc @@ -344,17 +344,7 @@ void JointController::PreUpdate(const UpdateInfo &_info, // Update joint velocity for (Entity joint : this->dataPtr->jointEntities) { - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent( - joint, components::JointVelocityCmd({targetVel})); - } - else - { - *vel = components::JointVelocityCmd({targetVel}); - } + _ecm.SetComponentData(joint, {targetVel}); } } } diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index 154b1f8ccc..06219197c7 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -446,17 +446,7 @@ void JointPositionController::PreUpdate( for (Entity joint : this->dataPtr->jointEntities) { // Update velocity command. - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent( - joint, components::JointVelocityCmd({targetVel})); - } - else - { - *vel = components::JointVelocityCmd({targetVel}); - } + _ecm.SetComponentData(joint, {targetVel}); } return; } diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index e7663924b2..87aa50dcb4 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -740,18 +740,8 @@ void Thruster::PreUpdate( // Velocity control else { - auto velocityComp = - _ecm.Component( - this->dataPtr->jointEntity); - if (velocityComp == nullptr) - { - _ecm.CreateComponent(this->dataPtr->jointEntity, - components::JointVelocityCmd({desiredPropellerAngVel})); - } - else - { - velocityComp->Data()[0] = desiredPropellerAngVel; - } + _ecm.SetComponentData( + this->dataPtr->jointEntity, {desiredPropellerAngVel}); angvel.set_data(desiredPropellerAngVel); } diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index 92728a11b4..75f8111962 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -201,38 +201,12 @@ void VelocityControl::PreUpdate(const UpdateInfo &_info, return; // update angular velocity of model - auto modelAngularVel = - _ecm.Component( - this->dataPtr->model.Entity()); - - if (modelAngularVel == nullptr) - { - _ecm.CreateComponent( - this->dataPtr->model.Entity(), - components::AngularVelocityCmd({this->dataPtr->angularVelocity})); - } - else - { - *modelAngularVel = - components::AngularVelocityCmd({this->dataPtr->angularVelocity}); - } + _ecm.SetComponentData( + this->dataPtr->model.Entity(), {this->dataPtr->angularVelocity}); // update linear velocity of model - auto modelLinearVel = - _ecm.Component( - this->dataPtr->model.Entity()); - - if (modelLinearVel == nullptr) - { - _ecm.CreateComponent( - this->dataPtr->model.Entity(), - components::LinearVelocityCmd({this->dataPtr->linearVelocity})); - } - else - { - *modelLinearVel = - components::LinearVelocityCmd({this->dataPtr->linearVelocity}); - } + _ecm.SetComponentData( + this->dataPtr->model.Entity(), {this->dataPtr->linearVelocity}); // If there are links, create link components // If the link hasn't been identified yet, look for it @@ -266,17 +240,8 @@ void VelocityControl::PreUpdate(const UpdateInfo &_info, auto it = this->dataPtr->links.find(linkName); if (it != this->dataPtr->links.end()) { - auto linkAngularVelComp = - _ecm.Component(it->second); - if (!linkAngularVelComp) - { - _ecm.CreateComponent(it->second, - components::AngularVelocityCmd({angularVel})); - } - else - { - *linkAngularVelComp = components::AngularVelocityCmd(angularVel); - } + _ecm.SetComponentData( + it->second, {angularVel}); } else { @@ -290,17 +255,8 @@ void VelocityControl::PreUpdate(const UpdateInfo &_info, auto it = this->dataPtr->links.find(linkName); if (it != this->dataPtr->links.end()) { - auto linkLinearVelComp = - _ecm.Component(it->second); - if (!linkLinearVelComp) - { - _ecm.CreateComponent(it->second, - components::LinearVelocityCmd({linearVel})); - } - else - { - *linkLinearVelComp = components::LinearVelocityCmd(linearVel); - } + _ecm.SetComponentData( + it->second, {linearVel}); } else { From 7dc33fe70d47312acd45406335547fec08a19fa2 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 10 Nov 2023 17:27:27 -0800 Subject: [PATCH 7/9] Use SetComponentData in MecanumDrive as well Signed-off-by: Steve Peters --- src/systems/mecanum_drive/MecanumDrive.cc | 53 ++++------------------- 1 file changed, 8 insertions(+), 45 deletions(-) diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index c1b084bce3..2e69d0328e 100644 --- a/src/systems/mecanum_drive/MecanumDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -430,66 +430,29 @@ void MecanumDrive::PreUpdate(const gz::sim::UpdateInfo &_info, for (Entity joint : this->dataPtr->frontLeftJoints) { // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd({this->dataPtr->frontLeftJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->frontLeftJointSpeed}); - } + _ecm.SetComponentData(joint, + {this->dataPtr->frontLeftJointSpeed}); } for (Entity joint : this->dataPtr->frontRightJoints) { // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd({this->dataPtr->frontRightJointSpeed})); - } - else - { - *vel = - components::JointVelocityCmd({this->dataPtr->frontRightJointSpeed}); - } + _ecm.SetComponentData(joint, + {this->dataPtr->frontRightJointSpeed}); } for (Entity joint : this->dataPtr->backLeftJoints) { // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd({this->dataPtr->backLeftJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->backLeftJointSpeed}); - } + _ecm.SetComponentData(joint, + {this->dataPtr->backLeftJointSpeed}); } for (Entity joint : this->dataPtr->backRightJoints) { // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd({this->dataPtr->backRightJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->backRightJointSpeed}); - } + _ecm.SetComponentData(joint, + {this->dataPtr->backRightJointSpeed}); } } From 120e5fef83cbf8fdca6649ee92c30ed09fd357fc Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 10 Nov 2023 17:28:32 -0800 Subject: [PATCH 8/9] 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 --- .../trajectory_follower/TrajectoryFollower.cc | 24 +++---------------- 1 file changed, 3 insertions(+), 21 deletions(-) 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)); From f4ffd4a8721915a8c9ac90ab45006927835b7bf3 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 7 Apr 2024 00:40:35 -0700 Subject: [PATCH 9/9] Update Migration.md Signed-off-by: Steve Peters --- Migration.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Migration.md b/Migration.md index 43d0e8f210..2e55298ea9 100644 --- a/Migration.md +++ b/Migration.md @@ -8,8 +8,9 @@ release will remove the deprecated code. ## Gazebo Sim 8.x to 9.0 * **Modified**: - + In the Physics system, all `*VelocityCmd` components are deleted after - each time step. Persistent velocity commands should be reapplied at + + 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