fix(gantry): ignore [ / ] rope adjustment when gantry is disabled

mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp

@@ -28,23 +28,23 @@
 
 #include <hardware_interface/version.h>
 #include <atomic>
+#include <geometry_msgs/msg/transform_stamped.hpp>
 #include <hardware_interface/handle.hpp>
 #include <hardware_interface/hardware_info.hpp>
 #include <hardware_interface/system_interface.hpp>
 #include <hardware_interface/types/hardware_interface_return_values.hpp>
-#include <geometry_msgs/msg/transform_stamped.hpp>
 #include <mujoco_ros2_control_msgs/srv/reset_world.hpp>
 #include <mujoco_ros2_control_msgs/srv/set_pause.hpp>
 #include <mujoco_ros2_control_msgs/srv/step_simulation.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <rclcpp/macros.hpp>
-#include <tf2_ros/transform_broadcaster.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp>
 #include <rclcpp_lifecycle/state.hpp>
 #include <realtime_tools/realtime_publisher.hpp>
 #include <rosgraph_msgs/msg/clock.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
+#include <tf2_ros/transform_broadcaster.hpp>
 
 #include <mujoco/mujoco.h>
 

mujoco_ros2_control/src/mujoco_system_interface.cpp

@@ -44,7 +44,6 @@
 #include <stdexcept>
 #include <string>
 #include <thread>
-#include <unistd.h>
 
 #include <tinyxml2.h>
 #include <std_msgs/msg/string.hpp>
@@ -251,7 +250,10 @@ class HeadlessAdapter : public mj::PlatformUIAdapter
 class ROS2ControlGlfwAdapter : public mj::GlfwAdapter
 {
 public:
-  explicit ROS2ControlGlfwAdapter(std::atomic<bool>& step_requested) : step_requested_(step_requested)
+  explicit ROS2ControlGlfwAdapter(
+      std::atomic<bool>& step_requested,
+      std::vector<std::shared_ptr<mujoco_ros2_control_plugins::MuJoCoROS2ControlPluginBase>>& plugins)
+    : step_requested_(step_requested), plugins_(plugins)
   {
   }
 
@@ -269,12 +271,22 @@ class ROS2ControlGlfwAdapter : public mj::GlfwAdapter
       return;
     }
 
-    // Forward all other keys so normal UI behaviour is preserved.
-    mj::GlfwAdapter::OnKey(key, scancode, act);
+    // 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_)
+    {
+      consumed |= plugin->on_key(key, scancode, act, 0);
+    }
+    if (!consumed)
+    {
+      mj::GlfwAdapter::OnKey(key, scancode, act);
+    }
   }
 
 private:
   std::atomic<bool>& step_requested_;
+  std::vector<std::shared_ptr<mujoco_ros2_control_plugins::MuJoCoROS2ControlPluginBase>>& plugins_;
 };
 
 // Clamps v to the lo or high value
@@ -829,9 +841,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<mj::Simulate>(std::make_unique<ROS2ControlGlfwAdapter>(keyboard_step_requested_), &cam_,
-                                            &opt_, &pert_,
-                                            /* is_passive = */ false);
+      sim_ = std::make_unique<mj::Simulate>(
+          std::make_unique<ROS2ControlGlfwAdapter>(keyboard_step_requested_, plugin_instances_), &cam_, &opt_, &pert_,
+          /* is_passive = */ false);
 
       // Add ros2 control icon for the taskbar
       std::string icon_location =
@@ -1513,8 +1525,8 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vector<std::string
         joint_it->is_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)
       {
@@ -2276,9 +2288,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)
           {
@@ -2298,8 +2310,7 @@ 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",
+            RCLCPP_DEBUG(get_logger(), "Velocity command interface for joint '%s' will be used for impedance control",
                          actuator_name.c_str());
           }
           else
