MoveIt Cartesian Path from Pose Markers

This example shows a Python-only Motion Control workflow:

  • create Slicer 3D pose markers using vtkMRMLLinearTransformNode;

  • collect those markers in a Python list;

  • ask MoveIt’s /compute_cartesian_path service to compute a Cartesian trajectory through each pose;

  • load the returned trajectory into the Motion Control GUI for preview, scrubbing, and execution.

Transform nodes are used as the primary marker type because they carry a full 6-DOF pose. Slicer Markups control points can also be converted to poses with GetPoseMatricesFromMarkups, but those points only provide position unless you supply an orientation reference.

Prerequisites

Start a robot with MoveIt, for example:

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_pose_markers.py

Script Setup

The complete script starts by importing Slicer/VTK modules, defining the robot and MoveIt configuration, creating the robot node, and initializing the Motion Control logic. If you are copying sections into Slicer’s Python console, run this setup block first:

import slicer
import vtk
import time

# ---------------------------------------------------------------------------
# Configuration
# ---------------------------------------------------------------------------
ROBOT_NAME = "ur5"
URDF_NODE_NAME = "robot_state_publisher"
URDF_PARAM_NAME = "robot_description"
FIXED_FRAME = "world"
TF_PREFIX = ""
JOINT_STATES_TOPIC = "/joint_states"
PLANNING_GROUP = "ur_manipulator"

# Lower the marker path from the bent demo seed pose.
MARKER_Z_OFFSET_MM = -200.0
PATH_X_MM = 80.0
PATH_Y_MM = 80.0
PLANNING_TIME_SEC = 15.0

# ---------------------------------------------------------------------------
# Robot and MotionControl setup
# ---------------------------------------------------------------------------
rosLogic = slicer.util.getModuleLogic("ROS2")
rosNode = rosLogic.GetDefaultROS2Node()

robotNode = rosNode.CreateAndAddRobotNode(
    ROBOT_NAME, URDF_NODE_NAME, URDF_PARAM_NAME, FIXED_FRAME, TF_PREFIX
)
if robotNode is None:
    raise RuntimeError(f"Robot node '{ROBOT_NAME}' was not created.")

rootTipLinks = None
for _ in range(50):
    slicer.app.processEvents()
    rootTipLinks = robotNode.FindRootAndTipLinks()
    if rootTipLinks and len(rootTipLinks) >= 2:
        break
    time.sleep(0.2)

if not rootTipLinks or len(rootTipLinks) < 2:
    raise RuntimeError(
        "Could not resolve robot root/tip links within timeout. "
        "Verify robot_state_publisher and TF are running."
    )

motionLogic = slicer.util.getModuleLogic("ROS2MotionControl")
paramNode = motionLogic.getParameterNode()
paramNode.robotNodeID = robotNode.GetID()
paramNode.jointStateTopic = JOINT_STATES_TOPIC
paramNode.planningGroup = PLANNING_GROUP
paramNode.moveGroupExists = True

if not motionLogic.SetupRobotForMotionControl(paramNode):
    raise RuntimeError("Failed to set up robot for motion control.")

motionControlNode = slicer.mrmlScene.GetNodeByID(paramNode.motionControlNodeID)
if motionControlNode is None:
    raise RuntimeError("Motion control node was not created.")

rootLink, tipLink = rootTipLinks[:2]
rootTransform = motionLogic.findRobotTransforms(rootLink, goal=True)


# ---------------------------------------------------------------------------

Planning Through Markers

After setup, the script builds the pose marker list and plans through it:

# Build a Python list of built-in 3D pose markers
# ---------------------------------------------------------------------------
def wait_for_joint_state(joint_names, timeout_sec=5.0):
    deadline = time.time() + float(timeout_sec)
    while time.time() < deadline:
        slicer.app.processEvents()
        rosLogic.Spin()
        joint_values = motionLogic.GetCurrentJointState(joint_names)
        if joint_values and len(joint_values) == len(joint_names):
            return list(joint_values)
        time.sleep(0.05)
    return []


