fix(gantry): ignore [ / ] rope adjustment when gantry is disabled#4
Closed
eraimondi-jpg wants to merge 19 commits intoNVIDIA-ISAAC-ROS:impedence-control-supportfrom
Closed
Conversation
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 <noreply@anthropic.com>
… stabilize PD gains - 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 <noreply@anthropic.com>
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.
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.
- '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).
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 <noreply@anthropic.com>
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
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 <noreply@anthropic.com>
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 <noreply@anthropic.com>
…ory registration
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 <noreply@anthropic.com>
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 <noreply@anthropic.com>
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
…, cleanup UAF - 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 <noreply@anthropic.com>
…d 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 <noreply@anthropic.com>
…uJoCo 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 <noreply@anthropic.com>
…etBool 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 <noreply@anthropic.com>
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 <noreply@anthropic.com>
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
[/]while the gantry is disabled previously silently modifiedrope_length_and logged a misleading length-change message with no indication the gantry was inactiveWARNis emitted instead:VirtualGantryPlugin: gantry disabled — rope length unchangedon_key()so they don't fall through to MuJoCo's camera handlerFollow-up to #3.
Test plan
[/]→ rope shortens/lengthens as expectedGto disable gantry; press[/]→ WARN logged, rope length unchanged, MuJoCo camera does not switchGto re-enable; press[/]→ rope adjustment resumes normally🤖 Generated with Claude Code