@@ -2896,6 +2907,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 +3140,7 @@ void MujocoSystemInterface::PhysicsLoop()
             // Copy data to the control
             mju_copy(mj_data_->ctrl, mj_data_control_->ctrl, static_cast<int>(mj_model_->nu));
             mju_copy(mj_data_->qfrc_applied, mj_data_control_->qfrc_applied, static_cast<int>(mj_model_->nu));
+            mju_copy(mj_data_->xfrc_applied, mj_data_control_->xfrc_applied, 6 * static_cast<int>(mj_model_->nbody));
             // run single step, let next iteration deal with timing
             mj_step(mj_model_, mj_data_);
 
@@ -3175,6 +3193,7 @@ void MujocoSystemInterface::PhysicsLoop()
               // Copy data to the control
               mju_copy(mj_data_->ctrl, mj_data_control_->ctrl, static_cast<int>(mj_model_->nu));
               mju_copy(mj_data_->qfrc_applied, mj_data_control_->qfrc_applied, static_cast<int>(mj_model_->nu));
+              mju_copy(mj_data_->xfrc_applied, mj_data_control_->xfrc_applied, 6 * static_cast<int>(mj_model_->nbody));
               // call mj_step
               mj_step(mj_model_, mj_data_);
 
@@ -3231,6 +3250,7 @@ void MujocoSystemInterface::PhysicsLoop()
           {
             mju_copy(mj_data_->ctrl, mj_data_control_->ctrl, static_cast<int>(mj_model_->nu));
             mju_copy(mj_data_->qfrc_applied, mj_data_control_->qfrc_applied, static_cast<int>(mj_model_->nu));
+            mju_copy(mj_data_->xfrc_applied, mj_data_control_->xfrc_applied, 6 * static_cast<int>(mj_model_->nbody));
             mj_step(mj_model_, mj_data_);
             publish_clock();
 

mujoco_ros2_control_msgs/CMakeLists.txt

@@ -4,11 +4,13 @@ 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/SetGantryTarget.srv
   srv/SetPause.srv
   srv/StepSimulation.srv
 )
@@ -18,6 +20,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   ${srv_files}
   DEPENDENCIES
     builtin_interfaces
+    geometry_msgs
 )
 ament_export_dependencies(rosidl_default_runtime)
 ament_package()

mujoco_ros2_control_msgs/package.xml

@@ -21,6 +21,7 @@
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
   <depend>builtin_interfaces</depend>
+  <depend>geometry_msgs</depend>
 
   <exec_depend>rosidl_default_runtime</exec_depend>
 

mujoco_ros2_control_msgs/srv/SetGantryTarget.srv

@@ -0,0 +1,4 @@
+geometry_msgs/Point target_position
+---
+bool success
+string message

mujoco_ros2_control_plugins/CMakeLists.txt

@@ -7,9 +7,11 @@ export_windows_symbols()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(geometry_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)
@@ -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
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -38,7 +41,9 @@ target_link_libraries(mujoco_ros2_control_plugins PUBLIC
   ${MUJOCO_LIB}
   rclcpp::rclcpp
   pluginlib::pluginlib
+  ${geometry_msgs_TARGETS}
   ${std_msgs_TARGETS}
+  ${std_srvs_TARGETS}
 )
 
 # Install the plugin library
@@ -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_vendor rclcpp pluginlib std_msgs std_srvs ros2_control_cmake)
 ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
 
 ament_package()

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 *(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
+
+**Service**: `set_gantry_enabled` (`std_srvs/srv/SetBool`)
+
+The full service path is `/<node_name>/<plugin_key>/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 \
+  std_srvs/srv/SetBool '{data: true}'
+
+# Disable
+ros2 service call /mujoco_ros2_control_node/virtual_gantry/set_gantry_enabled \
+  std_srvs/srv/SetBool '{data: 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

mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp

@@ -61,6 +61,26 @@ 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()
+  {
+  }
+
+  /**
+   * @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 bool on_key(int /*key*/, int /*scancode*/, int /*action*/, int /*mods*/)
+  {
+    return false;
+  }
 };
 
 }  // namespace mujoco_ros2_control_plugins