Add force-torque determinism plugin and demo world (#10)

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

ionic_demo/plugins/ft_sensor_determinism/CMakeLists.txt

@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 3.22.1 FATAL_ERROR)
+
+find_package(gz-cmake4 REQUIRED)
+
+project(ft_sensor_determinism)
+
+gz_find_package(gz-plugin3 REQUIRED COMPONENTS register)
+set(GZ_PLUGIN_VER ${gz-plugin3_VERSION_MAJOR})
+
+gz_find_package(gz-sensors9 REQUIRED)
+set(GZ_SENSORS_VER ${gz-sensors9_VERSION_MAJOR})
+
+gz_find_package(gz-sim9 REQUIRED)
+set(GZ_SIM_VER ${gz-sim9_VERSION_MAJOR})
+
+add_library(FTSensorDeterminism SHARED FTSensorDeterminism.cc)
+set_property(TARGET FTSensorDeterminism PROPERTY CXX_STANDARD 17)
+target_link_libraries(FTSensorDeterminism
+  PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
+  PRIVATE gz-sensors${GZ_SENSORS_VER}::gz-sensors${GZ_SENSORS_VER}
+  PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER})

ionic_demo/plugins/ft_sensor_determinism/FTSensorDeterminism.cc

@@ -0,0 +1,133 @@
+/*
+ * 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.
+ *
+*/
+
+// We'll use a string and the gzmsg command below for a brief example.
+// Remove these includes if your plugin doesn't need them.
+#include <memory>
+#include <string>
+#include <gz/common/Console.hh>
+
+#include <gz/msgs/convert/StdTypes.hh>
+
+// This header is required to register plugins. It's good practice to place it
+// in the cc file, like it's done here.
+#include <gz/plugin/Register.hh>
+
+#include <gz/sensors/ForceTorqueSensor.hh>
+#include "gz/sim/components/WrenchMeasured.hh"
+#include <gz/sim/Sensor.hh>
+
+// Don't forget to include the plugin's header.
+#include "FTSensorDeterminism.hh"
+
+// This is required to register the plugin. Make sure the interfaces match
+// what's in the header.
+GZ_ADD_PLUGIN(
+    ft_sensor_determinism::FTSensorDeterminism,
+    gz::sim::System,
+    ft_sensor_determinism::FTSensorDeterminism::ISystemConfigure,
+    ft_sensor_determinism::FTSensorDeterminism::ISystemUpdate)
+
+using namespace ft_sensor_determinism;
+
+//////////////////////////////////////////////////
+void FTSensorDeterminism::Configure(
+        const gz::sim::Entity &_entity,
+        const std::shared_ptr<const sdf::Element> &_sdf,
+        gz::sim::EntityComponentManager &_ecm,
+        gz::sim::EventManager &_eventMgr)
+{
+  this->sensor = gz::sim::Sensor(_entity);
+  if (!this->sensor.Valid(_ecm))
+  {
+    gzerr << "This plugin should be attached to a sensor.\n";
+    return;
+  }
+
+  // Create WrenchMeasured component for sensor Entity
+  _ecm.CreateComponent(_entity, gz::sim::components::WrenchMeasured());
+
+}
+
+//////////////////////////////////////////////////
+void FTSensorDeterminism::OnWrench(const gz::msgs::Wrench &_msg)
+{
+  std::lock_guard<std::mutex> lock(this->mutex);
+  this->wrenchFromTopic = _msg;
+}
+
+//////////////////////////////////////////////////
+double wrenchTimeStamp(const gz::msgs::Wrench &_msg)
+{
+  return std::chrono::duration<double>(
+      gz::msgs::Convert(_msg.header().stamp())).count();
+}
+
+//////////////////////////////////////////////////
+void FTSensorDeterminism::Update(const gz::sim::UpdateInfo &_info,
+    gz::sim::EntityComponentManager &_ecm)
+{
+  double simTime = std::chrono::duration<double>(_info.simTime).count();
+
+  auto wrenchComponent =
+    _ecm.Component<gz::sim::components::WrenchMeasured>(this->sensor.Entity());
+  if (!wrenchComponent)
+  {
+    return;
+  }
+  this->wrenchFromECM = wrenchComponent->Data();
+
+  if (!subscribed)
+  {
+    auto topic = this->sensor.Topic(_ecm);
+    if (topic)
+    {
+      this->node.Subscribe(*topic, &FTSensorDeterminism::OnWrench, this);
+      this->subscribed = true;
+    }
+  }
+
+  if (_info.paused)
+  {
+    return;
+  }
+
+  double wrenchTimeFromECM = wrenchTimeStamp(*this->wrenchFromECM);
+  if (!gz::math::equal(simTime, wrenchTimeFromECM))
+  {
+    gzerr << "FT non-determinism in ECM data "
+          << "THIS SHOULD NOT HAPPEN!!! "
+          << "iteration " << _info.iterations
+          << ", simTime " << simTime
+          << ", wrenchFromECM time diff " << wrenchTimeFromECM - simTime
+          << '\n';
+  }
+
+  std::lock_guard<std::mutex> lock(this->mutex);
+  if (this->wrenchFromTopic)
+  {
+    double wrenchTimeFromTopic = wrenchTimeStamp(*this->wrenchFromTopic);
+    if (!gz::math::equal(simTime, wrenchTimeFromTopic))
+    {
+      gzerr << "FT non-determinism in gz-transport data "
+            << "iteration " << _info.iterations
+            << ", simTime " << simTime
+            << ", wrenchFromTopic time diff " << wrenchTimeFromTopic - simTime
+            << '\n';
+    }
+  }
+}

ionic_demo/plugins/ft_sensor_determinism/FTSensorDeterminism.hh

