From ca0dd8de0504a51e84e9c54482882ac1a84832cc Mon Sep 17 00:00:00 2001
From: Steve Peters <computersthatmove@gmail.com>
Date: Tue, 24 Sep 2024 19:09:47 -0700
Subject: [PATCH] Add force-torque determinism plugin and demo world (#10)

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 .../ft_sensor_determinism/CMakeLists.txt      |  21 +
 .../FTSensorDeterminism.cc                    | 133 ++++
 .../FTSensorDeterminism.hh                    |  56 ++
 .../plugins/ft_sensor_determinism/README.md   |  75 ++
 .../ft_sensor_determinism.sdf                 | 705 ++++++++++++++++++
 5 files changed, 990 insertions(+)
 create mode 100644 ionic_demo/plugins/ft_sensor_determinism/CMakeLists.txt
 create mode 100644 ionic_demo/plugins/ft_sensor_determinism/FTSensorDeterminism.cc
 create mode 100644 ionic_demo/plugins/ft_sensor_determinism/FTSensorDeterminism.hh
 create mode 100644 ionic_demo/plugins/ft_sensor_determinism/README.md
 create mode 100644 ionic_demo/plugins/ft_sensor_determinism/ft_sensor_determinism.sdf

diff --git a/ionic_demo/plugins/ft_sensor_determinism/CMakeLists.txt b/ionic_demo/plugins/ft_sensor_determinism/CMakeLists.txt
new file mode 100644
index 0000000..ec40dde
--- /dev/null
+++ b/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})
diff --git a/ionic_demo/plugins/ft_sensor_determinism/FTSensorDeterminism.cc b/ionic_demo/plugins/ft_sensor_determinism/FTSensorDeterminism.cc
new file mode 100644
index 0000000..8344b05
--- /dev/null
+++ b/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';
+    }
+  }
+}
diff --git a/ionic_demo/plugins/ft_sensor_determinism/FTSensorDeterminism.hh b/ionic_demo/plugins/ft_sensor_determinism/FTSensorDeterminism.hh
new file mode 100644
index 0000000..5cb1563
--- /dev/null
+++ b/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
diff --git a/ionic_demo/plugins/ft_sensor_determinism/README.md b/ionic_demo/plugins/ft_sensor_determinism/README.md
new file mode 100644
index 0000000..3da3630
--- /dev/null
+++ b/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
+~~~
diff --git a/ionic_demo/plugins/ft_sensor_determinism/ft_sensor_determinism.sdf b/ionic_demo/plugins/ft_sensor_determinism/ft_sensor_determinism.sdf
new file mode 100644
index 0000000..0a14556
--- /dev/null
+++ b/ionic_demo/plugins/ft_sensor_determinism/ft_sensor_determinism.sdf
@@ -0,0 +1,705 @@
+<?xml version="1.0" ?>
+<sdf version="1.11">
+  <!--
+    Gazebo force-torque sensor determinism demo
+
+    An extension of the mimic constraint demo
+
+    Currently requires bullet-feathersone plugin and can be run with the
+    following command:
+
+    gz sim -v 4 ft_sensor_determinism.sdf
+  -->
+  <world name="default">
+    <physics name="1ms" type="ode">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>0</real_time_factor>
+    </physics>
+
+    <plugin filename="gz-sim-physics-system" name="gz::sim::systems::Physics">
+      <engine><filename>gz-physics-bullet-featherstone-plugin</filename></engine>
+    </plugin>
+
+    <plugin
+      filename="gz-sim-forcetorque-system"
+      name="gz::sim::systems::ForceTorque"/>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <model name="pendulum_with_base">
+      <pose>0 0 0 0 0 0</pose>
+      <link name="base">
+        <inertial>
+          <mass>100</mass>
+        </inertial>
+        <visual name="vis_plate_on_ground">
+          <pose>0 0 0.01 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.8</radius>
+              <length>0.02</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <visual name="vis_pole">
+          <pose>-0.275 0 1.1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.2 0.2 2.2</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <collision name="col_plate_on_ground">
+          <pose>0 0 0.01 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.8</radius>
+              <length>0.02</length>
+            </cylinder>
+          </geometry>
+        </collision>
+        <collision name="col_pole">
+          <pose>-0.275 0 1.1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.2 0.2 2.2</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+
+      <!-- slow link, length 1, IC -90 degrees -->
+      <link name="slow_link">
+        <pose>0 0 2.1 -1.5708 0 0</pose>
+        <self_collide>0</self_collide>
+        <inertial>
+          <mass>1.0</mass>
+          <pose>0 0 0.5 0 0 0</pose>
+          <inertia>
+            <ixx>0.087</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.087</iyy>
+            <iyz>0</iyz>
+            <izz>0.006</izz>
+          </inertia>
+        </inertial>
+        <visual name="vis_slow_joint">
+          <pose>-0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <visual name="vis_cylinder">
+          <pose>0 0 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>1.0</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <collision name="col_slow_joint">
+          <pose>-0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+        </collision>
+        <collision name="col_cylinder">
+          <pose>0 0 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>1.0</length>
+            </cylinder>
+          </geometry>
+        </collision>
+      </link>
+
+      <!-- fast link, length 0.5, IC -90 degrees -->
+      <link name="fast_link">
+        <pose>-0.5 0 2.1 -1.5708 0 0</pose>
+        <self_collide>0</self_collide>
+        <inertial>
+          <mass>1.0</mass>
+          <pose>0 0 0.25 0 0 0</pose>
+          <inertia>
+            <ixx>0.024</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.024</iyy>
+            <iyz>0</iyz>
+            <izz>0.006</izz>
+          </inertia>
+        </inertial>
+        <visual name="vis_fast_joint">
+          <pose>0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <visual name="vis_cylinder_2">
+          <pose>0 0 0.25 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.5</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <collision name="col_fast_joint">
+          <pose>0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+        </collision>
+        <collision name="col_cylinder_2">
+          <pose>0 0 0.2 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.5</length>
+            </cylinder>
+          </geometry>
+        </collision>
+      </link>
+
+      <joint name="slow_joint" type="revolute">
+        <parent>base</parent>
+        <child>slow_link</child>
+        <axis>
+          <xyz>-1.0 0 0</xyz>
+        </axis>
+        <sensor name="ft_slow_joint" type="force_torque">
+          <update_rate>0</update_rate>
+          <plugin
+            filename="FTSensorDeterminism"
+            name="ft_sensor_determinism::FTSensorDeterminism"/>
+        </sensor>
+      </joint>
+
+      <joint name="fast_joint" type="revolute">
+        <parent>base</parent>
+        <child>fast_link</child>
+        <axis>
+          <xyz>-1.0 0 0</xyz>
+        </axis>
+        <sensor name="ft_fast_joint" type="force_torque">
+          <update_rate>0</update_rate>
+          <plugin
+            filename="FTSensorDeterminism"
+            name="ft_sensor_determinism::FTSensorDeterminism"/>
+        </sensor>
+      </joint>
+
+    </model>
+
+    <model name="pendulum_with_base_mimic_slow_follows_fast">
+      <pose>0 3 0 0 0 0</pose>
+      <link name="base">
+        <inertial>
+          <mass>100</mass>
+        </inertial>
+        <visual name="vis_plate_on_ground">
+          <pose>0 0 0.01 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.8</radius>
+              <length>0.02</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <visual name="vis_pole">
+          <pose>-0.275 0 1.1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.2 0.2 2.2</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <collision name="col_plate_on_ground">
+          <pose>0 0 0.01 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.8</radius>
+              <length>0.02</length>
+            </cylinder>
+          </geometry>
+        </collision>
+        <collision name="col_pole">
+          <pose>-0.275 0 1.1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.2 0.2 2.2</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+
+      <!-- slow link, length 1, IC -90 degrees -->
+      <link name="slow_link">
+        <pose>0 0 2.1 -1.5708 0 0</pose>
+        <self_collide>0</self_collide>
+        <inertial>
+          <mass>1.0</mass>
+          <pose>0 0 0.5 0 0 0</pose>
+          <inertia>
+            <ixx>0.087</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.087</iyy>
+            <iyz>0</iyz>
+            <izz>0.006</izz>
+          </inertia>
+        </inertial>
+        <visual name="vis_slow_joint">
+          <pose>-0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <visual name="vis_cylinder">
+          <pose>0 0 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>1.0</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <collision name="col_slow_joint">
+          <pose>-0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+        </collision>
+        <collision name="col_cylinder">
+          <pose>0 0 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>1.0</length>
+            </cylinder>
+          </geometry>
+        </collision>
+      </link>
+
+      <!-- fast link, length 0.5, IC -90 degrees -->
+      <link name="fast_link">
+        <pose>-0.5 0 2.1 -1.5708 0 0</pose>
+        <self_collide>0</self_collide>
+        <inertial>
+          <mass>1.0</mass>
+          <pose>0 0 0.25 0 0 0</pose>
+          <inertia>
+            <ixx>0.024</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.024</iyy>
+            <iyz>0</iyz>
+            <izz>0.006</izz>
+          </inertia>
+        </inertial>
+        <visual name="vis_fast_joint">
+          <pose>0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <visual name="vis_cylinder_2">
+          <pose>0 0 0.25 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.5</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <collision name="col_fast_joint">
+          <pose>0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+        </collision>
+        <collision name="col_cylinder_2">
+          <pose>0 0 0.2 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.5</length>
+            </cylinder>
+          </geometry>
+        </collision>
+      </link>
+
+      <joint name="slow_joint" type="revolute">
+        <parent>base</parent>
+        <child>slow_link</child>
+        <axis>
+          <xyz>-1.0 0 0</xyz>
+          <mimic joint="fast_joint" axis="axis">
+            <multiplier>1</multiplier>
+          </mimic>
+        </axis>
+        <sensor name="ft_slow_joint" type="force_torque">
+          <update_rate>0</update_rate>
+          <plugin
+            filename="FTSensorDeterminism"
+            name="ft_sensor_determinism::FTSensorDeterminism"/>
+        </sensor>
+      </joint>
+
+      <joint name="fast_joint" type="revolute">
+        <parent>base</parent>
+        <child>fast_link</child>
+        <axis>
+          <xyz>-1.0 0 0</xyz>
+        </axis>
+        <sensor name="ft_fast_joint" type="force_torque">
+          <update_rate>0</update_rate>
+          <plugin
+            filename="FTSensorDeterminism"
+            name="ft_sensor_determinism::FTSensorDeterminism"/>
+        </sensor>
+      </joint>
+
+    </model>
+
+    <model name="pendulum_with_base_mimic_fast_follows_slow">
+      <pose>0 -3 0 0 0 0</pose>
+      <link name="base">
+        <inertial>
+          <mass>100</mass>
+        </inertial>
+        <visual name="vis_plate_on_ground">
+          <pose>0 0 0.01 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.8</radius>
+              <length>0.02</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <visual name="vis_pole">
+          <pose>-0.275 0 1.1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.2 0.2 2.2</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <collision name="col_plate_on_ground">
+          <pose>0 0 0.01 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.8</radius>
+              <length>0.02</length>
+            </cylinder>
+          </geometry>
+        </collision>
+        <collision name="col_pole">
+          <pose>-0.275 0 1.1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.2 0.2 2.2</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+
+      <!-- slow link, length 1, IC -90 degrees -->
+      <link name="slow_link">
+        <pose>0 0 2.1 -1.5708 0 0</pose>
+        <self_collide>0</self_collide>
+        <inertial>
+          <mass>1.0</mass>
+          <pose>0 0 0.5 0 0 0</pose>
+          <inertia>
+            <ixx>0.087</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.087</iyy>
+            <iyz>0</iyz>
+            <izz>0.006</izz>
+          </inertia>
+        </inertial>
+        <visual name="vis_slow_joint">
+          <pose>-0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <visual name="vis_cylinder">
+          <pose>0 0 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>1.0</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <collision name="col_slow_joint">
+          <pose>-0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+        </collision>
+        <collision name="col_cylinder">
+          <pose>0 0 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>1.0</length>
+            </cylinder>
+          </geometry>
+        </collision>
+      </link>
+
+      <!-- fast link, length 0.5, IC -90 degrees -->
+      <link name="fast_link">
+        <pose>-0.5 0 2.1 -1.5708 0 0</pose>
+        <self_collide>0</self_collide>
+        <inertial>
+          <mass>1.0</mass>
+          <pose>0 0 0.25 0 0 0</pose>
+          <inertia>
+            <ixx>0.024</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.024</iyy>
+            <iyz>0</iyz>
+            <izz>0.006</izz>
+          </inertia>
+        </inertial>
+        <visual name="vis_fast_joint">
+          <pose>0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <visual name="vis_cylinder_2">
+          <pose>0 0 0.25 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.5</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+        <collision name="col_fast_joint">
+          <pose>0.05 0 0 0 1.5708 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.3</length>
+            </cylinder>
+          </geometry>
+        </collision>
+        <collision name="col_cylinder_2">
+          <pose>0 0 0.2 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.1</radius>
+              <length>0.5</length>
+            </cylinder>
+          </geometry>
+        </collision>
+      </link>
+
+      <joint name="slow_joint" type="revolute">
+        <parent>base</parent>
+        <child>slow_link</child>
+        <axis>
+          <xyz>-1.0 0 0</xyz>
+        </axis>
+        <sensor name="ft_slow_joint" type="force_torque">
+          <update_rate>0</update_rate>
+          <plugin
+            filename="FTSensorDeterminism"
+            name="ft_sensor_determinism::FTSensorDeterminism"/>
+        </sensor>
+      </joint>
+
+      <joint name="fast_joint" type="revolute">
+        <parent>base</parent>
+        <child>fast_link</child>
+        <axis>
+          <xyz>-1.0 0 0</xyz>
+          <mimic joint="slow_joint" axis="axis">
+            <multiplier>1</multiplier>
+          </mimic>
+        </axis>
+        <sensor name="ft_fast_joint" type="force_torque">
+          <update_rate>0</update_rate>
+          <plugin
+            filename="FTSensorDeterminism"
+            name="ft_sensor_determinism::FTSensorDeterminism"/>
+        </sensor>
+      </joint>
+
+    </model>
+
+  </world>
+</sdf>