def create_marker_visuals(markers, radius_mm=6.0):
    if not markers:
        return None

    sphere = vtk.vtkSphereSource()
    sphere.SetRadius(float(radius_mm))
    sphere.SetThetaResolution(20)
    sphere.SetPhiResolution(20)
    sphere.Update()

    visualNodes = []
    for i, marker in enumerate(markers):
        visualNode = slicer.mrmlScene.AddNewNodeByClass("vtkMRMLModelNode", f"{marker.GetName()}_Visual")
        visualNode.SetAndObservePolyData(sphere.GetOutput())

        displayNode = slicer.mrmlScene.AddNewNodeByClass("vtkMRMLModelDisplayNode", f"{marker.GetName()}_Visual_Display")
        displayNode.SetColor(0.0, 0.8, 0.3)
        displayNode.SetOpacity(0.9)
        displayNode.SetVisibility3D(True)
        displayNode.SetVisibility2D(True)
        visualNode.SetAndObserveDisplayNodeID(displayNode.GetID())

        # Parent visual geometry to the marker transform so it follows marker edits.
        visualNode.SetAndObserveTransformNodeID(marker.GetID())
        visualNodes.append(visualNode)

    return visualNodes


jointNames = list(robotNode.GetJoints())
liveJointValues = wait_for_joint_state(jointNames)
if not liveJointValues:
    raise RuntimeError(
        "Did not receive a valid /joint_states message within timeout. "
        "Verify the robot driver or state publisher is running, then retry."
    )

startJointValues = []
startJointNamesForPlanning = []
startStateSource = "MoveIt current state (from live /joint_states)"
markerAnchorJointValues = list(liveJointValues)
markerAnchorSource = "current tip pose"

anchorPoseRoot = vtk.vtkMatrix4x4()
if robotNode.ComputeKDLFK(markerAnchorJointValues, anchorPoseRoot, tipLink) is None:
    raise RuntimeError("Failed to compute FK for marker anchor joint state.")

rootWorld = vtk.vtkMatrix4x4()
rootTransform.GetMatrixTransformToWorld(rootWorld)

anchorPose = vtk.vtkMatrix4x4()
vtk.vtkMatrix4x4.Multiply4x4(rootWorld, anchorPoseRoot, anchorPose)


def pose_with_offset(sourceMatrix, dxMm, dyMm, dzMm):
    matrix = vtk.vtkMatrix4x4()
    matrix.DeepCopy(sourceMatrix)
    matrix.SetElement(0, 3, matrix.GetElement(0, 3) + dxMm)
    matrix.SetElement(1, 3, matrix.GetElement(1, 3) + dyMm)
    matrix.SetElement(2, 3, matrix.GetElement(2, 3) + dzMm)
    return matrix


loweredAnchorPose = pose_with_offset(anchorPose, 0.0, 0.0, MARKER_Z_OFFSET_MM)
poseMarkers = [
    motionLogic.CreatePoseMarker("PathPose_0", anchorPose),
    motionLogic.CreatePoseMarker("PathPose_1", loweredAnchorPose),
    motionLogic.CreatePoseMarker("PathPose_2", pose_with_offset(loweredAnchorPose, PATH_X_MM, 0.0, 0.0)),
    motionLogic.CreatePoseMarker("PathPose_3", pose_with_offset(loweredAnchorPose, PATH_X_MM, PATH_Y_MM, 0.0)),
    motionLogic.CreatePoseMarker("PathPose_4", pose_with_offset(loweredAnchorPose, 0.0, PATH_Y_MM, 0.0)),
]

markerVisualNodes = create_marker_visuals(poseMarkers)

print("Created pose markers:")
for marker in poseMarkers:
    print("  ", marker.GetName())
if markerVisualNodes:
    print(f"Created {len(markerVisualNodes)} marker visuals (spheres attached to transforms).")
