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/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 860443fe..232e9b20 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -44,7 +44,6 @@ #include #include #include -#include #include #include @@ -251,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) { } @@ -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& step_requested_; + std::vector>& 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(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 = @@ -1513,8 +1525,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) { @@ -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(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 +3193,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 +3250,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_msgs/CMakeLists.txt b/mujoco_ros2_control_msgs/CMakeLists.txt index 76d0f9e2..feeff3dc 100644 --- a/mujoco_ros2_control_msgs/CMakeLists.txt +++ b/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() 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/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..bae9bd05 100644 --- a/mujoco_ros2_control_plugins/CMakeLists.txt +++ b/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 $ @@ -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() diff --git a/mujoco_ros2_control_plugins/README.md b/mujoco_ros2_control_plugins/README.md index fb1c07e1..9e2b1879 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` (`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 \ + 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 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..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 @@ -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 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..61bd6ee2 --- /dev/null +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp @@ -0,0 +1,101 @@ +// 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 + +#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 +{ + +// 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. +// +// 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. +// '[' 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 +{ +public: + 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; + bool on_key(int key, int scancode, int action, int mods) override; + +private: + rclcpp::Node::SharedPtr node_; + + // Target body (attachment point on the robot). + 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 } }; + + // Rope tension spring/damper gains. + 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 }; + std::array anchor_pos_{}; + + // 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. + // 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 }; + + bool enabled_{ true }; + bool spawn_pos_captured_{ false }; + + // 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_; + + rclcpp::Service::SharedPtr enable_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..432fb935 100644 --- a/mujoco_ros2_control_plugins/package.xml +++ b/mujoco_ros2_control_plugins/package.xml @@ -18,11 +18,13 @@ ament_cmake + geometry_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 new file mode 100644 index 00000000..ac5c0e31 --- /dev/null +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -0,0 +1,269 @@ +// 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 +#include + +#include +#include + +namespace mujoco_ros2_control_plugins +{ + +bool VirtualGantryPlugin::init(rclcpp::Node::SharedPtr node, const mjModel* model, mjData* /*data*/) +{ + node_ = node; + + // Parameters live in the parent node's "mujoco_plugins..*" namespace, + // declared via automatically_declare_parameters_from_overrides. + 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)) + { + 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_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] } }; + } + } + + 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()); + 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_); + + 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->data; + 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_ = toggle_counter_.load(std::memory_order_relaxed); + + return true; +} + +void VirtualGantryPlugin::update(const mjModel* /*model*/, mjData* data) +{ + std::lock_guard lock(state_mutex_); + + // --- Keyboard toggle ('G' key) ------------------------------------------- + const int tc = toggle_counter_.load(std::memory_order_acquire); + const int delta = tc - last_toggle_count_; + if (delta != 0) + { + last_toggle_count_ = tc; + if (delta & 1) + { + 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 ('[' / ']' keys) ------------------------------- + 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_); + } + + // --- 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]; + + 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_) + { + // 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=%.3f m", + anchor_pos_[0], anchor_pos_[1], anchor_pos_[2], rope_length_); + } + + // 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); + + // --- 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 < 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; + } + + // 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; + + 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 + // fast, which creates an asymmetric energy cycle and growing oscillations. + const double extension = rope_dist - rope_length_; + 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]; + 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]; + + 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(state_mutex_); + spawn_pos_captured_ = false; + rope_dist_prev_ = -1.0; + rope_dist_dot_ = 0.0; + last_update_time_ = -1.0; +} + +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; + constexpr int kRepeat = 2; + constexpr int kKeyG = 71; + constexpr int kKeyLeftBracket = 91; + constexpr int kKeyRightBracket = 93; + + if (action != kPress && action != kRepeat) + { + return false; + } + if (key == kKeyG && action == kPress) + { + toggle_counter_.fetch_add(1, std::memory_order_release); + return true; + } + if (key == kKeyLeftBracket) + { + rope_length_ticks_.fetch_add(-1, std::memory_order_release); + return true; + } + if (key == kKeyRightBracket) + { + rope_length_ticks_.fetch_add(1, std::memory_order_release); + return true; + } + return false; +} + +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(); +} + +} // namespace mujoco_ros2_control_plugins + +PLUGINLIB_EXPORT_CLASS(mujoco_ros2_control_plugins::VirtualGantryPlugin, + mujoco_ros2_control_plugins::MuJoCoROS2ControlPluginBase)