ForceTorque system: write WrenchMeasured to ECM (#2494)

* ForceTorque system: use Update, not PostUpdate and
  set gz:system_priority to positive value for force_torque
  system in example and test worlds to ensure it runs after
  Physics::Update.
* Add WrenchMeasured component
* Write WrenchMeasured to ECM in ForceTorque::Update if
  component exists. This allows users to enable writing the
  sensor data to the ECM by creating the component.
* Model.hh: add nested model accessor APIs ModelByName
  and ModelCount. These are needed for the updated
  ForceTorque test.
* sensor_TEST.py: move PreUpdate callback to Update
  The test's PreUpdate callback assumes that it executes
  after the ForceTorque::PreUpdate, so just move it to
  Update to guarantee it.

Signed-off-by: Steve Peters <[email protected]>

Migration.md

@@ -12,6 +12,11 @@ release will remove the deprecated code.
      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.
+   + The ForceTorque system has been changed from updating sensor data during
+     the parallelized `PostUpdate` phase to use the sequential `Update` phase
+     and writing directly to the ECM if a sensor entity has a `WrenchMeasured`
+     component. The ForceTorque system priority is specified to ensure that its
+     `Update` phase executes after `Physics::Update`.
 
 ## Gazebo Sim 7.x to 8.0
 * **Deprecated**

examples/worlds/sensors.sdf

@@ -38,6 +38,7 @@
     <plugin
       filename="gz-sim-forcetorque-system"
       name="gz::sim::systems::ForceTorque">
+      <gz::system_priority>10</gz::system_priority>
     </plugin>
     <plugin
       filename="gz-sim-user-commands-system"

include/gz/sim/Model.hh

@@ -137,6 +137,14 @@ namespace gz
       public: sim::Entity LinkByName(const EntityComponentManager &_ecm,
           const std::string &_name) const;
 
+      /// \brief Get the ID of a nested model entity which is an immediate
+      /// child of this model.
+      /// \param[in] _ecm Entity-component manager.
+      /// \param[in] _name Nested model name.
+      /// \return Nested model entity.
+      public: sim::Entity ModelByName(const EntityComponentManager &_ecm,
+          const std::string &_name) const;
+
       /// \brief Get all joints which are immediate children of this model.
       /// \param[in] _ecm Entity-component manager.
       /// \return All joints in this model.
@@ -167,6 +175,12 @@ namespace gz
       /// \return Number of links in this model.
       public: uint64_t LinkCount(const EntityComponentManager &_ecm) const;
 
+      /// \brief Get the number of nested models which are immediate children
+      /// of this model.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return Number of nested models in this model.
+      public: uint64_t ModelCount(const EntityComponentManager &_ecm) const;
+
       /// \brief Set a command to change the model's pose.
       /// \param[in] _ecm Entity-component manager.
       /// \param[in] _pose New model pose.

include/gz/sim/components/WrenchMeasured.hh

@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) 2024 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+#ifndef GZ_SIM_COMPONENTS_WRENCHMEASURED_HH_
+#define GZ_SIM_COMPONENTS_WRENCHMEASURED_HH_
+
+#include <gz/msgs/wrench.pb.h>
+
+#include <gz/sim/components/Factory.hh>
+#include <gz/sim/components/Component.hh>
+#include <gz/sim/components/Serialization.hh>
+#include <gz/sim/config.hh>
+
+namespace gz
+{
+namespace sim
+{
+// Inline bracket to help doxygen filtering.
+inline namespace GZ_SIM_VERSION_NAMESPACE {
+
+namespace components
+{
+/// \brief Wrench measured by a ForceTorqueSensor in SI units (Nm for torque,
+/// N for force).
+/// The wrench is expressed in the Sensor frame and the force component is
+/// applied at the sensor origin.
+/// \note The term Wrench is used here to mean a pair of 3D vectors representing
+/// torque and force quantities expressed in a given frame and where the force
+/// is applied at the origin of the frame. This is different from the Wrench
+/// used in screw theory.
+/// \note The value of force_offset in msgs::Wrench is ignored for this
+/// component. The force is assumed to be applied at the origin of the sensor
+/// frame.
+using WrenchMeasured =
+    Component<msgs::Wrench, class WrenchMeasuredTag,
+              serializers::MsgSerializer>;
+GZ_SIM_REGISTER_COMPONENT("gz_sim_components.WrenchMeasured",
+                          WrenchMeasured)
+}  // namespace components
+}
+}
+}
+
+#endif