print(f"Start state source: {startStateSource}")
print(f"Path starts at {markerAnchorSource}, then lowers before XY motion.")
print(f"PathPose_1 is {abs(MARKER_Z_OFFSET_MM):.0f} mm below PathPose_0.")
print(
    "Pose markers are visible; 3D editor widgets are hidden. "
    "Set marker.GetDisplayNode().SetEditorVisibility(True) to edit one interactively."
)


# ---------------------------------------------------------------------------
# Plan a Cartesian trajectory through all marker poses
# ---------------------------------------------------------------------------
trajectory = motionLogic.PlanMoveItCartesianTrajectoryFromPoseMarkers(
    motionControlNode=motionControlNode,
    groupName=PLANNING_GROUP,
    poseMarkers=poseMarkers,
    relativeToNode=rootTransform,
    robotNode=robotNode,
    eefStepMeters=0.01,
    jumpThreshold=0.0,
    avoidCollisions=True,
    velocityScaling=0.5,
    accelerationScaling=0.5,
    planningTimeSec=PLANNING_TIME_SEC,
    startJointNames=startJointNamesForPlanning,
    startJointValues=startJointValues,
)
fraction = motionControlNode.GetLastCartesianPathFraction()
if trajectory is None:
    print(f"MoveIt Cartesian path fraction: {fraction:.3f}")
    print("MoveIt did not return a usable Cartesian trajectory.")
    print("Try enabling one marker's editor widget and moving the marker closer to PathPose_0.")
else:
    pointCount = len(trajectory.GetJointTrajectory().GetPoints())
    print(f"MoveIt Cartesian path fraction: {fraction:.3f}")
    print(f"Trajectory points: {pointCount}")
    if fraction < 0.99:
        print("Warning: MoveIt planned only a partial Cartesian path.")

    slicer.util.selectModule("ROS2MotionControl")
    motionWidget = slicer.util.getModuleWidget("ROS2MotionControl")

    if motionWidget.loadPlannedTrajectory(trajectory, enableExecute=True, lockPlanning=True):
        print("Trajectory loaded into the Motion Control GUI.")
        pathNode = motionWidget.addPlannedTrajectoryPolyline()
        if pathNode:
            print(f"End-effector path displayed as: {pathNode.GetName()}")
        print("Plan controls are temporarily locked to avoid overwriting this external trajectory.")
        print("Use Preview, the scrubber, or Execute from the Motion Control module.")

The script first waits for a valid live /joint_states message. The marker list then starts with a lead-in segment for feasibility: PathPose_0 is anchored to the current tip pose, and PathPose_1 is the same pose lowered by 100 mm. The remaining markers build the XY path at that lowered height. Planning uses MoveIt’s current state, without forcing an explicit start state. The script also creates small sphere visuals attached to each marker transform, so marker positions stay easy to see while editor widgets are hidden. To edit one marker interactively, enable its editor widget:

marker = slicer.util.getNode("PathPose_1")
marker.GetDisplayNode().SetEditorVisibility(True)

After moving a marker, rerun the planning block from the Python console. The marker list can also be built entirely by another Python algorithm:

poseMarkers = [
    slicer.util.getNode("PathPose_0"),
    slicer.util.getNode("PathPose_1"),
    slicer.util.getNode("PathPose_2"),
]

trajectory = motionLogic.PlanMoveItCartesianTrajectoryFromPoseMarkers(
    motionControlNode,
    PLANNING_GROUP,
    poseMarkers,
    relativeToNode=rootTransform,
    robotNode=robotNode,
)

fraction = motionControlNode.GetLastCartesianPathFraction()

fraction is the fraction returned by MoveIt’s Cartesian path planner. A value less than 1.0 means MoveIt returned only a partial path.

Execution safety

The example requires live /joint_states. If no valid joint-state message arrives within the timeout, the script raises an error instead of planning from guessed joint values.

When planning succeeds, the script selects the Motion Control module and loads the trajectory into the same widget state used by the Plan button. Use the GUI Preview button or trajectory scrubber to inspect the result before pressing Execute. The Python example intentionally does not execute the trajectory directly.