ForceTorque system: write WrenchMeasured to ECM

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

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;
 
@@ -152,15 +153,18 @@ void ForceTorque::PreUpdate(const UpdateInfo &/*_info*/,
         _ecm.Component<components::ParentEntity>(entity)->Data();
     gzdbg << "Adding JointTransmittedWrench to: " << jointEntity << std::endl;
     _ecm.CreateComponent(jointEntity, components::JointTransmittedWrench());
+    // Enable WrenchMeasured to save sensor measurements
+    gzdbg << "Adding WrenchMeasured to: " << entity << std::endl;
+    _ecm.CreateComponent(entity, components::WrenchMeasured());
   }
   this->dataPtr->newSensors.clear();
 }
 
 //////////////////////////////////////////////////
-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 +180,13 @@ 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())
+      if (sensor->NextDataUpdateTime() <= _info.simTime)
       {
         needsUpdate = true;
         break;
@@ -203,6 +205,9 @@ void ForceTorque::PostUpdate(const UpdateInfo &_info,
       // * Apply noise
       // * Publish to gz-transport topic
       sensor->Update(_info.simTime, false);
+      const auto &measuredWrench = sensor->MeasuredWrench();
+      _ecm.SetComponentData<components::WrenchMeasured>(
+        sensorEntity, measuredWrench);
     }
   }
 
@@ -445,7 +450,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)

test/integration/force_torque_system.cc

@@ -24,10 +24,15 @@
 #include <gz/transport/Node.hh>
 #include <gz/utils/ExtraTestMacros.hh>
 
+#include "gz/sim/components/ForceTorque.hh"
+#include "gz/sim/components/Model.hh"
 #include "gz/sim/components/Name.hh"
 #include "gz/sim/components/Sensor.hh"
-#include "gz/sim/components/ForceTorque.hh"
+#include "gz/sim/components/WrenchMeasured.hh"
 
+#include "gz/sim/Joint.hh"
+#include "gz/sim/Link.hh"
+#include "gz/sim/Model.hh"
 #include "gz/sim/Server.hh"
 #include "gz/sim/SystemLoader.hh"
 #include "test_config.hh"
@@ -43,7 +48,7 @@ class ForceTorqueTest : public InternalFixture<::testing::Test>
 };
 
 /////////////////////////////////////////////////
-TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MeasureWeight))
+TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MeasureWeightTopic))
 {
   using namespace std::chrono_literals;
   // Start server
@@ -59,8 +64,8 @@ TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MeasureWeight))
 
   // Having iters exactly in sync with update rate can lead to a race condition
   // in the test between simulation and transport
-  size_t iters = 999u;
-  size_t updates = 100u;
+  const size_t iters = 999u;
+  const size_t updates = 100u;
 
   std::vector<msgs::Wrench> wrenches;
   wrenches.reserve(updates);
@@ -100,6 +105,68 @@ TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MeasureWeight))
   }
 }
 
+/////////////////////////////////////////////////
+TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MeasureWeightECM))
+{
+  using namespace std::chrono_literals;
+  // Start server
+  ServerConfig serverConfig;
+  const auto sdfFile = common::joinPaths(
+      std::string(PROJECT_SOURCE_PATH), "test", "worlds", "force_torque.sdf");
+  serverConfig.SetSdfFile(sdfFile);
+
+  Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+  server.SetUpdatePeriod(1ns);
+
+  test::Relay testSystem;
+
+  // Get the MeasuredWrench for //test1/scale/sensor_joint/force_torque_sensor
+  msgs::Wrench wrench;
+  testSystem.OnPostUpdate(
+    [&wrench](const UpdateInfo &,
+              const EntityComponentManager &_ecm)
+    {
+      auto test1ModelEntity = _ecm.EntityByComponents(
+          components::Model(), components::Name("test1"));
+      Model test1Model(test1ModelEntity);
+
+      Model scaleModel(test1Model.ModelByName(_ecm, "scale"));
+      ASSERT_TRUE(scaleModel.Valid(_ecm));
+
+      Joint sensorJoint(scaleModel.JointByName(_ecm, "sensor_joint"));
+      ASSERT_TRUE(sensorJoint.Valid(_ecm));
+
+      auto sensorEntity =
+          sensorJoint.SensorByName(_ecm, "force_torque_sensor");
+      auto measuredWrench =
+          _ecm.Component<components::WrenchMeasured>(sensorEntity);
+      if (measuredWrench)
+      {
+        wrench = measuredWrench->Data();
+      }
+    });
+
+  // Add the system
+  server.AddSystem(testSystem.systemPtr);
+  server.Run(true, 1, false);
+
+  const size_t iters = 999u;
+
+  // Run server (iters-1) steps, since we already took 1 step above
+  server.Run(true, iters - 1, false);
+  ASSERT_EQ(iters, *server.IterationCount());
+
+  const double kSensorMass = 0.2;
+  const double kWeightMass = 10;
+  const double kGravity = 9.8;
+  const math::Vector3 expectedForce =
+      math::Vector3d{0, 0, kGravity * (kSensorMass + kWeightMass)};
+  EXPECT_EQ(expectedForce, msgs::Convert(wrench.force()));
+  EXPECT_EQ(math::Vector3d::Zero, msgs::Convert(wrench.torque()));
+}
+
 /////////////////////////////////////////////////
 TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SensorPoseOffset))
 {

test/integration/model.cc

@@ -164,6 +164,30 @@ TEST_F(ModelIntegrationTest, SourceFilePath)
   EXPECT_EQ("/tmp/path", model.SourceFilePath(ecm));
 }
 
+//////////////////////////////////////////////////
+TEST_F(ModelIntegrationTest, ModelByName)
+{
+  EntityComponentManager ecm;
+
+  // Model
+  auto eModel = ecm.CreateEntity();
+  Model model(eModel);
+  EXPECT_EQ(eModel, model.Entity());
+  EXPECT_EQ(0u, model.ModelCount(ecm));
+
+  // Nested Model
+  auto eNestedModel = ecm.CreateEntity();
+  ecm.CreateComponent<components::Model>(eNestedModel, components::Model());
+  ecm.CreateComponent<components::ParentEntity>(eNestedModel,
+      components::ParentEntity(eModel));
+  ecm.CreateComponent<components::Name>(eNestedModel,
+      components::Name("nested_model_name"));
+
+  // Check model
+  EXPECT_EQ(eNestedModel, model.ModelByName(ecm, "nested_model_name"));
+  EXPECT_EQ(1u, model.ModelCount(ecm));
+}
+
 //////////////////////////////////////////////////
 TEST_F(ModelIntegrationTest, LinkByName)
 {

test/worlds/force_torque.sdf

@@ -5,7 +5,9 @@
       <real_time_factor>0</real_time_factor>
     </physics>
     <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-forcetorque-system" name="gz::sim::systems::ForceTorque">
+      <gz:system_priority>10</gz:system_priority>
+    </plugin>
     <plugin filename='gz-sim-scene-broadcaster-system' name='gz::sim::systems::SceneBroadcaster' />
     <model name="ground_plane">
       <static>true</static>