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 <scpeters@openrobotics.org>

Author: scpeters

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

src/systems/multicopter_motor_model/MulticopterMotorModel.cc

@@ -447,14 +447,6 @@ void MulticopterMotorModel::PreUpdate(const UpdateInfo &_info,
     doUpdateForcesAndMoments = false;
   }
 
-  if (!_ecm.Component<components::JointVelocityCmd>(
-      this->dataPtr->jointEntity))
-  {
-    _ecm.CreateComponent(this->dataPtr->jointEntity,
-        components::JointVelocityCmd({0}));
-    doUpdateForcesAndMoments = false;
-  }
-
   if (!_ecm.Component<components::WorldPose>(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<components::JointVelocityCmd>(
-          this->jointEntity);
-      *jointVelCmd = components::JointVelocityCmd(
-          {this->turningDirection * refMotorRotVel
-                              / this->rotorVelocitySlowdownSim});
+      _ecm.SetComponentData<components::JointVelocityCmd>(
+        this->jointEntity,
+        {this->turningDirection * refMotorRotVel
+                                / this->rotorVelocitySlowdownSim});
     }
   }
 }

src/systems/physics/Physics.cc

@@ -3628,34 +3628,58 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm,
         return true;
       });
 
-  _ecm.Each<components::JointVelocityCmd>(
-      [&](const Entity &, components::JointVelocityCmd *_vel) -> bool
-      {
-        std::fill(_vel->Data().begin(), _vel->Data().end(), 0.0);
-        return true;
-      });
+  {
+    std::vector<Entity> entitiesJointVelocityCmd;
+    _ecm.Each<components::JointVelocityCmd>(
+        [&](const Entity &_entity, components::JointVelocityCmd *) -> bool
+        {
+          entitiesJointVelocityCmd.push_back(_entity);
+          return true;
+        });
+
+    for (const auto entity : entitiesJointVelocityCmd)
+    {
+      _ecm.RemoveComponent<components::JointVelocityCmd>(entity);
+    }
+  }
 
   _ecm.Each<components::SlipComplianceCmd>(
       [&](const Entity &, components::SlipComplianceCmd *_slip) -> bool
       {
         std::fill(_slip->Data().begin(), _slip->Data().end(), 0.0);
         return true;
       });
-  GZ_PROFILE_END();
 
-  _ecm.Each<components::AngularVelocityCmd>(
-      [&](const Entity &, components::AngularVelocityCmd *_vel) -> bool
-      {
-        _vel->Data() = math::Vector3d::Zero;
-        return true;
-      });
+  {
+    std::vector<Entity> entitiesAngularVelocityCmd;
+    _ecm.Each<components::AngularVelocityCmd>(
+        [&](const Entity &_entity, components::AngularVelocityCmd *) -> bool
+        {
+          entitiesAngularVelocityCmd.push_back(_entity);
+          return true;
+        });
 
-  _ecm.Each<components::LinearVelocityCmd>(
-      [&](const Entity &, components::LinearVelocityCmd *_vel) -> bool
-      {
-        _vel->Data() = math::Vector3d::Zero;
-        return true;
-      });
+    for (const auto entity : entitiesAngularVelocityCmd)
+    {
+      _ecm.RemoveComponent<components::AngularVelocityCmd>(entity);
+    }
+  }
+
+  {
+    std::vector<Entity> entitiesLinearVelocityCmd;
+    _ecm.Each<components::LinearVelocityCmd>(
+        [&](const Entity &_entity, components::LinearVelocityCmd *) -> bool
+        {
+          entitiesLinearVelocityCmd.push_back(_entity);
+          return true;
+        });
+
+    for (const auto entity : entitiesLinearVelocityCmd)
+    {
+      _ecm.RemoveComponent<components::LinearVelocityCmd>(entity);
+    }
+  }
+  GZ_PROFILE_END();
 
   // Update joint positions
   GZ_PROFILE_BEGIN("Joints");

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<components::AngularVelocityCmd>(
-          this->dataPtr->link.Entity());
-      if (angVelCmdComp)
-      {
-        _ecm.RemoveComponent<components::AngularVelocityCmd>(
-          this->dataPtr->link.Entity());
-        this->dataPtr->zeroAngVelSet = false;
-      }
-    }
-
     int sign = static_cast<int>(std::abs(bearing.Degree()) / bearing.Degree());
     torqueWorld = (*comPose).Rot().RotateVector(
        gz::math::Vector3d(0, 0, sign * this->dataPtr->torqueToApply));

test/worlds/quadcopter.sdf

@@ -91,7 +91,7 @@
           </geometry>
         </collision>
         <visual name="rotor_0_visual">
-          <pose>0 0 0 1.57 0 0 0</pose>
+          <pose>0 0 0 1.57 0 0</pose>
           <geometry>
             <cylinder>
               <length>0.2</length>
@@ -136,7 +136,7 @@
           </geometry>
         </collision>
         <visual name="rotor_1_visual">
-          <pose>0 0 0 1.57 0 0 0</pose>
+          <pose>0 0 0 1.57 0 0</pose>
           <geometry>
             <cylinder>
               <length>0.2</length>
@@ -181,7 +181,7 @@
           </geometry>
         </collision>
         <visual name="rotor_2_visual">
-          <pose>0 0 0 1.57 0 0 0</pose>
+          <pose>0 0 0 1.57 0 0</pose>
           <geometry>
             <cylinder>
               <length>0.2</length>
@@ -226,7 +226,7 @@
           </geometry>
         </collision>
         <visual name="rotor_3_visual">
-          <pose>0 0 0 1.57 0 0 0</pose>
+          <pose>0 0 0 1.57 0 0</pose>
           <geometry>
             <cylinder>
               <length>0.2</length>

test/worlds/quadcopter_velocity_control.sdf

@@ -91,7 +91,7 @@
           </geometry>
         </collision>
         <visual name="rotor_0_visual">
-          <pose>0 0 0 1.57 0 0 0</pose>
+          <pose>0 0 0 1.57 0 0</pose>
           <geometry>
             <cylinder>
               <length>0.2</length>
@@ -136,7 +136,7 @@
           </geometry>
         </collision>
         <visual name="rotor_1_visual">
-          <pose>0 0 0 1.57 0 0 0</pose>
+          <pose>0 0 0 1.57 0 0</pose>
           <geometry>
             <cylinder>
               <length>0.2</length>
@@ -181,7 +181,7 @@
           </geometry>
         </collision>
         <visual name="rotor_2_visual">
-          <pose>0 0 0 1.57 0 0 0</pose>
+          <pose>0 0 0 1.57 0 0</pose>
           <geometry>
             <cylinder>
               <length>0.2</length>
@@ -226,7 +226,7 @@
           </geometry>
         </collision>
         <visual name="rotor_3_visual">
-          <pose>0 0 0 1.57 0 0 0</pose>
+          <pose>0 0 0 1.57 0 0</pose>
           <geometry>
             <cylinder>
               <length>0.2</length>