python/test/joint_test.sdf

@@ -1,8 +1,10 @@
 <?xml version="1.0" ?>
 <sdf version="1.6">
     <world name="world_test">
-    <plugin filename="gz-sim-physics-system" name="gz::sim::systems::Physics" />
-    <plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque" />
+        <plugin filename="gz-sim-physics-system" name="gz::sim::systems::Physics" />
+        <plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque">
+            <gz:system_priority>10</gz:system_priority>
+        </plugin>
         <model name="model_test">
 
             <link name="link_test_1">

python/test/sensor_TEST.py

@@ -33,11 +33,11 @@ def test_model(self):
         file_path = os.path.dirname(os.path.realpath(__file__))
         fixture = TestFixture(os.path.join(file_path, 'joint_test.sdf'))
 
-        def on_post_udpate_cb(_info, _ecm):
+        def on_post_update_cb(_info, _ecm):
             self.post_iterations += 1
 
-        def on_pre_udpate_cb(_info, _ecm):
-            self.pre_iterations += 1
+        def on_update_cb(_info, _ecm):
+            self.iterations += 1
             world_e = world_entity(_ecm)
             self.assertNotEqual(K_NULL_ENTITY, world_e)
             w = World(world_e)
@@ -53,19 +53,19 @@ def on_pre_udpate_cb(_info, _ecm):
             # Pose Test
             self.assertEqual(Pose3d(0, 1, 0, 0, 0, 0), sensor.pose(_ecm))
             # Topic Test
-            if self.pre_iterations <= 1:
+            if self.iterations <= 1:
                 self.assertEqual(None, sensor.topic(_ecm))
             else:
                 self.assertEqual('sensor_topic_test', sensor.topic(_ecm))
             # Parent Test
             self.assertEqual(j.entity(), sensor.parent(_ecm))
 
-        def on_udpate_cb(_info, _ecm):
-            self.iterations += 1
+        def on_pre_update_cb(_info, _ecm):
+            self.pre_iterations += 1
 
-        fixture.on_post_update(on_post_udpate_cb)
-        fixture.on_update(on_udpate_cb)
-        fixture.on_pre_update(on_pre_udpate_cb)
+        fixture.on_post_update(on_post_update_cb)
+        fixture.on_update(on_update_cb)
+        fixture.on_pre_update(on_pre_update_cb)
         fixture.finalize()
 
         server = fixture.server()

src/Model.cc

@@ -148,6 +148,16 @@ Entity Model::LinkByName(const EntityComponentManager &_ecm,
       components::Link());
 }
 
+//////////////////////////////////////////////////
+Entity Model::ModelByName(const EntityComponentManager &_ecm,
+    const std::string &_name) const
+{
+  return _ecm.EntityByComponents(
+      components::ParentEntity(this->dataPtr->id),
+      components::Name(_name),
+      components::Model());
+}
+
 //////////////////////////////////////////////////
 std::vector<Entity> Model::Joints(const EntityComponentManager &_ecm) const
 {
@@ -184,6 +194,12 @@ uint64_t Model::LinkCount(const EntityComponentManager &_ecm) const
   return this->Links(_ecm).size();
 }
 
+//////////////////////////////////////////////////
+uint64_t Model::ModelCount(const EntityComponentManager &_ecm) const
+{
+  return this->Models(_ecm).size();
+}
+
 //////////////////////////////////////////////////
 void Model::SetWorldPoseCmd(EntityComponentManager &_ecm,
     const math::Pose3d &_pose)

src/systems/force_torque/ForceTorque.cc

@@ -44,6 +44,7 @@
 #include "gz/sim/components/Pose.hh"
 #include "gz/sim/components/Sensor.hh"
 #include "gz/sim/components/World.hh"
+#include "gz/sim/components/WrenchMeasured.hh"
 #include "gz/sim/EntityComponentManager.hh"
 #include "gz/sim/Util.hh"
 
