service-piper-moveit-rbnx

Robonix package for arm motion planning + execution on the Piper + Orbbec Dabai DCW grasp pipeline. Wraps the upstream MoveIt config (piper_with_gripper_moveit) plus the C++ moveit_control_node_yolo grasp executor. Owns the service/manipulation/* namespace.

Catalog name: robonix.service.piper_moveit.

Capability surface

Extra contract TOMLs shipped under capabilities/service/manipulation/ (demo, demo_place) are handler helpers used during bring-up; they are not exposed in the deploy manifest.

Design intent (most-conservative path)

The C++ moveit_control_node_yolo is battle-tested upstream. The MoveIt MoveGroupInterface Python API is less stable than its C++ counterpart. Re-implementing the planning + execution logic in Python carries real risk for zero gain. So we vendor the C++ source verbatim and put a typed front door on it via ROS topic + status poll:

Signalling flow

                 Pilot LLM (or skill-pick-rbnx)
                          │
                          ▼
          MCP   manipulation/execute_grasp  (PoseStamped + width)
                          │
                          ▼
              ┌─ publishes /graspnet/grasps  (graspnet_msgs/GraspPose)
              │
              ▼
       C++ moveit_control_node_yolo  (vendored)
              │
              ├─ tf2 transform → arm/base_link
              ├─ MoveGroupInterface plan
              └─ execute via FollowJointTrajectory action
                          │
                          ▼
              primitive-agilex-piper-arm-rbnx executes on real hardware
                          │
                          ▼
              piper_ctl publishes /arm/arm_status (PiperStatusMsg)
                          │
                          ▼
   ◄─── execute_grasp polls busy → idle, returns success/elapsed

Launch fork (launch/piper_moveit_rbnx.launch.py)

Four deltas from upstream piper_moveit.launch.py:

1. Removed fake ros2_control_node — upstream's launch spawns a simulated joint driver publishing /arm/joint_states. With primitive-agilex-piper-arm-rbnx already publishing real hardware joint states, running both = two publishers fighting on the topic = MoveIt plans against garbage.
2. Removed RViz — robonix-managed deploy is headless. Bring up RViz manually in another shell if needed:

   ros2 launch piper_with_gripper_moveit moveit_rviz.launch.py
   

3. move_group joint_states remap target switched from /arm/joint_states (which had no real publisher after we removed the fake controller) to /arm/joint_states_single (what primitive-agilex-piper-arm-rbnx actually publishes).
4. Added link6 → arm/link6 static TF bridge to glue the unprefixed TF tree (primitive-agilex-piper-description-rbnx publishes; primitive-agilex-piper-handeye-rbnx publishes link6 → camera_color_optical_frame into) onto the prefixed one (this launch publishes via frame_prefix="arm/" for MoveIt's planning frame). Without the bridge, the C++ node's tf_buffer_->transform(grasp_pose_in_camera_frame, "arm/base_link") call fails because the camera frame and arm/base_link are in different subtrees.

The rest (rsp.launch.py, static_virtual_joint_tfs.launch.py) is included verbatim via IncludeLaunchDescription, so future upstream tweaks don't need a re-fork.

Why no spawn_controllers.launch.py?

Upstream uses spawn_controllers to start joint_trajectory_controller + gripper_controller ros2_control instances. Those drive the fake controller_manager we removed. The C++ node does not talk to controller_manager — it submits FollowJointTrajectory action goals directly to whoever advertises the action server. primitive-agilex-piper-arm-rbnx must advertise that action server for the real-hardware path to work.

Frame tree (post-boot)

Two parallel subtrees, joined at link6 ↔ arm/link6:

unprefixed (piper_description + easy_handeye2):    prefixed (this package):
    base_link                                          arm/world  ──┐
       └── link1 ── ... ── link6                       arm/base_link │ static
                              ├── camera_*             arm/link1     │ joints
                              │                        ...           │ from URDF
                              └── arm/link6 ◄═════════ arm/link6 ────┘
                                  (identity static TF — the bridge)

Both subtrees describe the same physical robot, just under different frame-name conventions. The bridge means tf_buffer_->transform(camera_color_optical_frame_pose, arm/base_link) walks: camera_* → link6 → arm/link6 → arm/base_link, and works.

Boot ordering

• Requires primitive-agilex-piper-arm-rbnx (publishes /arm/arm_status; the sentinel waits for the first sample);
• Requires primitive-agilex-piper-handeye-rbnx (static link6 → camera_color_optical_frame TF used inside execute_grasp);
• Requires primitive-agilex-piper-description-rbnx (provides the unprefixed TF subtree);
• Independent of service-object-detect-rbnx / service-grasp-pose-rbnx at boot time (no perception dep during Init).

Driver-init lifecycle

on_init ── parse cfg
       ── log atlas deps (informational)
       ── spawn rclpy bridge thread
            (publisher /graspnet/grasps,
             subscriber /arm/arm_status)
       ── spawn `ros2 launch piper_moveit_rbnx.launch.py`
            (RSP + move_group + cpp executor)
       ── sentinel-wait for first /arm/arm_status sample
            (proves piper_ctl_rbnx is alive)

on_deactivate ── kill launch group + stop rclpy thread

on_init is heavy — sentinel timeout default 60 s because launch takes ~5–10 s to bring up move_group, and we need real piper joint feedback to flow before we declare ourselves ACTIVE.

execute_grasp semantics — two-phase wait

1. busy phase (≤ ⅓ × timeout_s): wait for /arm/arm_status to transition to non-zero (i.e. arm started moving). If never: either the C++ node did not pick up our publish, or the goal was already physically satisfied → fail conservatively.
2. idle phase (remaining budget): wait for return to 0 (idle).

success=True iff both phases complete in budget.

Layout

service-piper-moveit-rbnx/
├── package_manifest.yaml
├── capabilities/
│   ├── service/manipulation/{driver,execute_grasp,reset,demo,demo_place}.v1.toml
│   └── lib/manipulation/srv/ExecuteGrasp.srv
├── launch/
│   └── piper_moveit_rbnx.launch.py   # fork of upstream launch
├── piper_moveit/
│   ├── __init__.py
│   └── main.py                       # robonix Service + rclpy bridge
├── scripts/
│   ├── build.sh                      # colcon + rbnx codegen --mcp
│   └── start.sh
└── src/                              # vendored ROS packages
    ├── piper_moveit/                 # piper_with_gripper_moveit + piper_no_gripper_moveit
    ├── piper_moveit_control/         # C++ moveit_control_node_yolo
    ├── piper_humble/                 # empty shell (referenced by cmakelists)
    ├── piper_msgs/                   # PiperStatusMsg etc.
    └── graspnet_msgs/                # GraspPose etc. (full version)

Config (passed via Driver(CMD_INIT, config_json))

arm_group_name:       arm                              # MoveIt SRDF group
end_effector_link:    arm/link6                        # in prefixed tree
gripper_action_name:  /gripper_controller/follow_joint_trajectory
sentinel_timeout_s:   60.0                             # wait for /arm/arm_status

All four are optional (defaults match upstream). end_effector_link intentionally uses the prefixed arm/link6 — MoveIt's planning frame is arm/world, so the EE link lives in the prefixed subtree.

Build / run standalone

bash scripts/build.sh                                 # colcon (~30–60 s)
ROBONIX_ATLAS=127.0.0.1:50051 \
    bash scripts/start.sh

Verification

# 1. atlas-side
rbnx caps | grep manipulation
# expect: piper_moveit ACTIVE, contracts:
#   robonix/service/manipulation/{driver, execute_grasp, reset}

# 2. ROS-side (the cpp bridge is up)
ros2 topic info /graspnet/grasps                 # expect 1+ subscribers (cpp node)
ros2 node list | grep moveit_control             # the cpp node

# 3. TF bridge
ros2 run tf2_ros tf2_echo arm/base_link camera_color_optical_frame
# Expect a transform composed of:
#   arm/base_link ← arm/joint{1..6} ← arm/link6  (RSP, prefixed)
#   arm/link6 ←IDENTITY← link6                   (the bridge)
#   link6 ← <calib> ← camera_color_optical_frame (easy_handeye2)

# 4. Direct execute_grasp test (CAREFUL — this moves the real arm).
#    Park the arm in safe position first.
rbnx ask "move the arm 30cm up, no rotation"

Failure modes

License

This package: MulanPSL-2.0. Vendored MoveIt config + grasp executor: see their respective LICENSE files.