@@ -0,0 +1,56 @@
+/*
+ * 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 EXAMPLE_PLUGIN_FTSENSORDETERMINISM_HH_
+#define EXAMPLE_PLUGIN_FTSENSORDETERMINISM_HH_
+
+#include <memory>
+#include <mutex>
+#include <optional>
+#include <string>
+#include <gz/transport/Node.hh>
+#include <gz/sim/System.hh>
+
+namespace ft_sensor_determinism
+{
+  // This plugin prints the number of elapsed simulation iterations,
+  // this system's priority value from the XML configuration,
+  // and a custom label from the XML configuration during the Update callback.
+  class FTSensorDeterminism:
+    public gz::sim::System,
+    public gz::sim::ISystemConfigure,
+    public gz::sim::ISystemUpdate
+  {
+    public: void Configure(const gz::sim::Entity &_entity,
+                 const std::shared_ptr<const sdf::Element> &_sdf,
+                 gz::sim::EntityComponentManager &_ecm,
+                 gz::sim::EventManager &_eventMgr) override;
+
+    public: void Update(const gz::sim::UpdateInfo &_info,
+                        gz::sim::EntityComponentManager &_ecm) override;
+
+    private: void OnWrench(const gz::msgs::Wrench &_msg);
+
+    private: gz::sim::Sensor sensor{gz::sim::kNullEntity};
+    private: bool subscribed{false};
+    private: std::optional<gz::msgs::Wrench> wrenchFromECM;
+    private: std::optional<gz::msgs::Wrench> wrenchFromTopic;
+    private: std::mutex mutex;
+    private: gz::transport::Node node;
+  };
+}
+#endif

ionic_demo/plugins/ft_sensor_determinism/README.md

@@ -0,0 +1,75 @@
+# Force-torque sensor determinism
+
+The Force-torque wrench data can now be read directly from the ECM in
+addition to subscribing to a gz-transport topic.
+This example plugin can be attached to a force-torque sensor to compare the
+determinism of data access via these two methods by printing a console error
+message whenever old data is accessed.
+
+## Build
+
+From this folder, do the following to build the plugin:
+
+~~~
+mkdir build
+cd build
+cmake ..
+make
+~~~
+
+This will generate the `FTSensorDeterminism` library under `build`.
+
+## Run
+
+A demo world is adapted from the
+[mimic\_fast\_slow\_pendulums\_world.sdf](https://github.com/gazebosim/gz-sim/blob/gz-sim9/examples/worlds/mimic_fast_slow_pendulums_world.sdf)
+example world added to demonstrate mimic constraints in Gazebo Harmonic.
+A force-torque sensor with an instance of the `FTSensorDeterminism` plugin is
+added to each joint in the world.
+
+Before starting Gazebo, we must make sure it can find the plugin by doing:
+
+~~~
+cd ionic_demo/plugins/ft_sensor_determinism
+export GZ_SIM_SYSTEM_PLUGIN_PATH=`pwd`/build
+~~~
+
+Then run the demo world:
+
+    gz sim -v 3 ft_sensor_determinism.sdf -r
+
+The data accessed via the ECM should never be out of date, but data from the
+gz-transport topic may be outdated depending on the computing load on your
+system. To add stress to the system, the `real_time_factor` SDFormat parameter
+is set to `0` to maximize the update rate and the example command above opens
+the GUI. To add additional stress, use the `stress` command with a `--cpu`
+parameter equal to the number of CPU cores on your system, which should
+increase the number of error messages printed about "FT non-determinism in
+gz-transport data."
+
+Example error messages:
+
+~~~
+(2024-09-24 01:24:10.347) [error] FT non-determinism in gz-transport data iteration 57818, simTime 57.818, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:10.347) [error] FT non-determinism in gz-transport data iteration 57818, simTime 57.818, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:10.347) [error] FT non-determinism in gz-transport data iteration 57818, simTime 57.818, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:10.347) [error] FT non-determinism in gz-transport data iteration 57818, simTime 57.818, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:10.347) [error] FT non-determinism in gz-transport data iteration 57818, simTime 57.818, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:10.347) [error] FT non-determinism in gz-transport data iteration 57818, simTime 57.818, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:10.367) [error] FT non-determinism in gz-transport data iteration 57860, simTime 57.86, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:10.367) [error] FT non-determinism in gz-transport data iteration 57860, simTime 57.86, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:10.367) [error] FT non-determinism in gz-transport data iteration 57860, simTime 57.86, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:10.367) [error] FT non-determinism in gz-transport data iteration 57860, simTime 57.86, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:10.500) [error] FT non-determinism in gz-transport data iteration 58140, simTime 58.14, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:10.540) [error] FT non-determinism in gz-transport data iteration 58226, simTime 58.226, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:10.540) [error] FT non-determinism in gz-transport data iteration 58226, simTime 58.226, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:10.540) [error] FT non-determinism in gz-transport data iteration 58226, simTime 58.226, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:10.752) [error] FT non-determinism in gz-transport data iteration 58668, simTime 58.668, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:10.752) [error] FT non-determinism in gz-transport data iteration 58668, simTime 58.668, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:10.752) [error] FT non-determinism in gz-transport data iteration 58668, simTime 58.668, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:11.063) [error] FT non-determinism in gz-transport data iteration 59334, simTime 59.334, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:11.078) [error] FT non-determinism in gz-transport data iteration 59363, simTime 59.363, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:11.111) [error] FT non-determinism in gz-transport data iteration 59427, simTime 59.427, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:11.344) [error] FT non-determinism in gz-transport data iteration 59892, simTime 59.892, wrenchFromTopic time diff -0.001
+(2024-09-24 01:24:11.533) [error] FT non-determinism in gz-transport data iteration 60283, simTime 60.283, wrenchFromTopic time diff -0.001
+~~~