@@ -77,7 +78,7 @@ class gz::sim::systems::ForceTorquePrivate
   public: sensors::SensorFactory sensorFactory;
 
   /// \brief Keep list of sensors that were created during the previous
-  /// `PostUpdate`, so that components can be created during the next
+  /// `Update`, so that components can be created during the next
   /// `PreUpdate`.
   public: std::unordered_set<Entity> newSensors;
 
@@ -157,10 +158,10 @@ void ForceTorque::PreUpdate(const UpdateInfo &/*_info*/,
 }
 
 //////////////////////////////////////////////////
-void ForceTorque::PostUpdate(const UpdateInfo &_info,
-                     const EntityComponentManager &_ecm)
+void ForceTorque::Update(const UpdateInfo &_info,
+                         EntityComponentManager &_ecm)
 {
-  GZ_PROFILE("ForceTorque::PostUpdate");
+  GZ_PROFILE("ForceTorque::Update");
 
   // \TODO(anyone) Support rewind
   if (_info.dt < std::chrono::steady_clock::duration::zero())
@@ -176,15 +177,15 @@ void ForceTorque::PostUpdate(const UpdateInfo &_info,
   if (!_info.paused)
   {
     // check to see if update is necessary
-    // we only update if there is at least one sensor that needs data
-    // and that sensor has subscribers.
+    // we only update if there is at least one sensor that needs data.
     // note: gz-sensors does its own throttling. Here the check is mainly
     // to avoid doing work in the ForceTorquePrivate::Update function
     bool needsUpdate = false;
     for (const auto &[sensorEntity, sensor] : this->dataPtr->entitySensorMap)
     {
       if (sensor->NextDataUpdateTime() <= _info.simTime &&
-          sensor->HasConnections())
+          (sensor->HasConnections() ||
+          _ecm.Component<components::WrenchMeasured>(sensorEntity) != nullptr))
       {
         needsUpdate = true;
         break;
@@ -203,6 +204,13 @@ void ForceTorque::PostUpdate(const UpdateInfo &_info,
       // * Apply noise
       // * Publish to gz-transport topic
       sensor->Update(_info.simTime, false);
+      auto wrenchComponent =
+          _ecm.Component<components::WrenchMeasured>(sensorEntity);
+      if (wrenchComponent)
+      {
+        const auto &measuredWrench = sensor->MeasuredWrench();
+        *wrenchComponent = components::WrenchMeasured(measuredWrench);
+      }
     }
   }
 
@@ -445,7 +453,7 @@ void ForceTorquePrivate::RemoveForceTorqueEntities(
 
 GZ_ADD_PLUGIN(ForceTorque, System,
   ForceTorque::ISystemPreUpdate,
-  ForceTorque::ISystemPostUpdate
+  ForceTorque::ISystemUpdate
 )
 
 GZ_ADD_PLUGIN_ALIAS(ForceTorque, "gz::sim::systems::ForceTorque")

src/systems/force_torque/ForceTorque.hh

@@ -42,7 +42,7 @@ namespace systems
   class ForceTorque:
     public System,
     public ISystemPreUpdate,
-    public ISystemPostUpdate
+    public ISystemUpdate
   {
     /// \brief Constructor
     public: ForceTorque();
@@ -55,8 +55,8 @@ namespace systems
                            EntityComponentManager &_ecm) final;
 
     /// Documentation inherited
-    public: void PostUpdate(const UpdateInfo &_info,
-                            const EntityComponentManager &_ecm) final;
+    public: void Update(const UpdateInfo &_info,
+                        EntityComponentManager &_ecm) final;
 
     /// \brief Private data pointer.
     private: std::unique_ptr<ForceTorquePrivate> dataPtr;

src/systems/physics/Physics.cc

@@ -432,7 +432,7 @@ class gz::sim::systems::PhysicsPrivate
                       }
                       return true;
                     }};
-  /// \brief msgs::Contacts equality comparison function.
+  /// \brief msgs::Wrench equality comparison function.
   public: std::function<bool(const msgs::Wrench &, const msgs::Wrench &)>
           wrenchEql{
           [](const msgs::Wrench &_a, const msgs::Wrench &_b)