Physics: remove *VelocityCmd at each time step

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

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})

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<components::JointVelocityCmd>(joint);
-
-      if (vel == nullptr)
-      {
-        _ecm.CreateComponent(
-            joint, components::JointVelocityCmd(
-              {this->dataPtr->leftJointSpeed}));
-      }
-      else
-      {
-        *vel = components::JointVelocityCmd({this->dataPtr->leftJointSpeed});
-      }
+      _ecm.SetComponentData<components::JointVelocityCmd>(
+        joint, {this->dataPtr->leftJointSpeed});
     }
 
     for (Entity joint : this->dataPtr->rightJoints)
     {
       // Update wheel velocity
-      auto vel = _ecm.Component<components::JointVelocityCmd>(joint);
-
-      if (vel == nullptr)
-      {
-        _ecm.CreateComponent(joint,
-            components::JointVelocityCmd({this->dataPtr->rightJointSpeed}));
-      }
-      else
-      {
-        *vel = components::JointVelocityCmd({this->dataPtr->rightJointSpeed});
-      }
+      _ecm.SetComponentData<components::JointVelocityCmd>(
+        joint, {this->dataPtr->rightJointSpeed});
     }
   }
 
   // Update steering
   for (Entity joint : this->dataPtr->leftSteeringJoints)
   {
-    auto vel = _ecm.Component<components::JointVelocityCmd>(joint);
-
-    if (vel == nullptr)
-    {
-      _ecm.CreateComponent(
-          joint, components::JointVelocityCmd(
-                             {this->dataPtr->leftSteeringJointSpeed}));
-    }
-    else
-    {
-      *vel = components::JointVelocityCmd(
-                         {this->dataPtr->leftSteeringJointSpeed});
-    }
+    _ecm.SetComponentData<components::JointVelocityCmd>(
+      joint, {this->dataPtr->leftSteeringJointSpeed});
   }
 
   for (Entity joint : this->dataPtr->rightSteeringJoints)
   {
-    auto vel = _ecm.Component<components::JointVelocityCmd>(joint);
-
-    if (vel == nullptr)
-    {
-      _ecm.CreateComponent(joint,
-          components::JointVelocityCmd(
-                  {this->dataPtr->rightSteeringJointSpeed}));
-    }
-    else
-    {
-      *vel = components::JointVelocityCmd(
-                     {this->dataPtr->rightSteeringJointSpeed});
-    }
+    _ecm.SetComponentData<components::JointVelocityCmd>(
+      joint, {this->dataPtr->rightSteeringJointSpeed});
   }
   if (!this->dataPtr->steeringOnly)
   {

src/systems/diff_drive/DiffDrive.cc

@@ -463,17 +463,8 @@ void DiffDrive::PreUpdate(const UpdateInfo &_info,
       continue;
 
     // Update wheel velocity
-    auto vel = _ecm.Component<components::JointVelocityCmd>(joint);
-
-    if (vel == nullptr)
-    {
-      _ecm.CreateComponent(
-          joint, components::JointVelocityCmd({this->dataPtr->leftJointSpeed}));
-    }
-    else
-    {
-      *vel = components::JointVelocityCmd({this->dataPtr->leftJointSpeed});
-    }
+    _ecm.SetComponentData<components::JointVelocityCmd>(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<components::JointVelocityCmd>(joint);
-
-    if (vel == nullptr)
-    {
-      _ecm.CreateComponent(joint,
-          components::JointVelocityCmd({this->dataPtr->rightJointSpeed}));
-    }
-    else
-    {
-      *vel = components::JointVelocityCmd({this->dataPtr->rightJointSpeed});
-    }
+    _ecm.SetComponentData<components::JointVelocityCmd>(joint,
+      {this->dataPtr->rightJointSpeed});
   }
 
   // Create the left and right side joint position components if they

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<components::JointVelocityCmd>(joint);
-
-      if (vel == nullptr)
-      {
-        _ecm.CreateComponent(
-            joint, components::JointVelocityCmd({targetVel}));
-      }
-      else
-      {
-        *vel = components::JointVelocityCmd({targetVel});
-      }
+      _ecm.SetComponentData<components::JointVelocityCmd>(joint, {targetVel});
     }
   }
 }

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<components::JointVelocityCmd>(joint);
-
-      if (vel == nullptr)
-      {
-        _ecm.CreateComponent(
-            joint, components::JointVelocityCmd({targetVel}));
-      }
-      else
-      {
-        *vel = components::JointVelocityCmd({targetVel});
-      }
+      _ecm.SetComponentData<components::JointVelocityCmd>(joint, {targetVel});
     }
     return;
   }

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/thruster/Thruster.cc

@@ -740,18 +740,8 @@ void Thruster::PreUpdate(
   // Velocity control
   else
   {
-    auto velocityComp =
-    _ecm.Component<gz::sim::components::JointVelocityCmd>(
-      this->dataPtr->jointEntity);
-    if (velocityComp == nullptr)
-    {
-      _ecm.CreateComponent(this->dataPtr->jointEntity,
-        components::JointVelocityCmd({desiredPropellerAngVel}));
-    }
-    else
-    {
-      velocityComp->Data()[0] = desiredPropellerAngVel;
-    }
+    _ecm.SetComponentData<gz::sim::components::JointVelocityCmd>(
+      this->dataPtr->jointEntity, {desiredPropellerAngVel});
     angvel.set_data(desiredPropellerAngVel);
   }
 

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<components::AngularVelocityCmd>(
-      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<components::AngularVelocityCmd>(
+    this->dataPtr->model.Entity(), {this->dataPtr->angularVelocity});
 
   // update linear velocity of model
-  auto modelLinearVel =
-    _ecm.Component<components::LinearVelocityCmd>(
-      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<components::LinearVelocityCmd>(
+    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<components::AngularVelocityCmd>(it->second);
-      if (!linkAngularVelComp)
-      {
-        _ecm.CreateComponent(it->second,
-            components::AngularVelocityCmd({angularVel}));
-      }
-      else
-      {
-        *linkAngularVelComp = components::AngularVelocityCmd(angularVel);
-      }
+      _ecm.SetComponentData<components::AngularVelocityCmd>(
+        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<components::LinearVelocityCmd>(it->second);
-      if (!linkLinearVelComp)
-      {
-        _ecm.CreateComponent(it->second,
-            components::LinearVelocityCmd({linearVel}));
-      }
-      else
-      {
-        *linkLinearVelComp = components::LinearVelocityCmd(linearVel);
-      }
+      _ecm.SetComponentData<components::LinearVelocityCmd>(
+        it->second, {linearVel});
     }
     else
     {