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 +#include +#include + +#include + +// This header is required to register plugins. It's good practice to place it +// in the cc file, like it's done here. +#include + +#include +#include "gz/sim/components/WrenchMeasured.hh" +#include + +// 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 &_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 lock(this->mutex); + this->wrenchFromTopic = _msg; +} + +////////////////////////////////////////////////// +double wrenchTimeStamp(const gz::msgs::Wrench &_msg) +{ + return std::chrono::duration( + gz::msgs::Convert(_msg.header().stamp())).count(); +} + +////////////////////////////////////////////////// +void FTSensorDeterminism::Update(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) +{ + double simTime = std::chrono::duration(_info.simTime).count(); + + auto wrenchComponent = + _ecm.Component(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 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 +#include +#include +#include +#include +#include + +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 &_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 wrenchFromECM; + private: std::optional 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 @@ + + + + + + 0.001 + 0 + + + + gz-physics-bullet-featherstone-plugin + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0 0 0 0 + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 1.0 + 0 0 0.5 0 0 0 + + 0.087 + 0 + 0 + 0.087 + 0 + 0.006 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 1.0 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 1.0 + + + + + + + + -0.5 0 2.1 -1.5708 0 0 + 0 + + 1.0 + 0 0 0.25 0 0 0 + + 0.024 + 0 + 0 + 0.024 + 0 + 0.006 + + + + 0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.25 0 0 0 + + + 0.1 + 0.5 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 0.2 0 0 0 + + + 0.1 + 0.5 + + + + + + + base + slow_link + + -1.0 0 0 + + + 0 + + + + + + base + fast_link + + -1.0 0 0 + + + 0 + + + + + + + + 0 3 0 0 0 0 + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 1.0 + 0 0 0.5 0 0 0 + + 0.087 + 0 + 0 + 0.087 + 0 + 0.006 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 1.0 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 1.0 + + + + + + + + -0.5 0 2.1 -1.5708 0 0 + 0 + + 1.0 + 0 0 0.25 0 0 0 + + 0.024 + 0 + 0 + 0.024 + 0 + 0.006 + + + + 0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.25 0 0 0 + + + 0.1 + 0.5 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 0.2 0 0 0 + + + 0.1 + 0.5 + + + + + + + base + slow_link + + -1.0 0 0 + + 1 + + + + 0 + + + + + + base + fast_link + + -1.0 0 0 + + + 0 + + + + + + + + 0 -3 0 0 0 0 + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 1.0 + 0 0 0.5 0 0 0 + + 0.087 + 0 + 0 + 0.087 + 0 + 0.006 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 1.0 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 1.0 + + + + + + + + -0.5 0 2.1 -1.5708 0 0 + 0 + + 1.0 + 0 0 0.25 0 0 0 + + 0.024 + 0 + 0 + 0.024 + 0 + 0.006 + + + + 0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.25 0 0 0 + + + 0.1 + 0.5 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 0.2 0 0 0 + + + 0.1 + 0.5 + + + + + + + base + slow_link + + -1.0 0 0 + + + 0 + + + + + + base + fast_link + + -1.0 0 0 + + 1 + + + + 0 + + + + + + + +