MoveIt Custom Tool with Needle Tip

This example extends the UR MoveIt workflow from MoveIt Obstacle from a Slicer Model by attaching a runtime tool to the robot without editing the URDF or SRDF. The tool is a 100 mm needle represented as a MoveIt AttachedCollisionObject. It is attached to the UR tool0 link and defines a MoveIt object subframe named tip at the distal end of the needle.

The complete script also creates a simple world obstacle using the same pattern as the previous example, then attaches the needle and prints the custom tip frame name SlicerNeedle/tip. The script refreshes the existing End effector link selector in Motion Control and selects that tip.

Prerequisites

Start the UR mock-hardware MoveIt launch:

ros2 launch slicer_ros2_module ur_sim_control.launch.py launch_rviz:=true

Then run the example script:

ros2 run slicer_ros2_module slicer --python-script $(ros2 pkg prefix slicer_ros2_module)/share/slicer_ros2_module/docs/code/moveit_custom_tool.py

Needle Tool

The obstacle setup is intentionally not repeated here; see MoveIt Obstacle from a Slicer Model for that pattern. The new part is the runtime tool attachment:

# ---------------------------------------------------------------------------
# Add a runtime 100 mm needle tool attached to the UR tool frame.
# ---------------------------------------------------------------------------

# The cylinder mesh is authored in the TOOL_LINK frame.  Its base is at the
# tool origin and its tip is 100 mm along +Z.  MoveIt receives this mesh as an
# AttachedCollisionObject, so the needle participates in robot collision checks.
cylinder = vtk.vtkCylinderSource()
cylinder.SetRadius(NEEDLE_RADIUS_MM)
cylinder.SetHeight(NEEDLE_LENGTH_MM)
cylinder.SetCenter(0.0, NEEDLE_LENGTH_MM / 2.0, 0.0)
cylinder.SetResolution(24)
cylinder.CappingOn()
cylinder.Update()

alignCylinderToZ = vtk.vtkTransform()
alignCylinderToZ.RotateX(90.0)

needleFilter = vtk.vtkTransformPolyDataFilter()
needleFilter.SetInputConnection(cylinder.GetOutputPort())
needleFilter.SetTransform(alignCylinderToZ)
needleFilter.Update()

needleNode = slicer.mrmlScene.AddNewNodeByClass("vtkMRMLModelNode", NEEDLE_OBJECT_NAME)
needleNode.SetAndObservePolyData(needleFilter.GetOutput())
needleNode.CreateDefaultDisplayNodes()
needleNode.GetDisplayNode().SetColor(0.1, 0.7, 1.0)
needleNode.GetDisplayNode().SetOpacity(0.85)

# Define a MoveIt object subframe named "tip" at the distal needle point.
needleTipPoseInTool = motionLogic.CreateToolTipPose(0.0, 0.0, NEEDLE_LENGTH_MM)
needleTipLink = motionLogic.AddMoveItAttachedTool(
    modelNode=needleNode,
    linkName=TOOL_LINK,
    touchLinks=[TOOL_LINK, "flange", "wrist_3_link"],
    tipSubframeName="tip",
    tipPose=needleTipPoseInTool,
    robotNode=robotNode,
)
if not needleTipLink:
    raise RuntimeError("Failed to attach needle tool to MoveIt.")

print(f"Attached needle tool.  MoveIt tip frame: {needleTipLink}")

After this block runs, MoveIt collision checking treats the needle as part of the robot. Planning requests that support attached-object subframes can use needleTipLink as the active Cartesian link name. For example, the scripted Cartesian helper accepts it through the linkName argument when you want waypoints to refer to the needle tip instead of the robot tool0 frame.

The Slicer model is also parented under the robot’s tool0 transform, so the needle appears at the UR tool in the 3D view.

Notes

The needle is not a URDF link. It is a planning-scene object attached at runtime. This is useful for swapping instruments without relaunching MoveIt, while still allowing the URDF/SRDF path for tools that should be permanent robot model links.