Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions mujoco_ros2_control_plugins/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
11 changes: 9 additions & 2 deletions mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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 ------------------------------
Expand Down
Loading