From 6c548ca200fc105a29ef9610f4ee1944d9099048 Mon Sep 17 00:00:00 2001 From: Eraimondi Date: Mon, 20 Apr 2026 09:28:13 +0200 Subject: [PATCH 01/19] Add VirtualGantryPlugin for humanoid simulation stability Introduces a PD spring-damper plugin that holds a configurable body (default: torso_link) at its spawn-time world-frame position. Intended as a safety net during policy startup and after simulation resets, so humanoid robots do not fall before the locomotion policy is active. - mujoco_ros2_control_msgs: add SetGantryEnabled and SetGantryTarget service definitions (depends on geometry_msgs for Point) - mujoco_ros2_control_plugins: add VirtualGantryPlugin implementing the PD controller via xfrc_applied; exposes set_gantry_enabled and set_gantry_target ROS 2 services on the plugin sub-namespace Co-Authored-By: Claude Sonnet 4.6 --- mujoco_ros2_control_msgs/CMakeLists.txt | 4 + mujoco_ros2_control_msgs/package.xml | 1 + .../srv/SetGantryEnabled.srv | 4 + .../srv/SetGantryTarget.srv | 4 + mujoco_ros2_control_plugins/CMakeLists.txt | 9 +- .../virtual_gantry_plugin.hpp | 60 ++++++++ .../mujoco_ros2_control_plugins.xml | 8 ++ mujoco_ros2_control_plugins/package.xml | 2 + .../src/virtual_gantry_plugin.cpp | 132 ++++++++++++++++++ 9 files changed, 222 insertions(+), 2 deletions(-) create mode 100644 mujoco_ros2_control_msgs/srv/SetGantryEnabled.srv create mode 100644 mujoco_ros2_control_msgs/srv/SetGantryTarget.srv create mode 100644 mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp create mode 100644 mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp diff --git a/mujoco_ros2_control_msgs/CMakeLists.txt b/mujoco_ros2_control_msgs/CMakeLists.txt index 76d0f9e2..a205cc9e 100644 --- a/mujoco_ros2_control_msgs/CMakeLists.txt +++ b/mujoco_ros2_control_msgs/CMakeLists.txt @@ -4,11 +4,14 @@ project(mujoco_ros2_control_msgs) find_package(ament_cmake REQUIRED) find_package(builtin_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) set(srv_files srv/ResetWorld.srv + srv/SetGantryEnabled.srv + srv/SetGantryTarget.srv srv/SetPause.srv srv/StepSimulation.srv ) @@ -18,6 +21,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} ${srv_files} DEPENDENCIES builtin_interfaces + geometry_msgs ) ament_export_dependencies(rosidl_default_runtime) ament_package() diff --git a/mujoco_ros2_control_msgs/package.xml b/mujoco_ros2_control_msgs/package.xml index 114cbaf2..669cd6d8 100644 --- a/mujoco_ros2_control_msgs/package.xml +++ b/mujoco_ros2_control_msgs/package.xml @@ -21,6 +21,7 @@ rosidl_default_generators builtin_interfaces + geometry_msgs rosidl_default_runtime diff --git a/mujoco_ros2_control_msgs/srv/SetGantryEnabled.srv b/mujoco_ros2_control_msgs/srv/SetGantryEnabled.srv new file mode 100644 index 00000000..ec42aae0 --- /dev/null +++ b/mujoco_ros2_control_msgs/srv/SetGantryEnabled.srv @@ -0,0 +1,4 @@ +bool enabled +--- +bool success +string message diff --git a/mujoco_ros2_control_msgs/srv/SetGantryTarget.srv b/mujoco_ros2_control_msgs/srv/SetGantryTarget.srv new file mode 100644 index 00000000..7cf95d27 --- /dev/null +++ b/mujoco_ros2_control_msgs/srv/SetGantryTarget.srv @@ -0,0 +1,4 @@ +geometry_msgs/Point target_position +--- +bool success +string message diff --git a/mujoco_ros2_control_plugins/CMakeLists.txt b/mujoco_ros2_control_plugins/CMakeLists.txt index 5fe232a2..e1262d01 100644 --- a/mujoco_ros2_control_plugins/CMakeLists.txt +++ b/mujoco_ros2_control_plugins/CMakeLists.txt @@ -7,6 +7,8 @@ export_windows_symbols() # find dependencies find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(mujoco_ros2_control_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(pluginlib REQUIRED) find_package(std_msgs REQUIRED) @@ -22,9 +24,10 @@ else() endif() -# Build the heartbeat publisher plugin +# Build the plugin library add_library(mujoco_ros2_control_plugins SHARED src/heartbeat_publisher_plugin.cpp + src/virtual_gantry_plugin.cpp ) target_include_directories(mujoco_ros2_control_plugins PUBLIC $ @@ -38,6 +41,8 @@ target_link_libraries(mujoco_ros2_control_plugins PUBLIC ${MUJOCO_LIB} rclcpp::rclcpp pluginlib::pluginlib + ${geometry_msgs_TARGETS} + ${mujoco_ros2_control_msgs_TARGETS} ${std_msgs_TARGETS} ) @@ -64,7 +69,7 @@ install(DIRECTORY include/ pluginlib_export_plugin_description_file(mujoco_ros2_control_plugins mujoco_ros2_control_plugins.xml) ament_export_include_directories(include) -ament_export_dependencies(mujoco_vendor rclcpp pluginlib std_msgs ros2_control_cmake) +ament_export_dependencies(geometry_msgs mujoco_ros2_control_msgs mujoco_vendor rclcpp pluginlib std_msgs ros2_control_cmake) ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_package() diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp new file mode 100644 index 00000000..f00635a6 --- /dev/null +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp @@ -0,0 +1,60 @@ +// Copyright 2026 NVIDIA CORPORATION & AFFILIATES +// +// 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 MUJOCO_ROS2_CONTROL_PLUGINS__VIRTUAL_GANTRY_PLUGIN_HPP_ +#define MUJOCO_ROS2_CONTROL_PLUGINS__VIRTUAL_GANTRY_PLUGIN_HPP_ + +#include +#include +#include + +#include "mujoco_ros2_control_msgs/srv/set_gantry_enabled.hpp" +#include "mujoco_ros2_control_msgs/srv/set_gantry_target.hpp" +#include "mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace mujoco_ros2_control_plugins +{ + +// PD spring-damper that holds a body at a fixed world-frame position. +// Acts as a virtual gantry: enabled by default, disabled once the policy is stable. +// The hold target is captured from the body's position at simulation startup. +class VirtualGantryPlugin : public MuJoCoROS2ControlPluginBase +{ +public: + bool init(rclcpp::Node::SharedPtr node, const mjModel * model, mjData * data) override; + void update(const mjModel * model, mjData * data) override; + void cleanup() override; + +private: + rclcpp::Node::SharedPtr node_; + + std::string body_name_{"torso_link"}; + int body_id_{-1}; + + double kp_pos_{200.0}; + double kd_pos_{20.0}; + + std::array target_pos_{}; + std::mutex target_mutex_; + + bool enabled_{true}; + + rclcpp::Service::SharedPtr enable_srv_; + rclcpp::Service::SharedPtr target_srv_; +}; + +} // namespace mujoco_ros2_control_plugins + +#endif // MUJOCO_ROS2_CONTROL_PLUGINS__VIRTUAL_GANTRY_PLUGIN_HPP_ diff --git a/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins.xml b/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins.xml index 7a9a813a..1ad1f4ba 100644 --- a/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins.xml +++ b/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins.xml @@ -6,4 +6,12 @@ A simple plugin that publishes a heartbeat message every second to demonstrate the plugin system. + + + PD spring-damper that holds a body at its spawn-time world-frame position. + Used to keep humanoid robots upright during policy startup and after simulation resets. + + diff --git a/mujoco_ros2_control_plugins/package.xml b/mujoco_ros2_control_plugins/package.xml index 19b4423a..9ae385f1 100644 --- a/mujoco_ros2_control_plugins/package.xml +++ b/mujoco_ros2_control_plugins/package.xml @@ -18,6 +18,8 @@ ament_cmake + geometry_msgs + mujoco_ros2_control_msgs mujoco_vendor rclcpp ros2_control_cmake diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp new file mode 100644 index 00000000..b2884957 --- /dev/null +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -0,0 +1,132 @@ +// Copyright 2026 NVIDIA CORPORATION & AFFILIATES +// +// 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. + +#include "mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp" + +#include +#include + +namespace mujoco_ros2_control_plugins +{ + +bool VirtualGantryPlugin::init( + rclcpp::Node::SharedPtr node, const mjModel * model, mjData * data) +{ + node_ = node; + + // Parameters are read from the parent node's "mujoco_plugins..*" namespace, + // which is already declared via automatically_declare_parameters_from_overrides. We access + // them through the shared NodeParametersInterface to bypass sub-namespace prepending. + const std::string prefix = "mujoco_plugins." + node->get_sub_namespace() + "."; + auto params = node->get_node_parameters_interface(); + + if (params->has_parameter(prefix + "body_name")) { + body_name_ = params->get_parameter(prefix + "body_name").get_parameter_value().get(); + } + if (params->has_parameter(prefix + "kp_pos")) { + kp_pos_ = params->get_parameter(prefix + "kp_pos").get_parameter_value().get(); + } + if (params->has_parameter(prefix + "kd_pos")) { + kd_pos_ = params->get_parameter(prefix + "kd_pos").get_parameter_value().get(); + } + + body_id_ = mj_name2id(model, mjOBJ_BODY, body_name_.c_str()); + if (body_id_ < 0) { + RCLCPP_ERROR(node_->get_logger(), "VirtualGantryPlugin: body '%s' not found in model", + body_name_.c_str()); + return false; + } + + // Capture spawn position as the hold target. + target_pos_ = {{ + data->xpos[body_id_ * 3 + 0], + data->xpos[body_id_ * 3 + 1], + data->xpos[body_id_ * 3 + 2], + }}; + + RCLCPP_INFO(node_->get_logger(), + "VirtualGantryPlugin: holding '%s' (id=%d) at [%.3f, %.3f, %.3f], " + "kp=%.1f, kd=%.1f", + body_name_.c_str(), body_id_, + target_pos_[0], target_pos_[1], target_pos_[2], + kp_pos_, kd_pos_); + + enable_srv_ = node_->create_service( + "set_gantry_enabled", + [this]( + const mujoco_ros2_control_msgs::srv::SetGantryEnabled::Request::SharedPtr req, + mujoco_ros2_control_msgs::srv::SetGantryEnabled::Response::SharedPtr resp) + { + std::lock_guard lock(target_mutex_); + enabled_ = req->enabled; + resp->success = true; + resp->message = enabled_ ? "Gantry enabled" : "Gantry disabled"; + RCLCPP_INFO(node_->get_logger(), "%s", resp->message.c_str()); + }); + + target_srv_ = node_->create_service( + "set_gantry_target", + [this]( + const mujoco_ros2_control_msgs::srv::SetGantryTarget::Request::SharedPtr req, + mujoco_ros2_control_msgs::srv::SetGantryTarget::Response::SharedPtr resp) + { + std::lock_guard lock(target_mutex_); + target_pos_ = {{req->target_position.x, req->target_position.y, req->target_position.z}}; + resp->success = true; + resp->message = "Gantry target updated"; + RCLCPP_INFO(node_->get_logger(), "Gantry target set to [%.3f, %.3f, %.3f]", + target_pos_[0], target_pos_[1], target_pos_[2]); + }); + + return true; +} + +void VirtualGantryPlugin::update(const mjModel * /*model*/, mjData * data) +{ + std::lock_guard lock(target_mutex_); + + if (!enabled_) { + // Clear any residual forces when disabled. + data->xfrc_applied[body_id_ * 6 + 3] = 0.0; + data->xfrc_applied[body_id_ * 6 + 4] = 0.0; + data->xfrc_applied[body_id_ * 6 + 5] = 0.0; + return; + } + + // Linear velocity of the body (cvel layout: [angular(3), linear(3)] per body). + const double vx = data->cvel[body_id_ * 6 + 3]; + const double vy = data->cvel[body_id_ * 6 + 4]; + const double vz = data->cvel[body_id_ * 6 + 5]; + + // PD forces: spring pulls toward target, damper opposes velocity. + data->xfrc_applied[body_id_ * 6 + 3] = + kp_pos_ * (target_pos_[0] - data->xpos[body_id_ * 3 + 0]) - kd_pos_ * vx; + data->xfrc_applied[body_id_ * 6 + 4] = + kp_pos_ * (target_pos_[1] - data->xpos[body_id_ * 3 + 1]) - kd_pos_ * vy; + data->xfrc_applied[body_id_ * 6 + 5] = + kp_pos_ * (target_pos_[2] - data->xpos[body_id_ * 3 + 2]) - kd_pos_ * vz; +} + +void VirtualGantryPlugin::cleanup() +{ + enable_srv_.reset(); + target_srv_.reset(); + node_.reset(); +} + +} // namespace mujoco_ros2_control_plugins + +PLUGINLIB_EXPORT_CLASS( + mujoco_ros2_control_plugins::VirtualGantryPlugin, + mujoco_ros2_control_plugins::MuJoCoROS2ControlPluginBase) From 495367648ca9df227a1c6ae98727a426203f0dac Mon Sep 17 00:00:00 2001 From: Eraimondi Date: Mon, 20 Apr 2026 10:34:51 +0200 Subject: [PATCH 02/19] Fix VirtualGantryPlugin: propagate xfrc_applied to physics thread and stabilize PD gains MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - mujoco_system_interface: copy xfrc_applied from mj_data_control_ to mj_data_ at all three mj_step call sites, matching the existing pattern for ctrl and qfrc_applied. Without this the plugin forces were written to the control buffer but never reached the physics simulation. - mujoco_ros2_control_plugins_base: add virtual reset() hook (default no-op) so plugins can clear accumulated state on world reset. - VirtualGantryPlugin: fix xfrc_applied indices (+3..+5 were torques, forces are at +0..+2); defer spawn-position capture to first update() to avoid a race with mj_forward() in the physics thread; implement reset() to re-arm spawn capture on world reset; tune PD gains to kp=10000 kd=3000 for overdamped behaviour (ζ≈2.5) with ~3 cm steady-state gravity offset; clean up comments and unused parameters. Co-Authored-By: Claude Sonnet 4.6 --- .../src/mujoco_system_interface.cpp | 9 +++ .../mujoco_ros2_control_plugins_base.hpp | 7 +++ .../virtual_gantry_plugin.hpp | 17 ++++-- .../src/virtual_gantry_plugin.cpp | 59 +++++++++++-------- 4 files changed, 62 insertions(+), 30 deletions(-) diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 860443fe..3596fb4b 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -2896,6 +2896,12 @@ void MujocoSystemInterface::reset_simulation_state(bool fill_initial_state) joint.velocity_interface.command_ = 0.0; joint.effort_interface.command_ = 0.0; } + + // Notify plugins so they can clear accumulated state (e.g. integrators). + for (auto& plugin : plugin_instances_) + { + plugin->reset(); + } } void MujocoSystemInterface::reset_world_callback( @@ -3123,6 +3129,7 @@ void MujocoSystemInterface::PhysicsLoop() // Copy data to the control mju_copy(mj_data_->ctrl, mj_data_control_->ctrl, static_cast(mj_model_->nu)); mju_copy(mj_data_->qfrc_applied, mj_data_control_->qfrc_applied, static_cast(mj_model_->nu)); + mju_copy(mj_data_->xfrc_applied, mj_data_control_->xfrc_applied, 6 * static_cast(mj_model_->nbody)); // run single step, let next iteration deal with timing mj_step(mj_model_, mj_data_); @@ -3175,6 +3182,7 @@ void MujocoSystemInterface::PhysicsLoop() // Copy data to the control mju_copy(mj_data_->ctrl, mj_data_control_->ctrl, static_cast(mj_model_->nu)); mju_copy(mj_data_->qfrc_applied, mj_data_control_->qfrc_applied, static_cast(mj_model_->nu)); + mju_copy(mj_data_->xfrc_applied, mj_data_control_->xfrc_applied, 6 * static_cast(mj_model_->nbody)); // call mj_step mj_step(mj_model_, mj_data_); @@ -3231,6 +3239,7 @@ void MujocoSystemInterface::PhysicsLoop() { mju_copy(mj_data_->ctrl, mj_data_control_->ctrl, static_cast(mj_model_->nu)); mju_copy(mj_data_->qfrc_applied, mj_data_control_->qfrc_applied, static_cast(mj_model_->nu)); + mju_copy(mj_data_->xfrc_applied, mj_data_control_->xfrc_applied, 6 * static_cast(mj_model_->nbody)); mj_step(mj_model_, mj_data_); publish_clock(); diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp index ce866665..225e3676 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp @@ -61,6 +61,13 @@ class MuJoCoROS2ControlPluginBase * @brief Cleanup the plugin */ virtual void cleanup() = 0; + + /** + * @brief Called when the simulation world is reset (e.g. Backspace in the viewer). + * @note Override to clear any accumulated state (e.g. integrators) that should not + * carry over across resets. + */ + virtual void reset() {} }; } // namespace mujoco_ros2_control_plugins diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp index f00635a6..8754d86e 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp @@ -27,14 +27,18 @@ namespace mujoco_ros2_control_plugins { -// PD spring-damper that holds a body at a fixed world-frame position. -// Acts as a virtual gantry: enabled by default, disabled once the policy is stable. -// The hold target is captured from the body's position at simulation startup. +// PD spring-damper that holds a body at a fixed world-frame XYZ position. +// Intended as a safety net during policy startup: keeps a humanoid robot upright +// while joints are uncontrolled, without interfering once the policy is active. +// Enable/disable and retarget at runtime via the set_gantry_enabled / +// set_gantry_target ROS 2 services. The hold position is captured automatically +// from the body's world-frame coordinates on the first simulation step. class VirtualGantryPlugin : public MuJoCoROS2ControlPluginBase { public: - bool init(rclcpp::Node::SharedPtr node, const mjModel * model, mjData * data) override; + bool init(rclcpp::Node::SharedPtr node, const mjModel * model, mjData * /*data*/) override; void update(const mjModel * model, mjData * data) override; + void reset() override; void cleanup() override; private: @@ -43,13 +47,14 @@ class VirtualGantryPlugin : public MuJoCoROS2ControlPluginBase std::string body_name_{"torso_link"}; int body_id_{-1}; - double kp_pos_{200.0}; - double kd_pos_{20.0}; + double kp_pos_{10000.0}; + double kd_pos_{3000.0}; std::array target_pos_{}; std::mutex target_mutex_; bool enabled_{true}; + bool spawn_pos_captured_{false}; rclcpp::Service::SharedPtr enable_srv_; rclcpp::Service::SharedPtr target_srv_; diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index b2884957..92e695a7 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -21,13 +21,13 @@ namespace mujoco_ros2_control_plugins { bool VirtualGantryPlugin::init( - rclcpp::Node::SharedPtr node, const mjModel * model, mjData * data) + rclcpp::Node::SharedPtr node, const mjModel * model, mjData * /*data*/) { node_ = node; - // Parameters are read from the parent node's "mujoco_plugins..*" namespace, - // which is already declared via automatically_declare_parameters_from_overrides. We access - // them through the shared NodeParametersInterface to bypass sub-namespace prepending. + // Parameters live in the parent node's "mujoco_plugins..*" namespace, + // declared via automatically_declare_parameters_from_overrides. Access them through + // the NodeParametersInterface directly to bypass sub-namespace prepending. const std::string prefix = "mujoco_plugins." + node->get_sub_namespace() + "."; auto params = node->get_node_parameters_interface(); @@ -48,19 +48,11 @@ bool VirtualGantryPlugin::init( return false; } - // Capture spawn position as the hold target. - target_pos_ = {{ - data->xpos[body_id_ * 3 + 0], - data->xpos[body_id_ * 3 + 1], - data->xpos[body_id_ * 3 + 2], - }}; - + // Spawn position is captured on the first update() call, not here, to avoid + // a race with mj_forward() which runs in a separate physics thread. RCLCPP_INFO(node_->get_logger(), - "VirtualGantryPlugin: holding '%s' (id=%d) at [%.3f, %.3f, %.3f], " - "kp=%.1f, kd=%.1f", - body_name_.c_str(), body_id_, - target_pos_[0], target_pos_[1], target_pos_[2], - kp_pos_, kd_pos_); + "VirtualGantryPlugin: will hold '%s' (id=%d) at spawn position, kp=%.1f kd=%.1f", + body_name_.c_str(), body_id_, kp_pos_, kd_pos_); enable_srv_ = node_->create_service( "set_gantry_enabled", @@ -96,11 +88,23 @@ void VirtualGantryPlugin::update(const mjModel * /*model*/, mjData * data) { std::lock_guard lock(target_mutex_); + // Defer spawn-position capture to the first update so mj_forward() has already run. + if (!spawn_pos_captured_) { + target_pos_ = {{ + data->xpos[body_id_ * 3 + 0], + data->xpos[body_id_ * 3 + 1], + data->xpos[body_id_ * 3 + 2], + }}; + spawn_pos_captured_ = true; + RCLCPP_INFO(node_->get_logger(), + "VirtualGantryPlugin: holding '%s' at [%.3f, %.3f, %.3f]", + body_name_.c_str(), target_pos_[0], target_pos_[1], target_pos_[2]); + } + if (!enabled_) { - // Clear any residual forces when disabled. - data->xfrc_applied[body_id_ * 6 + 3] = 0.0; - data->xfrc_applied[body_id_ * 6 + 4] = 0.0; - data->xfrc_applied[body_id_ * 6 + 5] = 0.0; + data->xfrc_applied[body_id_ * 6 + 0] = 0.0; + data->xfrc_applied[body_id_ * 6 + 1] = 0.0; + data->xfrc_applied[body_id_ * 6 + 2] = 0.0; return; } @@ -109,15 +113,22 @@ void VirtualGantryPlugin::update(const mjModel * /*model*/, mjData * data) const double vy = data->cvel[body_id_ * 6 + 4]; const double vz = data->cvel[body_id_ * 6 + 5]; - // PD forces: spring pulls toward target, damper opposes velocity. - data->xfrc_applied[body_id_ * 6 + 3] = + // xfrc_applied layout: [force(3), torque(3)] per body — write forces at +0..+2. + // cvel layout: [angular(3), linear(3)] per body — read linear velocity from +3..+5. + data->xfrc_applied[body_id_ * 6 + 0] = kp_pos_ * (target_pos_[0] - data->xpos[body_id_ * 3 + 0]) - kd_pos_ * vx; - data->xfrc_applied[body_id_ * 6 + 4] = + data->xfrc_applied[body_id_ * 6 + 1] = kp_pos_ * (target_pos_[1] - data->xpos[body_id_ * 3 + 1]) - kd_pos_ * vy; - data->xfrc_applied[body_id_ * 6 + 5] = + data->xfrc_applied[body_id_ * 6 + 2] = kp_pos_ * (target_pos_[2] - data->xpos[body_id_ * 3 + 2]) - kd_pos_ * vz; } +void VirtualGantryPlugin::reset() +{ + std::lock_guard lock(target_mutex_); + spawn_pos_captured_ = false; +} + void VirtualGantryPlugin::cleanup() { enable_srv_.reset(); From 0c505327290b71b92466ca31f509e5a92af607cf Mon Sep 17 00:00:00 2001 From: Eraimondi Date: Tue, 21 Apr 2026 14:22:31 +0200 Subject: [PATCH 03/19] Add GantryKeyboardState singleton for cross-thread key/scroll signalling Zero-dependency header-only singleton with two atomics: - toggle_counter: incremented by the GLFW adapter on each 'G' press - rope_scroll_ticks: accumulated Shift+scroll ticks VirtualGantryPlugin reads these in update() to toggle the gantry and adjust rope length without any mutex or inter-component coupling. --- .../gantry_keyboard_state.hpp | 47 +++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/gantry_keyboard_state.hpp diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/gantry_keyboard_state.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/gantry_keyboard_state.hpp new file mode 100644 index 00000000..4381f7d8 --- /dev/null +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/gantry_keyboard_state.hpp @@ -0,0 +1,47 @@ +// Copyright 2026 NVIDIA CORPORATION & AFFILIATES +// +// 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 MUJOCO_ROS2_CONTROL_PLUGINS__GANTRY_KEYBOARD_STATE_HPP_ +#define MUJOCO_ROS2_CONTROL_PLUGINS__GANTRY_KEYBOARD_STATE_HPP_ + +#include + +namespace mujoco_ros2_control_plugins +{ + +// Singleton that bridges the GLFW UI thread (key/scroll events) and the +// VirtualGantryPlugin physics thread. All fields are atomic — no mutex needed. +struct GantryKeyboardState +{ + // Incremented on each 'G' key press. Plugin compares against its last-seen + // value to detect a toggle edge without consuming the count. + std::atomic toggle_counter{0}; + + // Accumulated Shift+scroll ticks. Plugin exchanges to 0 after reading. + // Positive = scroll up (lengthen rope), negative = scroll down (shorten). + std::atomic rope_scroll_ticks{0}; + + static GantryKeyboardState & get() + { + static GantryKeyboardState instance; + return instance; + } + +private: + GantryKeyboardState() = default; +}; + +} // namespace mujoco_ros2_control_plugins + +#endif // MUJOCO_ROS2_CONTROL_PLUGINS__GANTRY_KEYBOARD_STATE_HPP_ From 5bfe6818f3f94de0da2f12e0739c708d246cd2a3 Mon Sep 17 00:00:00 2001 From: Eraimondi Date: Tue, 21 Apr 2026 14:22:49 +0200 Subject: [PATCH 04/19] Rewrite VirtualGantryPlugin with rope-constraint physics Replaces the PD spring-to-fixed-position with a one-sided cable constraint: - Anchor spawns anchor_height (2 m) directly above the configured attachment point on the first update() after enable or reset. - When distance from anchor to attachment exceeds rope_length, a radial-only tension force is applied toward the anchor. No lateral force is ever applied, so the robot swings freely like a pendulum. - Attachment point is configurable: body_name + body_offset (in body frame, default 0,0,0). - Keyboard toggle: reads GantryKeyboardState::toggle_counter each step; 'G' re-enables with a fresh anchor above the current position. - Rope length adjustment via GantryKeyboardState::rope_scroll_ticks (Shift+scroll, 5 cm per notch, minimum 0.1 m). - Rope capsule geom half-length updated each frame to match the current anchor-to-attachment distance. --- .../virtual_gantry_plugin.hpp | 53 +++- .../src/virtual_gantry_plugin.cpp | 266 ++++++++++++++---- 2 files changed, 251 insertions(+), 68 deletions(-) diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp index 8754d86e..3accbf1b 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp @@ -20,23 +20,30 @@ #include #include "mujoco_ros2_control_msgs/srv/set_gantry_enabled.hpp" -#include "mujoco_ros2_control_msgs/srv/set_gantry_target.hpp" #include "mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp" #include "rclcpp/rclcpp.hpp" namespace mujoco_ros2_control_plugins { -// PD spring-damper that holds a body at a fixed world-frame XYZ position. -// Intended as a safety net during policy startup: keeps a humanoid robot upright -// while joints are uncontrolled, without interfering once the policy is active. -// Enable/disable and retarget at runtime via the set_gantry_enabled / -// set_gantry_target ROS 2 services. The hold position is captured automatically -// from the body's world-frame coordinates on the first simulation step. +// Rope-constraint gantry that suspends a humanoid robot from a fixed anchor point. +// +// Physics model: a one-sided cable constraint. When the distance from the anchor +// to the configured attachment point on the robot exceeds rope_length_, a radial +// tension force is applied toward the anchor. No lateral force is ever applied, +// so the robot swings freely like a pendulum at all times. +// +// The anchor spawns anchor_height_ metres directly above the attachment point the +// first time update() runs (and after every re-enable). The rope_length_ defaults +// to anchor_height_ so the rope is just taut at spawn. +// +// 'G' in the MuJoCo viewer toggles the gantry on/off. +// Shift+scroll adjusts rope_length_ at 5 cm per scroll notch. +// The set_gantry_enabled ROS 2 service also enables/disables the gantry. class VirtualGantryPlugin : public MuJoCoROS2ControlPluginBase { public: - bool init(rclcpp::Node::SharedPtr node, const mjModel * model, mjData * /*data*/) override; + bool init(rclcpp::Node::SharedPtr node, const mjModel * model, mjData * data) override; void update(const mjModel * model, mjData * data) override; void reset() override; void cleanup() override; @@ -44,20 +51,40 @@ class VirtualGantryPlugin : public MuJoCoROS2ControlPluginBase private: rclcpp::Node::SharedPtr node_; + // Target body (attachment point on the robot). std::string body_name_{"torso_link"}; int body_id_{-1}; - double kp_pos_{10000.0}; - double kd_pos_{3000.0}; + // Offset from the body CoM to the rope attachment point, expressed in the body frame. + std::array body_offset_{0.0, 0.0, 0.0}; + + // Rope tension spring/damper gains. + double kp_pos_{50000.0}; + double kd_pos_{5000.0}; - std::array target_pos_{}; - std::mutex target_mutex_; + // Fixed anchor position (world frame); set once on first update after (re-)enable. + double anchor_height_{2.0}; + std::array anchor_pos_{}; + + // Maximum rope length. Defaults to anchor_height_ so the rope is taut at spawn. + double rope_length_{2.0}; bool enabled_{true}; bool spawn_pos_captured_{false}; + // mocap_id for the anchor sphere visual (-1 = not present in model). + int anchor_mocap_id_{-1}; + // mocap_id for the rope capsule visual (-1 = not present in model). + int rope_mocap_id_{-1}; + // geom index of the rope capsule (for dynamic half-length update, -1 = none). + int rope_geom_id_{-1}; + + // Last toggle_counter value seen; used to detect 'G' key edges. + int last_toggle_count_{0}; + + std::mutex state_mutex_; + rclcpp::Service::SharedPtr enable_srv_; - rclcpp::Service::SharedPtr target_srv_; }; } // namespace mujoco_ros2_control_plugins diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index 92e695a7..6f507f7d 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -14,9 +14,15 @@ #include "mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp" +#include +#include +#include + #include #include +#include "mujoco_ros2_control_plugins/gantry_keyboard_state.hpp" + namespace mujoco_ros2_control_plugins { @@ -26,33 +32,78 @@ bool VirtualGantryPlugin::init( node_ = node; // Parameters live in the parent node's "mujoco_plugins..*" namespace, - // declared via automatically_declare_parameters_from_overrides. Access them through - // the NodeParametersInterface directly to bypass sub-namespace prepending. + // declared via automatically_declare_parameters_from_overrides. const std::string prefix = "mujoco_plugins." + node->get_sub_namespace() + "."; auto params = node->get_node_parameters_interface(); - if (params->has_parameter(prefix + "body_name")) { - body_name_ = params->get_parameter(prefix + "body_name").get_parameter_value().get(); - } - if (params->has_parameter(prefix + "kp_pos")) { - kp_pos_ = params->get_parameter(prefix + "kp_pos").get_parameter_value().get(); - } - if (params->has_parameter(prefix + "kd_pos")) { - kd_pos_ = params->get_parameter(prefix + "kd_pos").get_parameter_value().get(); + auto get_double = [&](const std::string & key, double & out) { + if (params->has_parameter(prefix + key)) { + out = params->get_parameter(prefix + key).get_parameter_value().get(); + } + }; + auto get_string = [&](const std::string & key, std::string & out) { + if (params->has_parameter(prefix + key)) { + out = params->get_parameter(prefix + key).get_parameter_value().get(); + } + }; + + get_string("body_name", body_name_); + get_double("kp_pos", kp_pos_); + get_double("kd_pos", kd_pos_); + get_double("anchor_height", anchor_height_); + get_double("rope_length", rope_length_); + + if (params->has_parameter(prefix + "body_offset")) { + const auto v = params->get_parameter(prefix + "body_offset") + .get_parameter_value().get>(); + if (v.size() == 3) { + body_offset_ = {v[0], v[1], v[2]}; + } } body_id_ = mj_name2id(model, mjOBJ_BODY, body_name_.c_str()); if (body_id_ < 0) { - RCLCPP_ERROR(node_->get_logger(), "VirtualGantryPlugin: body '%s' not found in model", + RCLCPP_ERROR(node_->get_logger(), "VirtualGantryPlugin: body '%s' not found", body_name_.c_str()); return false; } - // Spawn position is captured on the first update() call, not here, to avoid - // a race with mj_forward() which runs in a separate physics thread. + // Resolve optional mocap visual bodies. + auto resolve_mocap = [&](const std::string & param_key, int & mocap_id_out) { + std::string name; + get_string(param_key, name); + if (name.empty()) {return;} + const int bid = mj_name2id(model, mjOBJ_BODY, name.c_str()); + if (bid >= 0 && model->body_mocapid[bid] >= 0) { + mocap_id_out = model->body_mocapid[bid]; + RCLCPP_INFO(node_->get_logger(), "VirtualGantryPlugin: %s '%s' (mocap_id=%d)", + param_key.c_str(), name.c_str(), mocap_id_out); + } else { + RCLCPP_WARN(node_->get_logger(), + "VirtualGantryPlugin: '%s' (param %s) not found or not a mocap body", + name.c_str(), param_key.c_str()); + } + }; + resolve_mocap("anchor_visual_body", anchor_mocap_id_); + resolve_mocap("rope_visual_body", rope_mocap_id_); + + // Find the rope capsule geom for dynamic half-length updates. + if (rope_mocap_id_ >= 0) { + std::string rope_name; + get_string("rope_visual_body", rope_name); + if (!rope_name.empty()) { + const int bid = mj_name2id(model, mjOBJ_BODY, rope_name.c_str()); + if (bid >= 0 && model->body_geomnum[bid] > 0) { + rope_geom_id_ = model->body_geomadr[bid]; + } + } + } + RCLCPP_INFO(node_->get_logger(), - "VirtualGantryPlugin: will hold '%s' (id=%d) at spawn position, kp=%.1f kd=%.1f", - body_name_.c_str(), body_id_, kp_pos_, kd_pos_); + "VirtualGantryPlugin: body '%s' (id=%d), anchor_height=%.2f, rope_length=%.2f, " + "offset=[%.3f,%.3f,%.3f], kp=%.0f, kd=%.0f", + body_name_.c_str(), body_id_, anchor_height_, rope_length_, + body_offset_[0], body_offset_[1], body_offset_[2], kp_pos_, kd_pos_); enable_srv_ = node_->create_service( "set_gantry_enabled", @@ -60,79 +111,184 @@ bool VirtualGantryPlugin::init( const mujoco_ros2_control_msgs::srv::SetGantryEnabled::Request::SharedPtr req, mujoco_ros2_control_msgs::srv::SetGantryEnabled::Response::SharedPtr resp) { - std::lock_guard lock(target_mutex_); + std::lock_guard lock(state_mutex_); + const bool was_enabled = enabled_; enabled_ = req->enabled; + if (enabled_ && !was_enabled) { + spawn_pos_captured_ = false; // re-anchor above current attach position + } resp->success = true; resp->message = enabled_ ? "Gantry enabled" : "Gantry disabled"; RCLCPP_INFO(node_->get_logger(), "%s", resp->message.c_str()); }); - target_srv_ = node_->create_service( - "set_gantry_target", - [this]( - const mujoco_ros2_control_msgs::srv::SetGantryTarget::Request::SharedPtr req, - mujoco_ros2_control_msgs::srv::SetGantryTarget::Response::SharedPtr resp) - { - std::lock_guard lock(target_mutex_); - target_pos_ = {{req->target_position.x, req->target_position.y, req->target_position.z}}; - resp->success = true; - resp->message = "Gantry target updated"; - RCLCPP_INFO(node_->get_logger(), "Gantry target set to [%.3f, %.3f, %.3f]", - target_pos_[0], target_pos_[1], target_pos_[2]); - }); + // Snapshot the current toggle counter so the first update() doesn't misfire. + last_toggle_count_ = GantryKeyboardState::get().toggle_counter.load( + std::memory_order_relaxed); return true; } -void VirtualGantryPlugin::update(const mjModel * /*model*/, mjData * data) +void VirtualGantryPlugin::update(const mjModel * model, mjData * data) { - std::lock_guard lock(target_mutex_); + std::lock_guard lock(state_mutex_); + + // --- Keyboard toggle ('G' key) ------------------------------------------- + const int tc = GantryKeyboardState::get().toggle_counter.load( + std::memory_order_relaxed); + if (tc != last_toggle_count_) { + last_toggle_count_ = tc; + enabled_ = !enabled_; + if (enabled_) { + spawn_pos_captured_ = false; // re-anchor above current position on next step + } + RCLCPP_INFO(node_->get_logger(), "VirtualGantryPlugin: %s via keyboard", + enabled_ ? "enabled" : "disabled"); + } + + // --- Rope length adjustment (Shift+scroll) -------------------------------- + const int scroll_ticks = + GantryKeyboardState::get().rope_scroll_ticks.exchange(0, std::memory_order_relaxed); + if (scroll_ticks != 0) { + rope_length_ = std::max(0.1, rope_length_ + scroll_ticks * 0.05); + RCLCPP_INFO(node_->get_logger(), "VirtualGantryPlugin: rope_length=%.3f m", rope_length_); + } + + // --- Compute attachment point in world frame ------------------------------ + // attach_pos = body CoM + rotation_matrix * body_offset + const double * xpos = &data->xpos[body_id_ * 3]; + const double * xmat = &data->xmat[body_id_ * 9]; - // Defer spawn-position capture to the first update so mj_forward() has already run. + double attach_pos[3]; + for (int i = 0; i < 3; ++i) { + attach_pos[i] = xpos[i] + + xmat[i * 3 + 0] * body_offset_[0] + + xmat[i * 3 + 1] * body_offset_[1] + + xmat[i * 3 + 2] * body_offset_[2]; + } + + // --- Capture anchor on first step after (re-)enable ---------------------- if (!spawn_pos_captured_) { - target_pos_ = {{ - data->xpos[body_id_ * 3 + 0], - data->xpos[body_id_ * 3 + 1], - data->xpos[body_id_ * 3 + 2], - }}; + anchor_pos_ = {attach_pos[0], attach_pos[1], attach_pos[2] + anchor_height_}; spawn_pos_captured_ = true; RCLCPP_INFO(node_->get_logger(), - "VirtualGantryPlugin: holding '%s' at [%.3f, %.3f, %.3f]", - body_name_.c_str(), target_pos_[0], target_pos_[1], target_pos_[2]); + "VirtualGantryPlugin: anchor at [%.3f, %.3f, %.3f], rope_length=%.2f m", + anchor_pos_[0], anchor_pos_[1], anchor_pos_[2], rope_length_); } - if (!enabled_) { - data->xfrc_applied[body_id_ * 6 + 0] = 0.0; - data->xfrc_applied[body_id_ * 6 + 1] = 0.0; - data->xfrc_applied[body_id_ * 6 + 2] = 0.0; - return; + // --- Anchor visual -------------------------------------------------------- + if (anchor_mocap_id_ >= 0) { + data->mocap_pos[anchor_mocap_id_ * 3 + 0] = anchor_pos_[0]; + data->mocap_pos[anchor_mocap_id_ * 3 + 1] = anchor_pos_[1]; + data->mocap_pos[anchor_mocap_id_ * 3 + 2] = anchor_pos_[2]; } - // Linear velocity of the body (cvel layout: [angular(3), linear(3)] per body). + // --- Rope visual ---------------------------------------------------------- + // Rope vector from anchor to attachment point. + const double dx = attach_pos[0] - anchor_pos_[0]; + const double dy = attach_pos[1] - anchor_pos_[1]; + const double dz = attach_pos[2] - anchor_pos_[2]; + const double rope_dist = std::sqrt(dx * dx + dy * dy + dz * dz); + + if (rope_mocap_id_ >= 0 && rope_dist > 1e-6) { + // Midpoint between anchor and attachment. + data->mocap_pos[rope_mocap_id_ * 3 + 0] = (anchor_pos_[0] + attach_pos[0]) * 0.5; + data->mocap_pos[rope_mocap_id_ * 3 + 1] = (anchor_pos_[1] + attach_pos[1]) * 0.5; + data->mocap_pos[rope_mocap_id_ * 3 + 2] = (anchor_pos_[2] + attach_pos[2]) * 0.5; + + // Normalised direction: anchor → attachment. + const double ndx = dx / rope_dist; + const double ndy = dy / rope_dist; + const double ndz = dz / rope_dist; + + // Quaternion aligning capsule Z-axis with the anchor→attachment direction. + // Rotation axis = cross([0,0,1], [ndx,ndy,ndz]) = [-ndy, ndx, 0]. + const double ax = -ndy, ay = ndx; + const double axis_len = std::sqrt(ax * ax + ay * ay); + double qw, qx, qy, qz; + if (axis_len < 1e-6) { + // Direction is (anti-)parallel to Z. + if (ndz > 0.0) {qw = 1.0; qx = 0.0; qy = 0.0; qz = 0.0;} + else {qw = 0.0; qx = 1.0; qy = 0.0; qz = 0.0;} + } else { + const double clamped = ndz > 1.0 ? 1.0 : ndz < -1.0 ? -1.0 : ndz; + const double angle = std::acos(clamped); + const double sa = std::sin(angle * 0.5); + qw = std::cos(angle * 0.5); + qx = (ax / axis_len) * sa; + qy = (ay / axis_len) * sa; + qz = 0.0; + } + data->mocap_quat[rope_mocap_id_ * 4 + 0] = qw; + data->mocap_quat[rope_mocap_id_ * 4 + 1] = qx; + data->mocap_quat[rope_mocap_id_ * 4 + 2] = qy; + data->mocap_quat[rope_mocap_id_ * 4 + 3] = qz; + + // Update capsule half-length to match the current anchor–attachment distance. + // const_cast is intentional: geom_size is a display property; safe to + // modify at runtime without affecting physics. + if (rope_geom_id_ >= 0) { + const_cast(model)->geom_size[rope_geom_id_ * 3 + 1] = rope_dist * 0.5; + } + } + + // --- Rope constraint force ------------------------------------------------ + // Clear previously applied forces before writing new values. + for (int i = 0; i < 6; ++i) { + data->xfrc_applied[body_id_ * 6 + i] = 0.0; + } + + if (!enabled_ || rope_dist <= rope_length_) { + return; // rope slack or gantry disabled — no force applied + } + + // The rope is taut: apply a radial-only tension force toward the anchor. + // No tangential component is applied, so the robot swings freely like a pendulum. + const double rope_dir[3] = {dx / rope_dist, dy / rope_dist, dz / rope_dist}; + + // Radial velocity (positive = attachment moving away from anchor). const double vx = data->cvel[body_id_ * 6 + 3]; const double vy = data->cvel[body_id_ * 6 + 4]; const double vz = data->cvel[body_id_ * 6 + 5]; + const double vel_radial = vx * rope_dir[0] + vy * rope_dir[1] + vz * rope_dir[2]; + + // Tension spring + one-sided damping (damp only when rope is extending). + const double extension = rope_dist - rope_length_; + const double tension = kp_pos_ * extension + kd_pos_ * std::max(0.0, vel_radial); + + // Force toward anchor (opposite of rope_dir). + const double Fx = -tension * rope_dir[0]; + const double Fy = -tension * rope_dir[1]; + const double Fz = -tension * rope_dir[2]; + + data->xfrc_applied[body_id_ * 6 + 0] = Fx; + data->xfrc_applied[body_id_ * 6 + 1] = Fy; + data->xfrc_applied[body_id_ * 6 + 2] = Fz; + + // Torque correction for off-CoM attachment: τ = offset_world × F. + const double ox = xmat[0] * body_offset_[0] + xmat[1] * body_offset_[1] + + xmat[2] * body_offset_[2]; + const double oy = xmat[3] * body_offset_[0] + xmat[4] * body_offset_[1] + + xmat[5] * body_offset_[2]; + const double oz = xmat[6] * body_offset_[0] + xmat[7] * body_offset_[1] + + xmat[8] * body_offset_[2]; - // xfrc_applied layout: [force(3), torque(3)] per body — write forces at +0..+2. - // cvel layout: [angular(3), linear(3)] per body — read linear velocity from +3..+5. - data->xfrc_applied[body_id_ * 6 + 0] = - kp_pos_ * (target_pos_[0] - data->xpos[body_id_ * 3 + 0]) - kd_pos_ * vx; - data->xfrc_applied[body_id_ * 6 + 1] = - kp_pos_ * (target_pos_[1] - data->xpos[body_id_ * 3 + 1]) - kd_pos_ * vy; - data->xfrc_applied[body_id_ * 6 + 2] = - kp_pos_ * (target_pos_[2] - data->xpos[body_id_ * 3 + 2]) - kd_pos_ * vz; + data->xfrc_applied[body_id_ * 6 + 3] = oy * Fz - oz * Fy; + data->xfrc_applied[body_id_ * 6 + 4] = oz * Fx - ox * Fz; + data->xfrc_applied[body_id_ * 6 + 5] = ox * Fy - oy * Fx; } void VirtualGantryPlugin::reset() { - std::lock_guard lock(target_mutex_); + std::lock_guard lock(state_mutex_); + // Re-capture anchor on the next update() so the gantry re-anchors above the + // robot's current position after every simulation reset. spawn_pos_captured_ = false; } void VirtualGantryPlugin::cleanup() { enable_srv_.reset(); - target_srv_.reset(); node_.reset(); } From ad194fead3e23aece89bbe87452ce3bad4fb2711 Mon Sep 17 00:00:00 2001 From: Eraimondi Date: Tue, 21 Apr 2026 14:23:22 +0200 Subject: [PATCH 05/19] Handle G key and Shift+scroll in ROS2ControlGlfwAdapter - 'G' press increments GantryKeyboardState::toggle_counter (gantry on/off). - Shift+scroll accumulates ticks in GantryKeyboardState::rope_scroll_ticks (suppresses camera zoom while adjusting rope length). --- .../src/mujoco_system_interface.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 3596fb4b..546218f8 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -19,6 +19,7 @@ #include "mujoco_ros2_control/mujoco_system_interface.hpp" #include "array_safety.h" +#include "mujoco_ros2_control_plugins/gantry_keyboard_state.hpp" #include #include @@ -269,10 +270,28 @@ class ROS2ControlGlfwAdapter : public mj::GlfwAdapter return; } + // Toggle gantry on/off with 'G'. + if (key == GLFW_KEY_G && act == GLFW_PRESS) { + mujoco_ros2_control_plugins::GantryKeyboardState::get().toggle_counter.fetch_add( + 1, std::memory_order_relaxed); + return; + } + // Forward all other keys so normal UI behaviour is preserved. mj::GlfwAdapter::OnKey(key, scancode, act); } + void OnScroll(double xoffset, double yoffset) override + { + // Shift+scroll adjusts the gantry rope length (5 cm per notch). + if (IsShiftKeyPressed()) { + mujoco_ros2_control_plugins::GantryKeyboardState::get().rope_scroll_ticks.fetch_add( + static_cast(yoffset), std::memory_order_relaxed); + return; // suppress camera zoom while adjusting rope length + } + mj::GlfwAdapter::OnScroll(xoffset, yoffset); + } + private: std::atomic& step_requested_; }; From b1cef7154a9c06b80767a88819e31dd2b83a835a Mon Sep 17 00:00:00 2001 From: Eraimondi Date: Tue, 21 Apr 2026 14:47:19 +0200 Subject: [PATCH 06/19] Fix std::array brace initialization for C++17 compatibility --- .../mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp | 2 +- mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp index 3accbf1b..d08fde14 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp @@ -56,7 +56,7 @@ class VirtualGantryPlugin : public MuJoCoROS2ControlPluginBase int body_id_{-1}; // Offset from the body CoM to the rope attachment point, expressed in the body frame. - std::array body_offset_{0.0, 0.0, 0.0}; + std::array body_offset_{{0.0, 0.0, 0.0}}; // Rope tension spring/damper gains. double kp_pos_{50000.0}; diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index 6f507f7d..15d55e16 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -57,7 +57,7 @@ bool VirtualGantryPlugin::init( const auto v = params->get_parameter(prefix + "body_offset") .get_parameter_value().get>(); if (v.size() == 3) { - body_offset_ = {v[0], v[1], v[2]}; + body_offset_ = {{v[0], v[1], v[2]}}; } } @@ -169,7 +169,7 @@ void VirtualGantryPlugin::update(const mjModel * model, mjData * data) // --- Capture anchor on first step after (re-)enable ---------------------- if (!spawn_pos_captured_) { - anchor_pos_ = {attach_pos[0], attach_pos[1], attach_pos[2] + anchor_height_}; + anchor_pos_ = {{attach_pos[0], attach_pos[1], attach_pos[2] + anchor_height_}}; spawn_pos_captured_ = true; RCLCPP_INFO(node_->get_logger(), "VirtualGantryPlugin: anchor at [%.3f, %.3f, %.3f], rope_length=%.2f m", From eeac0805eb5ec23632d9091bba715420d6773d66 Mon Sep 17 00:00:00 2001 From: Eraimondi Date: Tue, 21 Apr 2026 14:55:51 +0200 Subject: [PATCH 07/19] Fix GantryKeyboardState singleton isolation and anchor world-frame Z Move GantryKeyboardState::get() from header to .cpp so both DSOs (mujoco_ros2_control and mujoco_ros2_control_plugins) share one instance rather than each getting their own -fvisibility=hidden-isolated static. Replace relative anchor_height with anchor_z_world (world-frame Z). Rope length is now computed at spawn as |anchor_z - attach_z| rather than read from YAML. Co-Authored-By: Claude Sonnet 4.6 --- mujoco_ros2_control_plugins/CMakeLists.txt | 1 + .../gantry_keyboard_state.hpp | 8 ++--- .../virtual_gantry_plugin.hpp | 16 +++++----- .../src/gantry_keyboard_state.cpp | 30 +++++++++++++++++++ .../src/virtual_gantry_plugin.cpp | 14 +++++---- 5 files changed, 51 insertions(+), 18 deletions(-) create mode 100644 mujoco_ros2_control_plugins/src/gantry_keyboard_state.cpp diff --git a/mujoco_ros2_control_plugins/CMakeLists.txt b/mujoco_ros2_control_plugins/CMakeLists.txt index e1262d01..312a91be 100644 --- a/mujoco_ros2_control_plugins/CMakeLists.txt +++ b/mujoco_ros2_control_plugins/CMakeLists.txt @@ -26,6 +26,7 @@ endif() # Build the plugin library add_library(mujoco_ros2_control_plugins SHARED + src/gantry_keyboard_state.cpp src/heartbeat_publisher_plugin.cpp src/virtual_gantry_plugin.cpp ) diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/gantry_keyboard_state.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/gantry_keyboard_state.hpp index 4381f7d8..90772935 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/gantry_keyboard_state.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/gantry_keyboard_state.hpp @@ -32,11 +32,9 @@ struct GantryKeyboardState // Positive = scroll up (lengthen rope), negative = scroll down (shorten). std::atomic rope_scroll_ticks{0}; - static GantryKeyboardState & get() - { - static GantryKeyboardState instance; - return instance; - } + // Defined in gantry_keyboard_state.cpp so there is exactly one instance across + // all shared libraries (mujoco_ros2_control_plugins + mujoco_ros2_control). + static GantryKeyboardState & get(); private: GantryKeyboardState() = default; diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp index d08fde14..daae6a6e 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp @@ -33,9 +33,10 @@ namespace mujoco_ros2_control_plugins // tension force is applied toward the anchor. No lateral force is ever applied, // so the robot swings freely like a pendulum at all times. // -// The anchor spawns anchor_height_ metres directly above the attachment point the -// first time update() runs (and after every re-enable). The rope_length_ defaults -// to anchor_height_ so the rope is just taut at spawn. +// On (re-)enable the anchor is placed at [attach_xy, anchor_z_world_] — the XY +// tracks the current attachment point but the Z is fixed in world frame (param +// anchor_z, default 1.7 m). rope_length_ is computed as |anchor_z - attach_z| +// at spawn so the rope is just taut at the moment of activation. // // 'G' in the MuJoCo viewer toggles the gantry on/off. // Shift+scroll adjusts rope_length_ at 5 cm per scroll notch. @@ -62,12 +63,13 @@ class VirtualGantryPlugin : public MuJoCoROS2ControlPluginBase double kp_pos_{50000.0}; double kd_pos_{5000.0}; - // Fixed anchor position (world frame); set once on first update after (re-)enable. - double anchor_height_{2.0}; + // World-frame Z of the fixed anchor point. Plugin sets anchor_pos_[2] = anchor_z_world_ + // on every (re-)enable, regardless of where the robot currently is. + double anchor_z_world_{1.7}; std::array anchor_pos_{}; - // Maximum rope length. Defaults to anchor_height_ so the rope is taut at spawn. - double rope_length_{2.0}; + // Rope length at spawn = |anchor_z_world_ - attach_z|; adjustable at runtime via Shift+scroll. + double rope_length_{0.0}; bool enabled_{true}; bool spawn_pos_captured_{false}; diff --git a/mujoco_ros2_control_plugins/src/gantry_keyboard_state.cpp b/mujoco_ros2_control_plugins/src/gantry_keyboard_state.cpp new file mode 100644 index 00000000..477f8574 --- /dev/null +++ b/mujoco_ros2_control_plugins/src/gantry_keyboard_state.cpp @@ -0,0 +1,30 @@ +// Copyright 2026 NVIDIA CORPORATION & AFFILIATES +// +// 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. + +#include "mujoco_ros2_control_plugins/gantry_keyboard_state.hpp" + +namespace mujoco_ros2_control_plugins +{ + +// Defined here (not in the header) so there is exactly one instance across all +// shared libraries that dlopen this package (mujoco_ros2_control_plugins and +// mujoco_ros2_control both link it; a header-inline static would produce two +// independent instances because of -fvisibility=hidden). +GantryKeyboardState & GantryKeyboardState::get() +{ + static GantryKeyboardState instance; + return instance; +} + +} // namespace mujoco_ros2_control_plugins diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index 15d55e16..d6cac49e 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -50,8 +50,7 @@ bool VirtualGantryPlugin::init( get_string("body_name", body_name_); get_double("kp_pos", kp_pos_); get_double("kd_pos", kd_pos_); - get_double("anchor_height", anchor_height_); - get_double("rope_length", rope_length_); + get_double("anchor_z", anchor_z_world_); if (params->has_parameter(prefix + "body_offset")) { const auto v = params->get_parameter(prefix + "body_offset") @@ -100,9 +99,9 @@ bool VirtualGantryPlugin::init( } RCLCPP_INFO(node_->get_logger(), - "VirtualGantryPlugin: body '%s' (id=%d), anchor_height=%.2f, rope_length=%.2f, " + "VirtualGantryPlugin: body '%s' (id=%d), anchor_z=%.2f, " "offset=[%.3f,%.3f,%.3f], kp=%.0f, kd=%.0f", - body_name_.c_str(), body_id_, anchor_height_, rope_length_, + body_name_.c_str(), body_id_, anchor_z_world_, body_offset_[0], body_offset_[1], body_offset_[2], kp_pos_, kd_pos_); enable_srv_ = node_->create_service( @@ -169,10 +168,13 @@ void VirtualGantryPlugin::update(const mjModel * model, mjData * data) // --- Capture anchor on first step after (re-)enable ---------------------- if (!spawn_pos_captured_) { - anchor_pos_ = {{attach_pos[0], attach_pos[1], attach_pos[2] + anchor_height_}}; + // Anchor XY follows the attachment point; Z is fixed in world frame. + anchor_pos_ = {{attach_pos[0], attach_pos[1], anchor_z_world_}}; + // Rope is just taut at spawn: length = vertical gap between anchor and attachment. + rope_length_ = std::abs(anchor_z_world_ - attach_pos[2]); spawn_pos_captured_ = true; RCLCPP_INFO(node_->get_logger(), - "VirtualGantryPlugin: anchor at [%.3f, %.3f, %.3f], rope_length=%.2f m", + "VirtualGantryPlugin: anchor at [%.3f, %.3f, %.3f], rope_length=%.3f m", anchor_pos_[0], anchor_pos_[1], anchor_pos_[2], rope_length_); } From eb18a7583f7d4961c06fb72cfb2c6bcf51b01861 Mon Sep 17 00:00:00 2001 From: Eraimondi Date: Tue, 21 Apr 2026 15:04:18 +0200 Subject: [PATCH 08/19] Set default anchor_z_world to 1.5 m Co-Authored-By: Claude Sonnet 4.6 --- .../mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp index daae6a6e..604f5703 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp @@ -65,7 +65,7 @@ class VirtualGantryPlugin : public MuJoCoROS2ControlPluginBase // World-frame Z of the fixed anchor point. Plugin sets anchor_pos_[2] = anchor_z_world_ // on every (re-)enable, regardless of where the robot currently is. - double anchor_z_world_{1.7}; + double anchor_z_world_{1.5}; std::array anchor_pos_{}; // Rope length at spawn = |anchor_z_world_ - attach_z|; adjustable at runtime via Shift+scroll. From 0adb3ea1233378af87eae0f42ddafa449a7f4f8c Mon Sep 17 00:00:00 2001 From: Eraimondi Date: Tue, 21 Apr 2026 15:25:59 +0200 Subject: [PATCH 09/19] Replace Shift+scroll with '['/']' keys for rope length adjustment GlfwAdapter's scroll GLFW callback calls PlatformUIAdapter::OnScroll via a devirtualized direct call (R_X86_64_PLT32), so virtual OnScroll overrides in ROS2ControlGlfwAdapter are never reached for scroll events. Replace Shift+scroll with '[' (shorten) and ']' (lengthen) key presses that go through the virtual OnKey dispatch. Hold for key-repeat to adjust continuously. Rename rope_scroll_ticks to rope_length_ticks accordingly. Co-Authored-By: Claude Sonnet 4.6 --- .../src/mujoco_system_interface.cpp | 26 +++++++++++-------- .../gantry_keyboard_state.hpp | 9 ++++--- .../virtual_gantry_plugin.hpp | 2 +- .../src/virtual_gantry_plugin.cpp | 4 +-- 4 files changed, 23 insertions(+), 18 deletions(-) diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 546218f8..2272e376 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -277,21 +277,25 @@ class ROS2ControlGlfwAdapter : public mj::GlfwAdapter return; } + // '[' = shorten rope, ']' = lengthen rope (5 cm per press; hold for repeat). + // Note: GlfwAdapter's GLFW scroll callback calls PlatformUIAdapter::OnScroll via + // a devirtualized direct call, so OnScroll overrides cannot intercept scroll events. + // Keyboard-based adjustment here is the reliable alternative. + if (key == GLFW_KEY_LEFT_BRACKET && (act == GLFW_PRESS || act == GLFW_REPEAT)) { + mujoco_ros2_control_plugins::GantryKeyboardState::get().rope_length_ticks.fetch_add( + -1, std::memory_order_relaxed); + return; + } + if (key == GLFW_KEY_RIGHT_BRACKET && (act == GLFW_PRESS || act == GLFW_REPEAT)) { + mujoco_ros2_control_plugins::GantryKeyboardState::get().rope_length_ticks.fetch_add( + 1, std::memory_order_relaxed); + return; + } + // Forward all other keys so normal UI behaviour is preserved. mj::GlfwAdapter::OnKey(key, scancode, act); } - void OnScroll(double xoffset, double yoffset) override - { - // Shift+scroll adjusts the gantry rope length (5 cm per notch). - if (IsShiftKeyPressed()) { - mujoco_ros2_control_plugins::GantryKeyboardState::get().rope_scroll_ticks.fetch_add( - static_cast(yoffset), std::memory_order_relaxed); - return; // suppress camera zoom while adjusting rope length - } - mj::GlfwAdapter::OnScroll(xoffset, yoffset); - } - private: std::atomic& step_requested_; }; diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/gantry_keyboard_state.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/gantry_keyboard_state.hpp index 90772935..b14c5000 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/gantry_keyboard_state.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/gantry_keyboard_state.hpp @@ -20,7 +20,7 @@ namespace mujoco_ros2_control_plugins { -// Singleton that bridges the GLFW UI thread (key/scroll events) and the +// Singleton that bridges the GLFW UI thread (key events) and the // VirtualGantryPlugin physics thread. All fields are atomic — no mutex needed. struct GantryKeyboardState { @@ -28,9 +28,10 @@ struct GantryKeyboardState // value to detect a toggle edge without consuming the count. std::atomic toggle_counter{0}; - // Accumulated Shift+scroll ticks. Plugin exchanges to 0 after reading. - // Positive = scroll up (lengthen rope), negative = scroll down (shorten). - std::atomic rope_scroll_ticks{0}; + // Accumulated rope-length ticks from '['/']' key presses. + // Plugin exchanges to 0 after reading. + // Positive = lengthen rope, negative = shorten. + std::atomic rope_length_ticks{0}; // Defined in gantry_keyboard_state.cpp so there is exactly one instance across // all shared libraries (mujoco_ros2_control_plugins + mujoco_ros2_control). diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp index 604f5703..e2b6dfa1 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp @@ -39,7 +39,7 @@ namespace mujoco_ros2_control_plugins // at spawn so the rope is just taut at the moment of activation. // // 'G' in the MuJoCo viewer toggles the gantry on/off. -// Shift+scroll adjusts rope_length_ at 5 cm per scroll notch. +// '[' shortens and ']' lengthens the rope by 5 cm per keypress (hold to repeat). // The set_gantry_enabled ROS 2 service also enables/disables the gantry. class VirtualGantryPlugin : public MuJoCoROS2ControlPluginBase { diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index d6cac49e..20163c90 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -145,9 +145,9 @@ void VirtualGantryPlugin::update(const mjModel * model, mjData * data) enabled_ ? "enabled" : "disabled"); } - // --- Rope length adjustment (Shift+scroll) -------------------------------- + // --- Rope length adjustment ('[' / ']' keys) ------------------------------- const int scroll_ticks = - GantryKeyboardState::get().rope_scroll_ticks.exchange(0, std::memory_order_relaxed); + GantryKeyboardState::get().rope_length_ticks.exchange(0, std::memory_order_relaxed); if (scroll_ticks != 0) { rope_length_ = std::max(0.1, rope_length_ + scroll_ticks * 0.05); RCLCPP_INFO(node_->get_logger(), "VirtualGantryPlugin: rope_length=%.3f m", rope_length_); From 0cbd7b1fa6e9d2866df794991d9e92d1b9b634e9 Mon Sep 17 00:00:00 2001 From: Eraimondi Date: Tue, 21 Apr 2026 15:32:48 +0200 Subject: [PATCH 10/19] Link mujoco_ros2_control against mujoco_ros2_control_plugins GantryKeyboardState::get() is defined in the plugins library. mujoco_ros2_control needs to link against it so the symbol is resolved at link time and both DSOs share the same singleton instance. Co-Authored-By: Claude Sonnet 4.6 --- mujoco_ros2_control/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index d393213d..5ed80602 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -188,6 +188,7 @@ PRIVATE Threads::Threads lodepng glfw + mujoco_ros2_control_plugins::mujoco_ros2_control_plugins ) # Note: the transmissions_interface must NOT be statically linked against the library, only headers # should be used. Otherwise it will be loaded by the linker at process startup and its From df36f8ffd873d3379910d4c387b349e92d9faabe Mon Sep 17 00:00:00 2001 From: Eraimondi Date: Tue, 21 Apr 2026 16:08:53 +0200 Subject: [PATCH 11/19] Move GantryKeyboardState::get() to main library to fix pluginlib factory registration MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Placing the definition in libmujoco_ros2_control_plugins.so and directly linking it from libmujoco_ros2_control.so caused the plugins DSO to be loaded at process startup — before pluginlib ClassLoader initialises — so PLUGINLIB_EXPORT_CLASS static constructors ran too early and VirtualGantryPlugin was never registered. Fix: define GantryKeyboardState::get() in libmujoco_ros2_control.so with visibility("default") so the symbol is exported to the global table. The plugins DSO now carries only an undefined reference resolved at dlopen time, eliminating the premature static-constructor problem while preserving the singleton guarantee. Co-Authored-By: Claude Sonnet 4.6 --- mujoco_ros2_control/CMakeLists.txt | 2 +- .../src/gantry_keyboard_state.cpp | 9 +++++---- mujoco_ros2_control_plugins/CMakeLists.txt | 1 - 3 files changed, 6 insertions(+), 6 deletions(-) rename {mujoco_ros2_control_plugins => mujoco_ros2_control}/src/gantry_keyboard_state.cpp (73%) diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index 5ed80602..d0feb2ef 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -152,6 +152,7 @@ add_library(mujoco_ros2_control SHARED src/mujoco_system_interface.cpp src/mujoco_cameras.cpp src/mujoco_lidar.cpp + src/gantry_keyboard_state.cpp ) # Ensure MuJoCo library can be found at runtime regardless of how it was installed above. # mujoco_vendor installs libmujoco.so under opt/mujoco_vendor/lib/ relative to the @@ -188,7 +189,6 @@ PRIVATE Threads::Threads lodepng glfw - mujoco_ros2_control_plugins::mujoco_ros2_control_plugins ) # Note: the transmissions_interface must NOT be statically linked against the library, only headers # should be used. Otherwise it will be loaded by the linker at process startup and its diff --git a/mujoco_ros2_control_plugins/src/gantry_keyboard_state.cpp b/mujoco_ros2_control/src/gantry_keyboard_state.cpp similarity index 73% rename from mujoco_ros2_control_plugins/src/gantry_keyboard_state.cpp rename to mujoco_ros2_control/src/gantry_keyboard_state.cpp index 477f8574..8acc160c 100644 --- a/mujoco_ros2_control_plugins/src/gantry_keyboard_state.cpp +++ b/mujoco_ros2_control/src/gantry_keyboard_state.cpp @@ -17,10 +17,11 @@ namespace mujoco_ros2_control_plugins { -// Defined here (not in the header) so there is exactly one instance across all -// shared libraries that dlopen this package (mujoco_ros2_control_plugins and -// mujoco_ros2_control both link it; a header-inline static would produce two -// independent instances because of -fvisibility=hidden). +// Defined in libmujoco_ros2_control.so (not in the plugins DSO) so there is +// exactly one instance. visibility("default") ensures the symbol is exported +// even when -fvisibility=hidden is active, so the plugins DSO can resolve it +// from the global symbol table at dlopen time. +__attribute__((visibility("default"))) GantryKeyboardState & GantryKeyboardState::get() { static GantryKeyboardState instance; diff --git a/mujoco_ros2_control_plugins/CMakeLists.txt b/mujoco_ros2_control_plugins/CMakeLists.txt index 312a91be..e1262d01 100644 --- a/mujoco_ros2_control_plugins/CMakeLists.txt +++ b/mujoco_ros2_control_plugins/CMakeLists.txt @@ -26,7 +26,6 @@ endif() # Build the plugin library add_library(mujoco_ros2_control_plugins SHARED - src/gantry_keyboard_state.cpp src/heartbeat_publisher_plugin.cpp src/virtual_gantry_plugin.cpp ) From c2d8318aa57021539fbbc8b4a84f23ec3f002b58 Mon Sep 17 00:00:00 2001 From: Eraimondi Date: Tue, 21 Apr 2026 17:00:25 +0200 Subject: [PATCH 12/19] Promote libmujoco_ros2_control.so to RTLD_GLOBAL before loading plugins MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit class_loader loads hardware-interface plugins with RTLD_LOCAL, which keeps this DSO's symbols (GantryKeyboardState::get) out of the global symbol table. When VirtualGantryPlugin then calls GantryKeyboardState::get() it gets an undefined-symbol crash (exit 127). Use dladdr to find our own DSO path, then dlopen with RTLD_NOLOAD|RTLD_GLOBAL to promote visibility in-place — no reload, no extra reference count. This runs before the ClassLoader is created, so plugin factory registration is unaffected. Also add ${CMAKE_DL_LIBS} to link libdl for dlopen/dladdr/dlclose. Co-Authored-By: Claude Sonnet 4.6 --- mujoco_ros2_control/CMakeLists.txt | 1 + .../mujoco_system_interface.hpp | 4 +- .../src/gantry_keyboard_state.cpp | 3 +- .../src/mujoco_system_interface.cpp | 36 +++-- mujoco_ros2_control_plugins/README.md | 85 ++++++++++++ .../gantry_keyboard_state.hpp | 6 +- .../mujoco_ros2_control_plugins_base.hpp | 4 +- .../virtual_gantry_plugin.hpp | 37 ++--- .../src/virtual_gantry_plugin.cpp | 128 +++++------------- 9 files changed, 171 insertions(+), 133 deletions(-) diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index d0feb2ef..9b03b961 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -189,6 +189,7 @@ PRIVATE Threads::Threads lodepng glfw + ${CMAKE_DL_LIBS} ) # Note: the transmissions_interface must NOT be statically linked against the library, only headers # should be used. Otherwise it will be loaded by the linker at process startup and its diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp index 35f7e865..80fc27d4 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp @@ -28,23 +28,23 @@ #include #include +#include #include #include #include #include -#include #include #include #include #include #include -#include #include #include #include #include #include #include +#include #include diff --git a/mujoco_ros2_control/src/gantry_keyboard_state.cpp b/mujoco_ros2_control/src/gantry_keyboard_state.cpp index 8acc160c..567620d9 100644 --- a/mujoco_ros2_control/src/gantry_keyboard_state.cpp +++ b/mujoco_ros2_control/src/gantry_keyboard_state.cpp @@ -21,8 +21,7 @@ namespace mujoco_ros2_control_plugins // exactly one instance. visibility("default") ensures the symbol is exported // even when -fvisibility=hidden is active, so the plugins DSO can resolve it // from the global symbol table at dlopen time. -__attribute__((visibility("default"))) -GantryKeyboardState & GantryKeyboardState::get() +__attribute__((visibility("default"))) GantryKeyboardState& GantryKeyboardState::get() { static GantryKeyboardState instance; return instance; diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 2272e376..5a4db5b4 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -45,7 +46,6 @@ #include #include #include -#include #include #include @@ -271,9 +271,9 @@ class ROS2ControlGlfwAdapter : public mj::GlfwAdapter } // Toggle gantry on/off with 'G'. - if (key == GLFW_KEY_G && act == GLFW_PRESS) { - mujoco_ros2_control_plugins::GantryKeyboardState::get().toggle_counter.fetch_add( - 1, std::memory_order_relaxed); + if (key == GLFW_KEY_G && act == GLFW_PRESS) + { + mujoco_ros2_control_plugins::GantryKeyboardState::get().toggle_counter.fetch_add(1, std::memory_order_relaxed); return; } @@ -281,14 +281,14 @@ class ROS2ControlGlfwAdapter : public mj::GlfwAdapter // Note: GlfwAdapter's GLFW scroll callback calls PlatformUIAdapter::OnScroll via // a devirtualized direct call, so OnScroll overrides cannot intercept scroll events. // Keyboard-based adjustment here is the reliable alternative. - if (key == GLFW_KEY_LEFT_BRACKET && (act == GLFW_PRESS || act == GLFW_REPEAT)) { - mujoco_ros2_control_plugins::GantryKeyboardState::get().rope_length_ticks.fetch_add( - -1, std::memory_order_relaxed); + if (key == GLFW_KEY_LEFT_BRACKET && (act == GLFW_PRESS || act == GLFW_REPEAT)) + { + mujoco_ros2_control_plugins::GantryKeyboardState::get().rope_length_ticks.fetch_add(-1, std::memory_order_relaxed); return; } - if (key == GLFW_KEY_RIGHT_BRACKET && (act == GLFW_PRESS || act == GLFW_REPEAT)) { - mujoco_ros2_control_plugins::GantryKeyboardState::get().rope_length_ticks.fetch_add( - 1, std::memory_order_relaxed); + if (key == GLFW_KEY_RIGHT_BRACKET && (act == GLFW_PRESS || act == GLFW_REPEAT)) + { + mujoco_ros2_control_plugins::GantryKeyboardState::get().rope_length_ticks.fetch_add(1, std::memory_order_relaxed); return; } @@ -3387,6 +3387,22 @@ rclcpp::Node::SharedPtr MujocoSystemInterface::get_node() const void MujocoSystemInterface::load_mujoco_plugins() { + // Ensure GantryKeyboardState::get() (defined in this DSO) is visible globally so + // that libmujoco_ros2_control_plugins.so can resolve it at dlopen time. + // class_loader may load hardware-interface plugins with RTLD_LOCAL, which + // would otherwise keep our symbols out of the global symbol table. + { + Dl_info info{}; + if (dladdr(reinterpret_cast(&MujocoSystemInterface::load_mujoco_plugins), &info) && info.dli_fname) + { + void* h = dlopen(info.dli_fname, RTLD_LAZY | RTLD_NOLOAD | RTLD_GLOBAL); + if (h) + { + dlclose(h); // just promoting visibility, don't hold an extra reference + } + } + } + try { plugin_loader_ = std::make_unique>( diff --git a/mujoco_ros2_control_plugins/README.md b/mujoco_ros2_control_plugins/README.md index fb1c07e1..7ead07e4 100644 --- a/mujoco_ros2_control_plugins/README.md +++ b/mujoco_ros2_control_plugins/README.md @@ -20,6 +20,91 @@ A simple demonstration plugin that publishes a heartbeat message every second to **Rate**: 1 Hz (every 1 second) **Message Format**: "MuJoCo ROS2 Control Heartbeat #N | Simulation time: Xs" +### VirtualGantryPlugin + +A rope-constraint gantry that suspends a robot from a fixed anchor point in the world. +Useful for humanoid training and testing: the rope catches falls without constraining +normal motion, so the robot can walk, balance, and recover freely. + +**Physics model**: a one-sided cable constraint. When the straight-line distance from the +anchor to the configured attachment point on the robot exceeds `rope_length`, a radial +tension force is applied toward the anchor. Because the force is purely radial, the robot +swings freely like a pendulum at all times — both when the rope is slack and when it is taut. +The spring force is always present when taut; the damper only acts when the rope is being +stretched further (one-sided), which prevents the asymmetric energy cycles that would cause +growing oscillations. + +**On (re-)enable**: the anchor is placed at `[attach_xy, anchor_z]`, and `rope_length` is +set to `|anchor_z - attach_z|` so the rope is just taut at the moment of activation. + +**On simulation reset**: the gantry auto-reactivates and re-anchors above the current +attachment position. + +#### Parameters + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `body_name` | string | `"torso_link"` | MuJoCo body the rope attaches to | +| `body_offset` | double[3] | `[0, 0, 0]` | Offset from the body CoM to the attachment point, in the body frame (m) | +| `kp_pos` | double | `5000.0` | Rope spring stiffness (N/m). Higher = stiffer rope, less sag when hanging | +| `kd_pos` | double | `800.0` | Rope damping (N·s/m). Higher = faster settling after a bounce. Only applied when the rope is extending | +| `anchor_z` | double | `1.5` | World-frame Z of the fixed anchor point (m) | + +#### Keyboard controls (MuJoCo viewer) + +| Key | Action | +|---|---| +| `G` | Toggle gantry on / off. Re-enabling re-anchors above the current attachment position | +| `[` | Shorten rope by 0.5 cm | +| `]` | Lengthen rope by 0.5 cm | + +#### ROS 2 interface + +**Service**: `set_gantry_enabled` (`mujoco_ros2_control_msgs/srv/SetGantryEnabled`) + +The full service path is `///set_gantry_enabled`. With the default node name and the plugin key `virtual_gantry` from the example config above: + +```bash +# Enable +ros2 service call /mujoco_ros2_control_node/virtual_gantry/set_gantry_enabled \ + mujoco_ros2_control_msgs/srv/SetGantryEnabled '{enabled: true}' + +# Disable +ros2 service call /mujoco_ros2_control_node/virtual_gantry/set_gantry_enabled \ + mujoco_ros2_control_msgs/srv/SetGantryEnabled '{enabled: false}' +``` + +#### Example configuration + +```yaml +/**: + ros__parameters: + mujoco_plugins: + virtual_gantry: + type: "mujoco_ros2_control_plugins/VirtualGantryPlugin" + body_name: "torso_link" + body_offset: [0.0, 0.0, 0.3] # attach at neck, 0.3 m above torso CoM + kp_pos: 5000.0 + kd_pos: 800.0 + anchor_z: 1.5 +``` + +#### Tuning guide + +- **`anchor_z`**: place the anchor well above the robot's normal standing attachment height. + At spawn, `rope_length` is set to the taut distance, so the robot starts just taut. A + higher anchor gives a longer rope and more pendulum-like, vertical forces. + +- **`kp_pos`**: controls how much the robot sags when hanging. For a 40 kg robot hanging + freely, equilibrium sag ≈ `m·g / kp`. At `kp=5000` this is ≈ 8 cm. + +- **`kd_pos`**: controls how quickly oscillations die out after pressing `[`/`]` or after + a fall. Effective damping ratio (one-sided spring) ≈ `kd / (4·√(kp·m))`. Values around + 0.4–0.6 give 2–3 settling bounces. Going much higher risks impulsive catch forces on + large rope-length steps. + +--- + ## Dependencies - `mujoco_vendor`: Provides the MuJoCo physics simulator library diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/gantry_keyboard_state.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/gantry_keyboard_state.hpp index b14c5000..38562b36 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/gantry_keyboard_state.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/gantry_keyboard_state.hpp @@ -26,16 +26,16 @@ struct GantryKeyboardState { // Incremented on each 'G' key press. Plugin compares against its last-seen // value to detect a toggle edge without consuming the count. - std::atomic toggle_counter{0}; + std::atomic toggle_counter{ 0 }; // Accumulated rope-length ticks from '['/']' key presses. // Plugin exchanges to 0 after reading. // Positive = lengthen rope, negative = shorten. - std::atomic rope_length_ticks{0}; + std::atomic rope_length_ticks{ 0 }; // Defined in gantry_keyboard_state.cpp so there is exactly one instance across // all shared libraries (mujoco_ros2_control_plugins + mujoco_ros2_control). - static GantryKeyboardState & get(); + static GantryKeyboardState& get(); private: GantryKeyboardState() = default; diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp index 225e3676..baf322de 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp @@ -67,7 +67,9 @@ class MuJoCoROS2ControlPluginBase * @note Override to clear any accumulated state (e.g. integrators) that should not * carry over across resets. */ - virtual void reset() {} + virtual void reset() + { + } }; } // namespace mujoco_ros2_control_plugins diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp index e2b6dfa1..b4e9140f 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp @@ -44,8 +44,8 @@ namespace mujoco_ros2_control_plugins class VirtualGantryPlugin : public MuJoCoROS2ControlPluginBase { public: - bool init(rclcpp::Node::SharedPtr node, const mjModel * model, mjData * data) override; - void update(const mjModel * model, mjData * data) override; + bool init(rclcpp::Node::SharedPtr node, const mjModel* model, mjData* data) override; + void update(const mjModel* model, mjData* data) override; void reset() override; void cleanup() override; @@ -53,36 +53,37 @@ class VirtualGantryPlugin : public MuJoCoROS2ControlPluginBase rclcpp::Node::SharedPtr node_; // Target body (attachment point on the robot). - std::string body_name_{"torso_link"}; - int body_id_{-1}; + std::string body_name_{ "torso_link" }; + int body_id_{ -1 }; // Offset from the body CoM to the rope attachment point, expressed in the body frame. - std::array body_offset_{{0.0, 0.0, 0.0}}; + std::array body_offset_{ { 0.0, 0.0, 0.0 } }; // Rope tension spring/damper gains. - double kp_pos_{50000.0}; - double kd_pos_{5000.0}; + double kp_pos_{ 5000.0 }; + double kd_pos_{ 3000.0 }; // World-frame Z of the fixed anchor point. Plugin sets anchor_pos_[2] = anchor_z_world_ // on every (re-)enable, regardless of where the robot currently is. - double anchor_z_world_{1.5}; + double anchor_z_world_{ 1.5 }; std::array anchor_pos_{}; // Rope length at spawn = |anchor_z_world_ - attach_z|; adjustable at runtime via Shift+scroll. - double rope_length_{0.0}; + double rope_length_{ 0.0 }; - bool enabled_{true}; - bool spawn_pos_captured_{false}; + // Finite-difference state for rope-extension-rate damping. + // Using d(rope_dist)/dt instead of cvel avoids phase errors when physics runs + // faster than the control loop (200 Hz control, 500 Hz physics). + // rope_dist_dot_ is an EMA-smoothed derivative (α≈0.2) to filter contact/joint noise. + double rope_dist_prev_{ -1.0 }; + double rope_dist_dot_{ 0.0 }; + double last_update_time_{ -1.0 }; - // mocap_id for the anchor sphere visual (-1 = not present in model). - int anchor_mocap_id_{-1}; - // mocap_id for the rope capsule visual (-1 = not present in model). - int rope_mocap_id_{-1}; - // geom index of the rope capsule (for dynamic half-length update, -1 = none). - int rope_geom_id_{-1}; + bool enabled_{ true }; + bool spawn_pos_captured_{ false }; // Last toggle_counter value seen; used to detect 'G' key edges. - int last_toggle_count_{0}; + int last_toggle_count_{ 0 }; std::mutex state_mutex_; diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index 20163c90..5329682b 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -67,37 +67,6 @@ bool VirtualGantryPlugin::init( return false; } - // Resolve optional mocap visual bodies. - auto resolve_mocap = [&](const std::string & param_key, int & mocap_id_out) { - std::string name; - get_string(param_key, name); - if (name.empty()) {return;} - const int bid = mj_name2id(model, mjOBJ_BODY, name.c_str()); - if (bid >= 0 && model->body_mocapid[bid] >= 0) { - mocap_id_out = model->body_mocapid[bid]; - RCLCPP_INFO(node_->get_logger(), "VirtualGantryPlugin: %s '%s' (mocap_id=%d)", - param_key.c_str(), name.c_str(), mocap_id_out); - } else { - RCLCPP_WARN(node_->get_logger(), - "VirtualGantryPlugin: '%s' (param %s) not found or not a mocap body", - name.c_str(), param_key.c_str()); - } - }; - resolve_mocap("anchor_visual_body", anchor_mocap_id_); - resolve_mocap("rope_visual_body", rope_mocap_id_); - - // Find the rope capsule geom for dynamic half-length updates. - if (rope_mocap_id_ >= 0) { - std::string rope_name; - get_string("rope_visual_body", rope_name); - if (!rope_name.empty()) { - const int bid = mj_name2id(model, mjOBJ_BODY, rope_name.c_str()); - if (bid >= 0 && model->body_geomnum[bid] > 0) { - rope_geom_id_ = model->body_geomadr[bid]; - } - } - } - RCLCPP_INFO(node_->get_logger(), "VirtualGantryPlugin: body '%s' (id=%d), anchor_z=%.2f, " "offset=[%.3f,%.3f,%.3f], kp=%.0f, kd=%.0f", @@ -128,7 +97,7 @@ bool VirtualGantryPlugin::init( return true; } -void VirtualGantryPlugin::update(const mjModel * model, mjData * data) +void VirtualGantryPlugin::update(const mjModel * /*model*/, mjData * data) { std::lock_guard lock(state_mutex_); @@ -149,7 +118,7 @@ void VirtualGantryPlugin::update(const mjModel * model, mjData * data) const int scroll_ticks = GantryKeyboardState::get().rope_length_ticks.exchange(0, std::memory_order_relaxed); if (scroll_ticks != 0) { - rope_length_ = std::max(0.1, rope_length_ + scroll_ticks * 0.05); + rope_length_ = std::max(0.1, rope_length_ + scroll_ticks * 0.005); RCLCPP_INFO(node_->get_logger(), "VirtualGantryPlugin: rope_length=%.3f m", rope_length_); } @@ -178,85 +147,49 @@ void VirtualGantryPlugin::update(const mjModel * model, mjData * data) anchor_pos_[0], anchor_pos_[1], anchor_pos_[2], rope_length_); } - // --- Anchor visual -------------------------------------------------------- - if (anchor_mocap_id_ >= 0) { - data->mocap_pos[anchor_mocap_id_ * 3 + 0] = anchor_pos_[0]; - data->mocap_pos[anchor_mocap_id_ * 3 + 1] = anchor_pos_[1]; - data->mocap_pos[anchor_mocap_id_ * 3 + 2] = anchor_pos_[2]; - } - - // --- Rope visual ---------------------------------------------------------- // Rope vector from anchor to attachment point. const double dx = attach_pos[0] - anchor_pos_[0]; const double dy = attach_pos[1] - anchor_pos_[1]; const double dz = attach_pos[2] - anchor_pos_[2]; const double rope_dist = std::sqrt(dx * dx + dy * dy + dz * dz); - if (rope_mocap_id_ >= 0 && rope_dist > 1e-6) { - // Midpoint between anchor and attachment. - data->mocap_pos[rope_mocap_id_ * 3 + 0] = (anchor_pos_[0] + attach_pos[0]) * 0.5; - data->mocap_pos[rope_mocap_id_ * 3 + 1] = (anchor_pos_[1] + attach_pos[1]) * 0.5; - data->mocap_pos[rope_mocap_id_ * 3 + 2] = (anchor_pos_[2] + attach_pos[2]) * 0.5; - - // Normalised direction: anchor → attachment. - const double ndx = dx / rope_dist; - const double ndy = dy / rope_dist; - const double ndz = dz / rope_dist; - - // Quaternion aligning capsule Z-axis with the anchor→attachment direction. - // Rotation axis = cross([0,0,1], [ndx,ndy,ndz]) = [-ndy, ndx, 0]. - const double ax = -ndy, ay = ndx; - const double axis_len = std::sqrt(ax * ax + ay * ay); - double qw, qx, qy, qz; - if (axis_len < 1e-6) { - // Direction is (anti-)parallel to Z. - if (ndz > 0.0) {qw = 1.0; qx = 0.0; qy = 0.0; qz = 0.0;} - else {qw = 0.0; qx = 1.0; qy = 0.0; qz = 0.0;} - } else { - const double clamped = ndz > 1.0 ? 1.0 : ndz < -1.0 ? -1.0 : ndz; - const double angle = std::acos(clamped); - const double sa = std::sin(angle * 0.5); - qw = std::cos(angle * 0.5); - qx = (ax / axis_len) * sa; - qy = (ay / axis_len) * sa; - qz = 0.0; - } - data->mocap_quat[rope_mocap_id_ * 4 + 0] = qw; - data->mocap_quat[rope_mocap_id_ * 4 + 1] = qx; - data->mocap_quat[rope_mocap_id_ * 4 + 2] = qy; - data->mocap_quat[rope_mocap_id_ * 4 + 3] = qz; - - // Update capsule half-length to match the current anchor–attachment distance. - // const_cast is intentional: geom_size is a display property; safe to - // modify at runtime without affecting physics. - if (rope_geom_id_ >= 0) { - const_cast(model)->geom_size[rope_geom_id_ * 3 + 1] = rope_dist * 0.5; - } - } - // --- Rope constraint force ------------------------------------------------ // Clear previously applied forces before writing new values. for (int i = 0; i < 6; ++i) { data->xfrc_applied[body_id_ * 6 + i] = 0.0; } - if (!enabled_ || rope_dist <= rope_length_) { - return; // rope slack or gantry disabled — no force applied + if (!enabled_ || rope_dist < 1e-6 || rope_dist <= rope_length_) { + // Rope is slack or gantry disabled: reset FD state so first taut step has no stale spike. + rope_dist_prev_ = -1.0; + rope_dist_dot_ = 0.0; + last_update_time_ = data->time; + return; } - // The rope is taut: apply a radial-only tension force toward the anchor. - // No tangential component is applied, so the robot swings freely like a pendulum. - const double rope_dir[3] = {dx / rope_dist, dy / rope_dist, dz / rope_dist}; + // Finite-difference rope-extension rate (taut branch only), EMA-smoothed. + // Raw d(rope_dist)/dt over 2 ms is noisy due to contact/joint vibrations; the + // EMA (α≈0.2, τ≈10 ms) keeps low-frequency fall/bounce dynamics while + // filtering out high-frequency noise that would otherwise cause large damp spikes. + if (rope_dist_prev_ >= 0.0 && last_update_time_ >= 0.0) { + const double dt = data->time - last_update_time_; + if (dt > 1e-9) { + const double raw_dot = (rope_dist - rope_dist_prev_) / dt; + const double alpha = std::min(1.0, dt / 0.01); // τ = 10 ms + rope_dist_dot_ = alpha * raw_dot + (1.0 - alpha) * rope_dist_dot_; + } + } + rope_dist_prev_ = rope_dist; + last_update_time_ = data->time; - // Radial velocity (positive = attachment moving away from anchor). - const double vx = data->cvel[body_id_ * 6 + 3]; - const double vy = data->cvel[body_id_ * 6 + 4]; - const double vz = data->cvel[body_id_ * 6 + 5]; - const double vel_radial = vx * rope_dir[0] + vy * rope_dir[1] + vz * rope_dir[2]; + const double rope_dir[3] = {dx / rope_dist, dy / rope_dist, dz / rope_dist}; - // Tension spring + one-sided damping (damp only when rope is extending). + // Spring always pulls toward anchor when taut; damping only resists extension. + // Bidirectional damping (max(0, spring+damp)) silences the spring when contracting + // fast, which creates an asymmetric energy cycle and growing oscillations. const double extension = rope_dist - rope_length_; - const double tension = kp_pos_ * extension + kd_pos_ * std::max(0.0, vel_radial); + const double damp = (rope_dist_dot_ > 0.0) ? kd_pos_ * rope_dist_dot_ : 0.0; + const double tension = kp_pos_ * extension + damp; // Force toward anchor (opposite of rope_dir). const double Fx = -tension * rope_dir[0]; @@ -283,9 +216,10 @@ void VirtualGantryPlugin::update(const mjModel * model, mjData * data) void VirtualGantryPlugin::reset() { std::lock_guard lock(state_mutex_); - // Re-capture anchor on the next update() so the gantry re-anchors above the - // robot's current position after every simulation reset. spawn_pos_captured_ = false; + rope_dist_prev_ = -1.0; + rope_dist_dot_ = 0.0; + last_update_time_ = -1.0; } void VirtualGantryPlugin::cleanup() From 3f69a113193a922f4a7de9be6c6fbaca312c7caa Mon Sep 17 00:00:00 2001 From: Eraimondi Date: Wed, 22 Apr 2026 15:55:37 +0200 Subject: [PATCH 13/19] fix(format): apply clang-format to gantry and impedance control sections Co-Authored-By: Claude Sonnet 4.6 --- .../src/mujoco_system_interface.cpp | 15 ++++---- .../src/virtual_gantry_plugin.cpp | 38 +++++++++---------- 2 files changed, 26 insertions(+), 27 deletions(-) diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 5a4db5b4..c9eb4f71 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -1536,8 +1536,8 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vectoris_position_control_enabled = has_position; joint_it->is_velocity_control_enabled = has_velocity; joint_it->is_effort_control_enabled = has_effort; // Track effort for feed-forward - RCLCPP_INFO(get_logger(), "Joint %s: impedance control enabled (kp/kd with position%s%s)", - joint_name.c_str(), has_velocity ? "/velocity" : "", has_effort ? "/effort_ff" : ""); + RCLCPP_INFO(get_logger(), "Joint %s: impedance control enabled (kp/kd with position%s%s)", joint_name.c_str(), + has_velocity ? "/velocity" : "", has_effort ? "/effort_ff" : ""); } else if (has_effort) { @@ -2299,9 +2299,9 @@ void MujocoSystemInterface::register_urdf_joints(const hardware_interface::Hardw else if (actuator_it->actuator_type == ActuatorType::MOTOR || actuator_it->actuator_type == ActuatorType::CUSTOM) { // Check if kp/kd interfaces are present (indicating impedance control mode) - bool has_impedance_interfaces = std::any_of( - command_interface_names.begin(), command_interface_names.end(), - [](const std::string& name) { return name == HW_IF_KP || name == HW_IF_KD; }); + bool has_impedance_interfaces = + std::any_of(command_interface_names.begin(), command_interface_names.end(), + [](const std::string& name) { return name == HW_IF_KP || name == HW_IF_KD; }); if (actuator_it->has_vel_pid) { @@ -2321,9 +2321,8 @@ void MujocoSystemInterface::register_urdf_joints(const hardware_interface::Hardw else if (has_impedance_interfaces) { // Velocity interface will be used for impedance control - no PID needed - RCLCPP_DEBUG(get_logger(), - "Velocity command interface for joint '%s' will be used for impedance control", - actuator_name.c_str()); + RCLCPP_DEBUG(get_logger(), "Velocity command interface for joint '%s' will be used for impedance control", + actuator_name.c_str()); } else { diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index 5329682b..ae967e31 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -136,15 +136,15 @@ void VirtualGantryPlugin::update(const mjModel * /*model*/, mjData * data) } // --- Capture anchor on first step after (re-)enable ---------------------- - if (!spawn_pos_captured_) { + if (!spawn_pos_captured_) + { // Anchor XY follows the attachment point; Z is fixed in world frame. - anchor_pos_ = {{attach_pos[0], attach_pos[1], anchor_z_world_}}; + anchor_pos_ = { { attach_pos[0], attach_pos[1], anchor_z_world_ } }; // Rope is just taut at spawn: length = vertical gap between anchor and attachment. rope_length_ = std::abs(anchor_z_world_ - attach_pos[2]); spawn_pos_captured_ = true; - RCLCPP_INFO(node_->get_logger(), - "VirtualGantryPlugin: anchor at [%.3f, %.3f, %.3f], rope_length=%.3f m", - anchor_pos_[0], anchor_pos_[1], anchor_pos_[2], rope_length_); + RCLCPP_INFO(node_->get_logger(), "VirtualGantryPlugin: anchor at [%.3f, %.3f, %.3f], rope_length=%.3f m", + anchor_pos_[0], anchor_pos_[1], anchor_pos_[2], rope_length_); } // Rope vector from anchor to attachment point. @@ -155,11 +155,13 @@ void VirtualGantryPlugin::update(const mjModel * /*model*/, mjData * data) // --- Rope constraint force ------------------------------------------------ // Clear previously applied forces before writing new values. - for (int i = 0; i < 6; ++i) { + for (int i = 0; i < 6; ++i) + { data->xfrc_applied[body_id_ * 6 + i] = 0.0; } - if (!enabled_ || rope_dist < 1e-6 || rope_dist <= rope_length_) { + if (!enabled_ || rope_dist < 1e-6 || rope_dist <= rope_length_) + { // Rope is slack or gantry disabled: reset FD state so first taut step has no stale spike. rope_dist_prev_ = -1.0; rope_dist_dot_ = 0.0; @@ -171,9 +173,11 @@ void VirtualGantryPlugin::update(const mjModel * /*model*/, mjData * data) // Raw d(rope_dist)/dt over 2 ms is noisy due to contact/joint vibrations; the // EMA (α≈0.2, τ≈10 ms) keeps low-frequency fall/bounce dynamics while // filtering out high-frequency noise that would otherwise cause large damp spikes. - if (rope_dist_prev_ >= 0.0 && last_update_time_ >= 0.0) { + if (rope_dist_prev_ >= 0.0 && last_update_time_ >= 0.0) + { const double dt = data->time - last_update_time_; - if (dt > 1e-9) { + if (dt > 1e-9) + { const double raw_dot = (rope_dist - rope_dist_prev_) / dt; const double alpha = std::min(1.0, dt / 0.01); // τ = 10 ms rope_dist_dot_ = alpha * raw_dot + (1.0 - alpha) * rope_dist_dot_; @@ -182,7 +186,7 @@ void VirtualGantryPlugin::update(const mjModel * /*model*/, mjData * data) rope_dist_prev_ = rope_dist; last_update_time_ = data->time; - const double rope_dir[3] = {dx / rope_dist, dy / rope_dist, dz / rope_dist}; + const double rope_dir[3] = { dx / rope_dist, dy / rope_dist, dz / rope_dist }; // Spring always pulls toward anchor when taut; damping only resists extension. // Bidirectional damping (max(0, spring+damp)) silences the spring when contracting @@ -201,12 +205,9 @@ void VirtualGantryPlugin::update(const mjModel * /*model*/, mjData * data) data->xfrc_applied[body_id_ * 6 + 2] = Fz; // Torque correction for off-CoM attachment: τ = offset_world × F. - const double ox = xmat[0] * body_offset_[0] + xmat[1] * body_offset_[1] - + xmat[2] * body_offset_[2]; - const double oy = xmat[3] * body_offset_[0] + xmat[4] * body_offset_[1] - + xmat[5] * body_offset_[2]; - const double oz = xmat[6] * body_offset_[0] + xmat[7] * body_offset_[1] - + xmat[8] * body_offset_[2]; + const double ox = xmat[0] * body_offset_[0] + xmat[1] * body_offset_[1] + xmat[2] * body_offset_[2]; + const double oy = xmat[3] * body_offset_[0] + xmat[4] * body_offset_[1] + xmat[5] * body_offset_[2]; + const double oz = xmat[6] * body_offset_[0] + xmat[7] * body_offset_[1] + xmat[8] * body_offset_[2]; data->xfrc_applied[body_id_ * 6 + 3] = oy * Fz - oz * Fy; data->xfrc_applied[body_id_ * 6 + 4] = oz * Fx - ox * Fz; @@ -230,6 +231,5 @@ void VirtualGantryPlugin::cleanup() } // namespace mujoco_ros2_control_plugins -PLUGINLIB_EXPORT_CLASS( - mujoco_ros2_control_plugins::VirtualGantryPlugin, - mujoco_ros2_control_plugins::MuJoCoROS2ControlPluginBase) +PLUGINLIB_EXPORT_CLASS(mujoco_ros2_control_plugins::VirtualGantryPlugin, + mujoco_ros2_control_plugins::MuJoCoROS2ControlPluginBase) From 2f7b7fa5fbad6a69d550590dbeeaaf61a111e92a Mon Sep 17 00:00:00 2001 From: Eraimondi Date: Wed, 22 Apr 2026 16:03:21 +0200 Subject: [PATCH 14/19] fix(format): apply clang-format v19.1.1 to gantry plugin and interface Co-Authored-By: Claude Sonnet 4.6 --- .../src/mujoco_system_interface.cpp | 2 +- .../src/virtual_gantry_plugin.cpp | 100 +++++++++--------- 2 files changed, 50 insertions(+), 52 deletions(-) diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index c9eb4f71..450051e7 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -2322,7 +2322,7 @@ void MujocoSystemInterface::register_urdf_joints(const hardware_interface::Hardw { // Velocity interface will be used for impedance control - no PID needed RCLCPP_DEBUG(get_logger(), "Velocity command interface for joint '%s' will be used for impedance control", - actuator_name.c_str()); + actuator_name.c_str()); } else { diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index ae967e31..95973338 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -26,8 +26,7 @@ namespace mujoco_ros2_control_plugins { -bool VirtualGantryPlugin::init( - rclcpp::Node::SharedPtr node, const mjModel * model, mjData * /*data*/) +bool VirtualGantryPlugin::init(rclcpp::Node::SharedPtr node, const mjModel* model, mjData* /*data*/) { node_ = node; @@ -36,13 +35,15 @@ bool VirtualGantryPlugin::init( const std::string prefix = "mujoco_plugins." + node->get_sub_namespace() + "."; auto params = node->get_node_parameters_interface(); - auto get_double = [&](const std::string & key, double & out) { - if (params->has_parameter(prefix + key)) { + auto get_double = [&](const std::string& key, double& out) { + if (params->has_parameter(prefix + key)) + { out = params->get_parameter(prefix + key).get_parameter_value().get(); } }; - auto get_string = [&](const std::string & key, std::string & out) { - if (params->has_parameter(prefix + key)) { + auto get_string = [&](const std::string& key, std::string& out) { + if (params->has_parameter(prefix + key)) + { out = params->get_parameter(prefix + key).get_parameter_value().get(); } }; @@ -52,87 +53,84 @@ bool VirtualGantryPlugin::init( get_double("kd_pos", kd_pos_); get_double("anchor_z", anchor_z_world_); - if (params->has_parameter(prefix + "body_offset")) { - const auto v = params->get_parameter(prefix + "body_offset") - .get_parameter_value().get>(); - if (v.size() == 3) { - body_offset_ = {{v[0], v[1], v[2]}}; + if (params->has_parameter(prefix + "body_offset")) + { + const auto v = params->get_parameter(prefix + "body_offset").get_parameter_value().get>(); + if (v.size() == 3) + { + body_offset_ = { { v[0], v[1], v[2] } }; } } body_id_ = mj_name2id(model, mjOBJ_BODY, body_name_.c_str()); - if (body_id_ < 0) { - RCLCPP_ERROR(node_->get_logger(), "VirtualGantryPlugin: body '%s' not found", - body_name_.c_str()); + if (body_id_ < 0) + { + RCLCPP_ERROR(node_->get_logger(), "VirtualGantryPlugin: body '%s' not found", body_name_.c_str()); return false; } RCLCPP_INFO(node_->get_logger(), "VirtualGantryPlugin: body '%s' (id=%d), anchor_z=%.2f, " "offset=[%.3f,%.3f,%.3f], kp=%.0f, kd=%.0f", - body_name_.c_str(), body_id_, anchor_z_world_, - body_offset_[0], body_offset_[1], body_offset_[2], kp_pos_, kd_pos_); + body_name_.c_str(), body_id_, anchor_z_world_, body_offset_[0], body_offset_[1], body_offset_[2], kp_pos_, + kd_pos_); enable_srv_ = node_->create_service( - "set_gantry_enabled", - [this]( - const mujoco_ros2_control_msgs::srv::SetGantryEnabled::Request::SharedPtr req, - mujoco_ros2_control_msgs::srv::SetGantryEnabled::Response::SharedPtr resp) - { - std::lock_guard lock(state_mutex_); - const bool was_enabled = enabled_; - enabled_ = req->enabled; - if (enabled_ && !was_enabled) { - spawn_pos_captured_ = false; // re-anchor above current attach position - } - resp->success = true; - resp->message = enabled_ ? "Gantry enabled" : "Gantry disabled"; - RCLCPP_INFO(node_->get_logger(), "%s", resp->message.c_str()); - }); + "set_gantry_enabled", [this](const mujoco_ros2_control_msgs::srv::SetGantryEnabled::Request::SharedPtr req, + mujoco_ros2_control_msgs::srv::SetGantryEnabled::Response::SharedPtr resp) { + std::lock_guard lock(state_mutex_); + const bool was_enabled = enabled_; + enabled_ = req->enabled; + if (enabled_ && !was_enabled) + { + spawn_pos_captured_ = false; // re-anchor above current attach position + } + resp->success = true; + resp->message = enabled_ ? "Gantry enabled" : "Gantry disabled"; + RCLCPP_INFO(node_->get_logger(), "%s", resp->message.c_str()); + }); // Snapshot the current toggle counter so the first update() doesn't misfire. - last_toggle_count_ = GantryKeyboardState::get().toggle_counter.load( - std::memory_order_relaxed); + last_toggle_count_ = GantryKeyboardState::get().toggle_counter.load(std::memory_order_relaxed); return true; } -void VirtualGantryPlugin::update(const mjModel * /*model*/, mjData * data) +void VirtualGantryPlugin::update(const mjModel* /*model*/, mjData* data) { std::lock_guard lock(state_mutex_); // --- Keyboard toggle ('G' key) ------------------------------------------- - const int tc = GantryKeyboardState::get().toggle_counter.load( - std::memory_order_relaxed); - if (tc != last_toggle_count_) { + const int tc = GantryKeyboardState::get().toggle_counter.load(std::memory_order_relaxed); + if (tc != last_toggle_count_) + { last_toggle_count_ = tc; enabled_ = !enabled_; - if (enabled_) { + if (enabled_) + { spawn_pos_captured_ = false; // re-anchor above current position on next step } - RCLCPP_INFO(node_->get_logger(), "VirtualGantryPlugin: %s via keyboard", - enabled_ ? "enabled" : "disabled"); + RCLCPP_INFO(node_->get_logger(), "VirtualGantryPlugin: %s via keyboard", enabled_ ? "enabled" : "disabled"); } // --- Rope length adjustment ('[' / ']' keys) ------------------------------- - const int scroll_ticks = - GantryKeyboardState::get().rope_length_ticks.exchange(0, std::memory_order_relaxed); - if (scroll_ticks != 0) { + const int scroll_ticks = GantryKeyboardState::get().rope_length_ticks.exchange(0, std::memory_order_relaxed); + if (scroll_ticks != 0) + { rope_length_ = std::max(0.1, rope_length_ + scroll_ticks * 0.005); RCLCPP_INFO(node_->get_logger(), "VirtualGantryPlugin: rope_length=%.3f m", rope_length_); } // --- Compute attachment point in world frame ------------------------------ // attach_pos = body CoM + rotation_matrix * body_offset - const double * xpos = &data->xpos[body_id_ * 3]; - const double * xmat = &data->xmat[body_id_ * 9]; + const double* xpos = &data->xpos[body_id_ * 3]; + const double* xmat = &data->xmat[body_id_ * 9]; double attach_pos[3]; - for (int i = 0; i < 3; ++i) { - attach_pos[i] = xpos[i] - + xmat[i * 3 + 0] * body_offset_[0] - + xmat[i * 3 + 1] * body_offset_[1] - + xmat[i * 3 + 2] * body_offset_[2]; + for (int i = 0; i < 3; ++i) + { + attach_pos[i] = xpos[i] + xmat[i * 3 + 0] * body_offset_[0] + xmat[i * 3 + 1] * body_offset_[1] + + xmat[i * 3 + 2] * body_offset_[2]; } // --- Capture anchor on first step after (re-)enable ---------------------- @@ -144,7 +142,7 @@ void VirtualGantryPlugin::update(const mjModel * /*model*/, mjData * data) rope_length_ = std::abs(anchor_z_world_ - attach_pos[2]); spawn_pos_captured_ = true; RCLCPP_INFO(node_->get_logger(), "VirtualGantryPlugin: anchor at [%.3f, %.3f, %.3f], rope_length=%.3f m", - anchor_pos_[0], anchor_pos_[1], anchor_pos_[2], rope_length_); + anchor_pos_[0], anchor_pos_[1], anchor_pos_[2], rope_length_); } // Rope vector from anchor to attachment point. From 82999e422ad3437f04afacec166f37e5a07ae56e Mon Sep 17 00:00:00 2001 From: Eraimondi Date: Thu, 23 Apr 2026 11:41:59 +0200 Subject: [PATCH 15/19] =?UTF-8?q?fix:=20address=20review=20comments=20?= =?UTF-8?q?=E2=80=94=20rope=20step=20docs,=20stale=20Shift+scroll=20ref,?= =?UTF-8?q?=20cleanup=20UAF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Fix '[ ]/]' step comment: 5 cm → 0.5 cm in hpp and mujoco_system_interface.cpp - Fix stale 'Shift+scroll' reference in rope_length_ docstring → '['/']' keys - Fix cleanup() UAF: hold state_mutex_ before resetting enable_srv_ and node_ Co-Authored-By: Claude Sonnet 4.6 --- mujoco_ros2_control/src/mujoco_system_interface.cpp | 2 +- .../mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp | 4 ++-- mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp | 3 +++ 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 450051e7..9d75af57 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -277,7 +277,7 @@ class ROS2ControlGlfwAdapter : public mj::GlfwAdapter return; } - // '[' = shorten rope, ']' = lengthen rope (5 cm per press; hold for repeat). + // '[' = shorten rope, ']' = lengthen rope (0.5 cm per press; hold for repeat). // Note: GlfwAdapter's GLFW scroll callback calls PlatformUIAdapter::OnScroll via // a devirtualized direct call, so OnScroll overrides cannot intercept scroll events. // Keyboard-based adjustment here is the reliable alternative. diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp index b4e9140f..dbf1696c 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp @@ -39,7 +39,7 @@ namespace mujoco_ros2_control_plugins // at spawn so the rope is just taut at the moment of activation. // // 'G' in the MuJoCo viewer toggles the gantry on/off. -// '[' shortens and ']' lengthens the rope by 5 cm per keypress (hold to repeat). +// '[' shortens and ']' lengthens the rope by 0.5 cm per keypress (hold to repeat). // The set_gantry_enabled ROS 2 service also enables/disables the gantry. class VirtualGantryPlugin : public MuJoCoROS2ControlPluginBase { @@ -68,7 +68,7 @@ class VirtualGantryPlugin : public MuJoCoROS2ControlPluginBase double anchor_z_world_{ 1.5 }; std::array anchor_pos_{}; - // Rope length at spawn = |anchor_z_world_ - attach_z|; adjustable at runtime via Shift+scroll. + // Rope length at spawn = |anchor_z_world_ - attach_z|; adjustable at runtime via '['/']' keys. double rope_length_{ 0.0 }; // Finite-difference state for rope-extension-rate damping. diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index 95973338..2a543944 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -223,6 +223,9 @@ void VirtualGantryPlugin::reset() void VirtualGantryPlugin::cleanup() { + // Hold state_mutex_ so any in-flight service callback (which also takes the lock and + // dereferences node_) finishes before we tear down the service and node references. + std::lock_guard lock(state_mutex_); enable_srv_.reset(); node_.reset(); } From 5b5d55b1cebe4e910329a0de149cfce446a2acf1 Mon Sep 17 00:00:00 2001 From: Eraimondi Date: Thu, 23 Apr 2026 11:54:53 +0200 Subject: [PATCH 16/19] refactor(plugins): dispatch GLFW key events via plugin virtual instead of shared singleton Replace the GantryKeyboardState singleton + RTLD_GLOBAL + dlopen/dladdr/CMAKE_DL_LIBS machinery with a single on_key() virtual on MuJoCoROS2ControlPluginBase. The GLFW adapter now fans out every key event to all loaded plugins; each plugin handles the keys it cares about via its own std::atomic members. - Add on_key() to MuJoCoROS2ControlPluginBase (UI-thread, non-blocking contract) - ROS2ControlGlfwAdapter accepts plugin_instances_ by reference and fans out OnKey - VirtualGantryPlugin overrides on_key(), owns toggle_counter_ and rope_length_ticks_ - Delete gantry_keyboard_state.hpp and gantry_keyboard_state.cpp - Remove CMAKE_DL_LIBS linkage and gantry_keyboard_state.cpp from CMakeLists.txt Co-Authored-By: Claude Sonnet 4.6 --- mujoco_ros2_control/CMakeLists.txt | 2 - .../src/gantry_keyboard_state.cpp | 30 ----------- .../src/mujoco_system_interface.cpp | 54 +++++-------------- .../gantry_keyboard_state.hpp | 46 ---------------- .../mujoco_ros2_control_plugins_base.hpp | 9 ++++ .../virtual_gantry_plugin.hpp | 8 ++- .../src/virtual_gantry_plugin.cpp | 49 +++++++++++++---- 7 files changed, 67 insertions(+), 131 deletions(-) delete mode 100644 mujoco_ros2_control/src/gantry_keyboard_state.cpp delete mode 100644 mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/gantry_keyboard_state.hpp diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index 9b03b961..d393213d 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -152,7 +152,6 @@ add_library(mujoco_ros2_control SHARED src/mujoco_system_interface.cpp src/mujoco_cameras.cpp src/mujoco_lidar.cpp - src/gantry_keyboard_state.cpp ) # Ensure MuJoCo library can be found at runtime regardless of how it was installed above. # mujoco_vendor installs libmujoco.so under opt/mujoco_vendor/lib/ relative to the @@ -189,7 +188,6 @@ PRIVATE Threads::Threads lodepng glfw - ${CMAKE_DL_LIBS} ) # Note: the transmissions_interface must NOT be statically linked against the library, only headers # should be used. Otherwise it will be loaded by the linker at process startup and its diff --git a/mujoco_ros2_control/src/gantry_keyboard_state.cpp b/mujoco_ros2_control/src/gantry_keyboard_state.cpp deleted file mode 100644 index 567620d9..00000000 --- a/mujoco_ros2_control/src/gantry_keyboard_state.cpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright 2026 NVIDIA CORPORATION & AFFILIATES -// -// 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. - -#include "mujoco_ros2_control_plugins/gantry_keyboard_state.hpp" - -namespace mujoco_ros2_control_plugins -{ - -// Defined in libmujoco_ros2_control.so (not in the plugins DSO) so there is -// exactly one instance. visibility("default") ensures the symbol is exported -// even when -fvisibility=hidden is active, so the plugins DSO can resolve it -// from the global symbol table at dlopen time. -__attribute__((visibility("default"))) GantryKeyboardState& GantryKeyboardState::get() -{ - static GantryKeyboardState instance; - return instance; -} - -} // namespace mujoco_ros2_control_plugins diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 9d75af57..97494286 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -19,12 +19,10 @@ #include "mujoco_ros2_control/mujoco_system_interface.hpp" #include "array_safety.h" -#include "mujoco_ros2_control_plugins/gantry_keyboard_state.hpp" #include #include -#include #include #include #include @@ -252,7 +250,10 @@ class HeadlessAdapter : public mj::PlatformUIAdapter class ROS2ControlGlfwAdapter : public mj::GlfwAdapter { public: - explicit ROS2ControlGlfwAdapter(std::atomic& step_requested) : step_requested_(step_requested) + explicit ROS2ControlGlfwAdapter( + std::atomic& step_requested, + std::vector>& plugins) + : step_requested_(step_requested), plugins_(plugins) { } @@ -270,34 +271,19 @@ class ROS2ControlGlfwAdapter : public mj::GlfwAdapter return; } - // Toggle gantry on/off with 'G'. - if (key == GLFW_KEY_G && act == GLFW_PRESS) + // Fan out to all plugins so each can handle the keys it cares about. + for (auto& plugin : plugins_) { - mujoco_ros2_control_plugins::GantryKeyboardState::get().toggle_counter.fetch_add(1, std::memory_order_relaxed); - return; - } - - // '[' = shorten rope, ']' = lengthen rope (0.5 cm per press; hold for repeat). - // Note: GlfwAdapter's GLFW scroll callback calls PlatformUIAdapter::OnScroll via - // a devirtualized direct call, so OnScroll overrides cannot intercept scroll events. - // Keyboard-based adjustment here is the reliable alternative. - if (key == GLFW_KEY_LEFT_BRACKET && (act == GLFW_PRESS || act == GLFW_REPEAT)) - { - mujoco_ros2_control_plugins::GantryKeyboardState::get().rope_length_ticks.fetch_add(-1, std::memory_order_relaxed); - return; - } - if (key == GLFW_KEY_RIGHT_BRACKET && (act == GLFW_PRESS || act == GLFW_REPEAT)) - { - mujoco_ros2_control_plugins::GantryKeyboardState::get().rope_length_ticks.fetch_add(1, std::memory_order_relaxed); - return; + plugin->on_key(key, scancode, act, 0); } - // Forward all other keys so normal UI behaviour is preserved. + // Forward to parent for all standard MuJoCo viewer bindings. mj::GlfwAdapter::OnKey(key, scancode, act); } private: std::atomic& step_requested_; + std::vector>& plugins_; }; // Clamps v to the lo or high value @@ -852,9 +838,9 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf { // Launch the UI loop in the background ui_thread_ = std::thread([this, sim_ready]() { - sim_ = std::make_unique(std::make_unique(keyboard_step_requested_), &cam_, - &opt_, &pert_, - /* is_passive = */ false); + sim_ = std::make_unique( + std::make_unique(keyboard_step_requested_, plugin_instances_), &cam_, &opt_, &pert_, + /* is_passive = */ false); // Add ros2 control icon for the taskbar std::string icon_location = @@ -3386,22 +3372,6 @@ rclcpp::Node::SharedPtr MujocoSystemInterface::get_node() const void MujocoSystemInterface::load_mujoco_plugins() { - // Ensure GantryKeyboardState::get() (defined in this DSO) is visible globally so - // that libmujoco_ros2_control_plugins.so can resolve it at dlopen time. - // class_loader may load hardware-interface plugins with RTLD_LOCAL, which - // would otherwise keep our symbols out of the global symbol table. - { - Dl_info info{}; - if (dladdr(reinterpret_cast(&MujocoSystemInterface::load_mujoco_plugins), &info) && info.dli_fname) - { - void* h = dlopen(info.dli_fname, RTLD_LAZY | RTLD_NOLOAD | RTLD_GLOBAL); - if (h) - { - dlclose(h); // just promoting visibility, don't hold an extra reference - } - } - } - try { plugin_loader_ = std::make_unique>( diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/gantry_keyboard_state.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/gantry_keyboard_state.hpp deleted file mode 100644 index 38562b36..00000000 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/gantry_keyboard_state.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2026 NVIDIA CORPORATION & AFFILIATES -// -// 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 MUJOCO_ROS2_CONTROL_PLUGINS__GANTRY_KEYBOARD_STATE_HPP_ -#define MUJOCO_ROS2_CONTROL_PLUGINS__GANTRY_KEYBOARD_STATE_HPP_ - -#include - -namespace mujoco_ros2_control_plugins -{ - -// Singleton that bridges the GLFW UI thread (key events) and the -// VirtualGantryPlugin physics thread. All fields are atomic — no mutex needed. -struct GantryKeyboardState -{ - // Incremented on each 'G' key press. Plugin compares against its last-seen - // value to detect a toggle edge without consuming the count. - std::atomic toggle_counter{ 0 }; - - // Accumulated rope-length ticks from '['/']' key presses. - // Plugin exchanges to 0 after reading. - // Positive = lengthen rope, negative = shorten. - std::atomic rope_length_ticks{ 0 }; - - // Defined in gantry_keyboard_state.cpp so there is exactly one instance across - // all shared libraries (mujoco_ros2_control_plugins + mujoco_ros2_control). - static GantryKeyboardState& get(); - -private: - GantryKeyboardState() = default; -}; - -} // namespace mujoco_ros2_control_plugins - -#endif // MUJOCO_ROS2_CONTROL_PLUGINS__GANTRY_KEYBOARD_STATE_HPP_ diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp index baf322de..5d395667 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp @@ -70,6 +70,15 @@ class MuJoCoROS2ControlPluginBase virtual void reset() { } + + /** + * @brief Called from the GLFW UI thread on every key event. + * @note Invoked on the UI thread, not the physics thread. Implementations must be + * non-blocking and hand off state to update() via atomics. + */ + virtual void on_key(int /*key*/, int /*scancode*/, int /*action*/, int /*mods*/) + { + } }; } // namespace mujoco_ros2_control_plugins diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp index dbf1696c..af4ff41e 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp @@ -16,6 +16,7 @@ #define MUJOCO_ROS2_CONTROL_PLUGINS__VIRTUAL_GANTRY_PLUGIN_HPP_ #include +#include #include #include @@ -48,6 +49,7 @@ class VirtualGantryPlugin : public MuJoCoROS2ControlPluginBase void update(const mjModel* model, mjData* data) override; void reset() override; void cleanup() override; + void on_key(int key, int scancode, int action, int mods) override; private: rclcpp::Node::SharedPtr node_; @@ -82,7 +84,11 @@ class VirtualGantryPlugin : public MuJoCoROS2ControlPluginBase bool enabled_{ true }; bool spawn_pos_captured_{ false }; - // Last toggle_counter value seen; used to detect 'G' key edges. + // Atomic key-event counters written by on_key() (UI thread) and consumed by update() (physics thread). + std::atomic toggle_counter_{ 0 }; + std::atomic rope_length_ticks_{ 0 }; + + // Last toggle_counter value seen; used to detect 'G' key edges in update(). int last_toggle_count_{ 0 }; std::mutex state_mutex_; diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index 2a543944..9c64b642 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -21,8 +21,6 @@ #include #include -#include "mujoco_ros2_control_plugins/gantry_keyboard_state.hpp" - namespace mujoco_ros2_control_plugins { @@ -91,7 +89,7 @@ bool VirtualGantryPlugin::init(rclcpp::Node::SharedPtr node, const mjModel* mode }); // Snapshot the current toggle counter so the first update() doesn't misfire. - last_toggle_count_ = GantryKeyboardState::get().toggle_counter.load(std::memory_order_relaxed); + last_toggle_count_ = toggle_counter_.load(std::memory_order_relaxed); return true; } @@ -101,20 +99,24 @@ void VirtualGantryPlugin::update(const mjModel* /*model*/, mjData* data) std::lock_guard lock(state_mutex_); // --- Keyboard toggle ('G' key) ------------------------------------------- - const int tc = GantryKeyboardState::get().toggle_counter.load(std::memory_order_relaxed); - if (tc != last_toggle_count_) + const int tc = toggle_counter_.load(std::memory_order_acquire); + const int delta = tc - last_toggle_count_; + if (delta != 0) { last_toggle_count_ = tc; - enabled_ = !enabled_; - if (enabled_) + if (delta & 1) { - spawn_pos_captured_ = false; // re-anchor above current position on next step + enabled_ = !enabled_; + if (enabled_) + { + spawn_pos_captured_ = false; // re-anchor above current position on next step + } + RCLCPP_INFO(node_->get_logger(), "VirtualGantryPlugin: %s via keyboard", enabled_ ? "enabled" : "disabled"); } - RCLCPP_INFO(node_->get_logger(), "VirtualGantryPlugin: %s via keyboard", enabled_ ? "enabled" : "disabled"); } // --- Rope length adjustment ('[' / ']' keys) ------------------------------- - const int scroll_ticks = GantryKeyboardState::get().rope_length_ticks.exchange(0, std::memory_order_relaxed); + const int scroll_ticks = rope_length_ticks_.exchange(0, std::memory_order_acquire); if (scroll_ticks != 0) { rope_length_ = std::max(0.1, rope_length_ + scroll_ticks * 0.005); @@ -221,6 +223,33 @@ void VirtualGantryPlugin::reset() last_update_time_ = -1.0; } +void VirtualGantryPlugin::on_key(int key, int /*scancode*/, int action, int /*mods*/) +{ + // GLFW constants (stable values matching glfw3.h; avoids adding glfw as a plugin dependency). + constexpr int kPress = 1; + constexpr int kRepeat = 2; + constexpr int kKeyG = 71; + constexpr int kKeyLeftBracket = 91; + constexpr int kKeyRightBracket = 93; + + if (action != kPress && action != kRepeat) + { + return; + } + if (key == kKeyG && action == kPress) + { + toggle_counter_.fetch_add(1, std::memory_order_release); + } + else if (key == kKeyLeftBracket) + { + rope_length_ticks_.fetch_add(-1, std::memory_order_release); + } + else if (key == kKeyRightBracket) + { + rope_length_ticks_.fetch_add(1, std::memory_order_release); + } +} + void VirtualGantryPlugin::cleanup() { // Hold state_mutex_ so any in-flight service callback (which also takes the lock and From 84766f5de603d910175255c967e5dedca06489ad Mon Sep 17 00:00:00 2001 From: Eraimondi Date: Thu, 23 Apr 2026 12:33:19 +0200 Subject: [PATCH 17/19] fix(plugins): on_key returns bool to prevent consumed keys reaching MuJoCo viewer [ and ] were being forwarded to the parent GlfwAdapter after plugin handling, triggering MuJoCo's native camera-switching binding. on_key now returns true when a key is consumed; the adapter skips mj::GlfwAdapter::OnKey in that case. Co-Authored-By: Claude Sonnet 4.6 --- mujoco_ros2_control/src/mujoco_system_interface.cpp | 13 ++++++++----- .../mujoco_ros2_control_plugins_base.hpp | 4 +++- .../virtual_gantry_plugin.hpp | 2 +- .../src/virtual_gantry_plugin.cpp | 12 ++++++++---- 4 files changed, 20 insertions(+), 11 deletions(-) diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 97494286..232e9b20 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -271,14 +271,17 @@ class ROS2ControlGlfwAdapter : public mj::GlfwAdapter return; } - // Fan out to all plugins so each can handle the keys it cares about. + // Fan out to all plugins. If any plugin consumes the event, do not forward + // to the MuJoCo viewer (prevents conflicts with native viewer key bindings). + bool consumed = false; for (auto& plugin : plugins_) { - plugin->on_key(key, scancode, act, 0); + consumed |= plugin->on_key(key, scancode, act, 0); + } + if (!consumed) + { + mj::GlfwAdapter::OnKey(key, scancode, act); } - - // Forward to parent for all standard MuJoCo viewer bindings. - mj::GlfwAdapter::OnKey(key, scancode, act); } private: diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp index 5d395667..e369d0e4 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp @@ -73,11 +73,13 @@ class MuJoCoROS2ControlPluginBase /** * @brief Called from the GLFW UI thread on every key event. + * @return true if the event was consumed (caller should not forward to the MuJoCo viewer). * @note Invoked on the UI thread, not the physics thread. Implementations must be * non-blocking and hand off state to update() via atomics. */ - virtual void on_key(int /*key*/, int /*scancode*/, int /*action*/, int /*mods*/) + virtual bool on_key(int /*key*/, int /*scancode*/, int /*action*/, int /*mods*/) { + return false; } }; diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp index af4ff41e..f079be61 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp @@ -49,7 +49,7 @@ class VirtualGantryPlugin : public MuJoCoROS2ControlPluginBase void update(const mjModel* model, mjData* data) override; void reset() override; void cleanup() override; - void on_key(int key, int scancode, int action, int mods) override; + bool on_key(int key, int scancode, int action, int mods) override; private: rclcpp::Node::SharedPtr node_; diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index 9c64b642..5bd6a3eb 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -223,7 +223,7 @@ void VirtualGantryPlugin::reset() last_update_time_ = -1.0; } -void VirtualGantryPlugin::on_key(int key, int /*scancode*/, int action, int /*mods*/) +bool VirtualGantryPlugin::on_key(int key, int /*scancode*/, int action, int /*mods*/) { // GLFW constants (stable values matching glfw3.h; avoids adding glfw as a plugin dependency). constexpr int kPress = 1; @@ -234,20 +234,24 @@ void VirtualGantryPlugin::on_key(int key, int /*scancode*/, int action, int /*mo if (action != kPress && action != kRepeat) { - return; + return false; } if (key == kKeyG && action == kPress) { toggle_counter_.fetch_add(1, std::memory_order_release); + return true; } - else if (key == kKeyLeftBracket) + if (key == kKeyLeftBracket) { rope_length_ticks_.fetch_add(-1, std::memory_order_release); + return true; } - else if (key == kKeyRightBracket) + if (key == kKeyRightBracket) { rope_length_ticks_.fetch_add(1, std::memory_order_release); + return true; } + return false; } void VirtualGantryPlugin::cleanup() From 190928566d554f06d145b2a852f6073aa3c9eaae Mon Sep 17 00:00:00 2001 From: Eraimondi Date: Thu, 23 Apr 2026 12:37:46 +0200 Subject: [PATCH 18/19] refactor(gantry): replace SetGantryEnabled custom srv with std_srvs/SetBool Drop mujoco_ros2_control_msgs/srv/SetGantryEnabled (structurally identical to std_srvs/SetBool). Update VirtualGantryPlugin, its deps (CMakeLists, package.xml), and the README service examples (req->enabled -> req->data, type updated). Co-Authored-By: Claude Sonnet 4.6 --- mujoco_ros2_control_msgs/CMakeLists.txt | 1 - mujoco_ros2_control_msgs/srv/SetGantryEnabled.srv | 4 ---- mujoco_ros2_control_plugins/CMakeLists.txt | 6 +++--- mujoco_ros2_control_plugins/README.md | 6 +++--- .../mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp | 4 ++-- mujoco_ros2_control_plugins/package.xml | 2 +- mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp | 8 ++++---- 7 files changed, 13 insertions(+), 18 deletions(-) delete mode 100644 mujoco_ros2_control_msgs/srv/SetGantryEnabled.srv diff --git a/mujoco_ros2_control_msgs/CMakeLists.txt b/mujoco_ros2_control_msgs/CMakeLists.txt index a205cc9e..feeff3dc 100644 --- a/mujoco_ros2_control_msgs/CMakeLists.txt +++ b/mujoco_ros2_control_msgs/CMakeLists.txt @@ -10,7 +10,6 @@ find_package(rosidl_default_generators REQUIRED) set(srv_files srv/ResetWorld.srv - srv/SetGantryEnabled.srv srv/SetGantryTarget.srv srv/SetPause.srv srv/StepSimulation.srv diff --git a/mujoco_ros2_control_msgs/srv/SetGantryEnabled.srv b/mujoco_ros2_control_msgs/srv/SetGantryEnabled.srv deleted file mode 100644 index ec42aae0..00000000 --- a/mujoco_ros2_control_msgs/srv/SetGantryEnabled.srv +++ /dev/null @@ -1,4 +0,0 @@ -bool enabled ---- -bool success -string message diff --git a/mujoco_ros2_control_plugins/CMakeLists.txt b/mujoco_ros2_control_plugins/CMakeLists.txt index e1262d01..bae9bd05 100644 --- a/mujoco_ros2_control_plugins/CMakeLists.txt +++ b/mujoco_ros2_control_plugins/CMakeLists.txt @@ -8,10 +8,10 @@ export_windows_symbols() # find dependencies find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(mujoco_ros2_control_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(pluginlib REQUIRED) find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) # Attempt to link MuJoCo via the vendor package, if available find_package(mujoco_vendor QUIET) @@ -42,8 +42,8 @@ target_link_libraries(mujoco_ros2_control_plugins PUBLIC rclcpp::rclcpp pluginlib::pluginlib ${geometry_msgs_TARGETS} - ${mujoco_ros2_control_msgs_TARGETS} ${std_msgs_TARGETS} + ${std_srvs_TARGETS} ) # Install the plugin library @@ -69,7 +69,7 @@ install(DIRECTORY include/ pluginlib_export_plugin_description_file(mujoco_ros2_control_plugins mujoco_ros2_control_plugins.xml) ament_export_include_directories(include) -ament_export_dependencies(geometry_msgs mujoco_ros2_control_msgs mujoco_vendor rclcpp pluginlib std_msgs ros2_control_cmake) +ament_export_dependencies(geometry_msgs mujoco_vendor rclcpp pluginlib std_msgs std_srvs ros2_control_cmake) ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_package() diff --git a/mujoco_ros2_control_plugins/README.md b/mujoco_ros2_control_plugins/README.md index 7ead07e4..9e2b1879 100644 --- a/mujoco_ros2_control_plugins/README.md +++ b/mujoco_ros2_control_plugins/README.md @@ -60,18 +60,18 @@ attachment position. #### ROS 2 interface -**Service**: `set_gantry_enabled` (`mujoco_ros2_control_msgs/srv/SetGantryEnabled`) +**Service**: `set_gantry_enabled` (`std_srvs/srv/SetBool`) The full service path is `///set_gantry_enabled`. With the default node name and the plugin key `virtual_gantry` from the example config above: ```bash # Enable ros2 service call /mujoco_ros2_control_node/virtual_gantry/set_gantry_enabled \ - mujoco_ros2_control_msgs/srv/SetGantryEnabled '{enabled: true}' + std_srvs/srv/SetBool '{data: true}' # Disable ros2 service call /mujoco_ros2_control_node/virtual_gantry/set_gantry_enabled \ - mujoco_ros2_control_msgs/srv/SetGantryEnabled '{enabled: false}' + std_srvs/srv/SetBool '{data: false}' ``` #### Example configuration diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp index f079be61..61bd6ee2 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp @@ -20,9 +20,9 @@ #include #include -#include "mujoco_ros2_control_msgs/srv/set_gantry_enabled.hpp" #include "mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp" #include "rclcpp/rclcpp.hpp" +#include "std_srvs/srv/set_bool.hpp" namespace mujoco_ros2_control_plugins { @@ -93,7 +93,7 @@ class VirtualGantryPlugin : public MuJoCoROS2ControlPluginBase std::mutex state_mutex_; - rclcpp::Service::SharedPtr enable_srv_; + rclcpp::Service::SharedPtr enable_srv_; }; } // namespace mujoco_ros2_control_plugins diff --git a/mujoco_ros2_control_plugins/package.xml b/mujoco_ros2_control_plugins/package.xml index 9ae385f1..432fb935 100644 --- a/mujoco_ros2_control_plugins/package.xml +++ b/mujoco_ros2_control_plugins/package.xml @@ -19,12 +19,12 @@ ament_cmake geometry_msgs - mujoco_ros2_control_msgs mujoco_vendor rclcpp ros2_control_cmake pluginlib std_msgs + std_srvs ament_cmake_gtest diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index 5bd6a3eb..ac5c0e31 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -73,12 +73,12 @@ bool VirtualGantryPlugin::init(rclcpp::Node::SharedPtr node, const mjModel* mode body_name_.c_str(), body_id_, anchor_z_world_, body_offset_[0], body_offset_[1], body_offset_[2], kp_pos_, kd_pos_); - enable_srv_ = node_->create_service( - "set_gantry_enabled", [this](const mujoco_ros2_control_msgs::srv::SetGantryEnabled::Request::SharedPtr req, - mujoco_ros2_control_msgs::srv::SetGantryEnabled::Response::SharedPtr resp) { + enable_srv_ = node_->create_service( + "set_gantry_enabled", + [this](const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr resp) { std::lock_guard lock(state_mutex_); const bool was_enabled = enabled_; - enabled_ = req->enabled; + enabled_ = req->data; if (enabled_ && !was_enabled) { spawn_pos_captured_ = false; // re-anchor above current attach position From af5bd239cb37f518617a60483c5a86e755053083 Mon Sep 17 00:00:00 2001 From: Eraimondi Date: Tue, 28 Apr 2026 12:04:07 +0200 Subject: [PATCH 19/19] fix(gantry): ignore [ / ] rope adjustment when gantry is disabled Pressing [ or ] while the gantry is disabled previously modified rope_length_ and logged a misleading length-change message, giving no indication that the gantry was inactive. Now the ticks are discarded and a WARN is emitted instead. Keys are still consumed so they don't fall through to the MuJoCo viewer camera handler. README updated to document the disabled-state behavior. Co-Authored-By: Claude Sonnet 4.6 --- mujoco_ros2_control_plugins/README.md | 4 ++-- .../src/virtual_gantry_plugin.cpp | 11 +++++++++-- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/mujoco_ros2_control_plugins/README.md b/mujoco_ros2_control_plugins/README.md index 9e2b1879..fa0d9d4f 100644 --- a/mujoco_ros2_control_plugins/README.md +++ b/mujoco_ros2_control_plugins/README.md @@ -55,8 +55,8 @@ attachment position. | Key | Action | |---|---| | `G` | Toggle gantry on / off. Re-enabling re-anchors above the current attachment position | -| `[` | Shorten rope by 0.5 cm | -| `]` | Lengthen rope by 0.5 cm | +| `[` | Shorten rope by 0.5 cm *(ignored with a warning when gantry is disabled)* | +| `]` | Lengthen rope by 0.5 cm *(ignored with a warning when gantry is disabled)* | #### ROS 2 interface diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index ac5c0e31..5196b5a7 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -119,8 +119,15 @@ void VirtualGantryPlugin::update(const mjModel* /*model*/, mjData* data) const int scroll_ticks = rope_length_ticks_.exchange(0, std::memory_order_acquire); if (scroll_ticks != 0) { - rope_length_ = std::max(0.1, rope_length_ + scroll_ticks * 0.005); - RCLCPP_INFO(node_->get_logger(), "VirtualGantryPlugin: rope_length=%.3f m", rope_length_); + if (enabled_) + { + rope_length_ = std::max(0.1, rope_length_ + scroll_ticks * 0.005); + RCLCPP_INFO(node_->get_logger(), "VirtualGantryPlugin: rope_length=%.3f m", rope_length_); + } + else + { + RCLCPP_WARN(node_->get_logger(), "VirtualGantryPlugin: gantry disabled — rope length unchanged"); + } } // --- Compute attachment point in world frame ------------------------------