Skip to content

Planning examples

Motion planning examples from simple freespace to complex industrial tasks.

The OMPL examples (freespace, hybrid OMPL + TrajOpt, objectives from Python) have their own page: OMPL Examples.

All examples install as console scripts. Run them with:

Bash
# Installed console script (preferred)
tesseract_basic_cartesian_example

# Or via Python module invocation
pixi run python -m tesseract_robotics.examples.basic_cartesian_example

Basic Cartesian

basic_cartesian_example, tesseract_robotics 0.35.0.9.dev554

Cartesian straight-line motion with TrajOpt, past an obstacle built from point-cloud data: the C++ example's octomap, a 1 m cube of points voxelised into 1000 boxes of 0.1 m — the collision load a scanned part presents. Replace the generated grid with a scanner's points and the rest of the example is unchanged:

Bash
tesseract_basic_cartesian_example

See the source at basic_cartesian_example.py.

Glass upright (orientation constraint)

glass_upright_example, tesseract_robotics 0.35.0.9.dev554

Keep end-effector orientation constrained (e.g. carrying a glass of water) — demonstrates 6-DOF Cartesian constraints inside a TrajOpt program:

Bash
tesseract_glass_upright_example

See the source at glass_upright_example.py.

Pick and place

pick_and_place_example, tesseract_robotics 0.35.0.9.dev554

Complete pick and place workflow with object attachment. A KUKA IIWA picks a box from a table, the box is reparented to iiwa_tool0 via move_link(), then it is placed on a shelf.

pick_and_place_example.py
    # ==================== PICK PHASE ====================
    # Motion: start -> approach (15cm above) -> grasp (on box)
    print("\n=== PICK ===")

    # Pick pose calculation:
    # - Z = table_height + box_size + offset
    # - Rotation: Ry(180) = gripper pointing down
    # - Rotation matrix: [[-1,0,0], [0,1,0], [0,0,-1]]
    pick_z = BOX_SIZE + TABLE_HEIGHT_M + OFFSET
    pick_rotation = np.array(PICK_ROTATION)
    pick_pose = Pose.from_matrix_position(pick_rotation, [box_pos[0], box_pos[1], pick_z])

    # Approach pose: APPROACH_HEIGHT_M directly above pick pose (same orientation)
    approach_pose = Pose.from_matrix_position(
        pick_rotation, [box_pos[0], box_pos[1], pick_z + APPROACH_HEIGHT_M]
    )

    # Build PICK program:
    # 1. Start at home joint configuration
    # 2. FREESPACE to approach pose (any collision-free path)
    # 3. LINEAR descent to grasp pose (Cartesian precision for gripper alignment)
    pick_program = (
        MotionProgram("manipulator", tcp_frame=LINK_TCP, working_frame=LINK_BASE)
        .set_joint_names(joint_names)
        .move_to(StateTarget(start_pos, names=joint_names, profile="FREESPACE"))
        .move_to(CartesianTarget(approach_pose, profile="FREESPACE"))
        .linear_to(CartesianTarget(pick_pose, profile="CARTESIAN"))
    )

    pick_result = composer.plan(robot, pick_program, pipeline=pipeline, profiles=profiles)
    require_success(pick_result, "PICK")
    print(f"PICK OK: {len(pick_result)} waypoints")

    # ==================== ATTACH BOX TO END EFFECTOR ====================
    # Reparent box link from workcell_base to iiwa_tool0
    print("\n=== ATTACH ===")

    # Update robot to final pick configuration
    pick_final = pick_result.trajectory[-1].positions
    robot.set_joints(pick_final, joint_names=joint_names)

    # Before the attach: the box still rests on the table here, for showing the pick.
    pick_env = robot.env.clone()

    # Create FIXED joint to attach box to tool flange
    # Origin offset: box center is box_size/2 below tool (in tool Z direction)
    attach_joint = create_fixed_joint(
        "joint_box2", LINK_TCP, LINK_BOX, origin=Pose.from_xyz(0, 0, BOX_SIZE / 2)
    )
    # move_link() reparents the box - removes old joint, adds new one
    robot.move_link(attach_joint)

    # Update Allowed Collision Matrix (ACM)
    # Without this, planner reports self-collision with attached object
    # "Never" = permanently ignore collisions (vs "Adjacent" for kinematic neighbors)
    robot.add_allowed_collision(LINK_BOX, LINK_TCP, "Never")
    robot.add_allowed_collision(LINK_BOX, "iiwa_link_7", "Never")  # Wrist
    robot.add_allowed_collision(LINK_BOX, "iiwa_link_6", "Never")  # Forearm
    print("Box attached with collision exceptions")

    # ==================== PLACE PHASE ====================
    # Motion: grasp -> retreat -> transit -> place
    print("\n=== PLACE ===")

    # Place pose: middle_left_shelf from C++ example
    # C++ Eigen::Quaterniond(0, 0, 0.7071068, 0.7071068) is scalar-first (w=0, x=0, y=0.7071,
    # z=0.7071): with w = 0 it is a 180 deg rotation about the axis (0, 1, 1)/sqrt(2), whose
    # matrix is 2 n n^T - I = [[-1,0,0], [0,0,1], [0,1,0]]
    place_rotation = np.array(SHELF_ROTATION)
    place_pos = list(SHELF_POSITION_M)
    place_pose = Pose.from_matrix_position(place_rotation, place_pos)

    # Approach: 25cm back from shelf in -Y direction (approach from front)
    place_approach_pos = [place_pos[0], place_pos[1] - PLACE_APPROACH_BACKOFF_M, place_pos[2]]
    place_approach_pose = Pose.from_matrix_position(place_rotation, place_approach_pos)

    # Build PLACE program:
    # 1. Start from final pick configuration (box now attached)
    # 2. LINEAR retreat to approach pose (controlled extraction)
    # 3. FREESPACE transit to shelf approach (collision-aware path)
    # 4. LINEAR approach to shelf (precise placement)
    place_program = (
        MotionProgram("manipulator", tcp_frame=LINK_TCP, working_frame=LINK_BASE)
        .set_joint_names(joint_names)
        .move_to(StateTarget(pick_final, names=joint_names))
        .linear_to(CartesianTarget(approach_pose, profile="CARTESIAN"))  # Retreat
        .move_to(CartesianTarget(place_approach_pose, profile="FREESPACE"))  # Transit
        .linear_to(CartesianTarget(place_pose, profile="CARTESIAN"))  # Place
    )

    place_result = composer.plan(robot, place_program, pipeline=pipeline, profiles=profiles)
    require_success(place_result, "PLACE")
    print(f"PLACE OK: {len(place_result)} waypoints")

Run it:

Bash
tesseract_pick_and_place_example

Full source: pick_and_place_example.py.

Raster pattern

raster_example, tesseract_robotics 0.35.0.9.dev554

Industrial raster pattern for welding / painting / milling. Multiple linear passes with freespace transitions between segments. The example builds the program programmatically from a list of Cartesian waypoints, then hands it to the TrajOptPipeline.

Building the program:

raster_example.py (build_program)
    # === BUILD MOTION PROGRAM ===
    # Raster structure (from C++ example):
    # - from_start: home -> first waypoint (freespace)
    # - raster_segment: linear moves through waypoints (3 times)
    # - transitions: freespace moves between segments
    # - to_end: final waypoint -> home (freespace)
    num_segments = SEGMENT_COUNT

    program = MotionProgram(
        "manipulator", tcp_frame="tool0", working_frame="base_link", profile="RASTER"
    ).set_joint_names(joint_names)

    # Start from home position
    program.move_to(StateTarget(home_pos, names=joint_names, profile="FREESPACE"))

    for seg in range(num_segments):
        # Approach: freespace move to first raster waypoint
        program.move_to(CartesianTarget(waypoints[0], profile="FREESPACE"))

        # Raster segment: linear moves through all waypoints (process path)
        for wp in waypoints[1:]:
            program.linear_to(CartesianTarget(wp, profile="RASTER"))

        # Transition: freespace move back to start of next segment
        if seg < num_segments - 1:
            program.move_to(CartesianTarget(waypoints[0], profile="FREESPACE"))

    # Return to home position
    program.move_to(StateTarget(home_pos, names=joint_names, profile="FREESPACE"))

    print(f"Created program with {len(program)} waypoints ({num_segments} segments)")

Planning it:

raster_example.py (plan)
    # === PLAN WITH TRAJOPT ===
    # TrajOpt optimizes the trajectory for smooth motion and collision avoidance
    print(f"\nRunning planner ({pipeline})...")
    if composer is None:
        composer = TaskComposer.from_config()
    profiles = create_trajopt_default_profiles()
    result = composer.plan(robot, program, pipeline=pipeline, profiles=profiles)

    require_success(result, "raster")
    print(f"Planning successful! {len(result)} waypoints")

Run it:

Bash
tesseract_raster_example

Full source: raster_example.py.

Multi-step with task composer (car seat)

car_seat_example, tesseract_robotics 0.35.0.9.dev534

Complex multi-phase sequence: an 8-DOF system (linear carriage + 7-axis arm) picks a car seat, attaches it via scene graph manipulation, then places it in a vehicle body. The snippet below is the core sequential-planning loop.

car_seat_example.py (multi_step)
    # === PHASE 2: PICK MOTION ===
    # Plan freespace motion from Home to Pick1 (above seat_1)
    pick_result = plan_motion(
        robot, composer, joint_names, home_pos, pick_pos, "PICK", pipeline, profiles
    )

    # === PHASE 3: ATTACH SEAT ===
    # Update environment state to Pick1, then reparent seat_1 to end_effector
    print("\n=== ATTACH SEAT ===")
    robot.set_joints(POSITIONS["Pick1"])
    # Before the attach: the seat still rests on its conveyor here, for showing the pick.
    pick_env = robot.env.clone()
    attach_seat(robot, "seat_1")

    # === PHASE 4: PLACE MOTION ===
    # Plan transport motion Pick1 -> Place1 with attached seat
    # TrajOpt now includes seat_1 geometry in collision checking
    place_result = plan_motion(
        robot, composer, joint_names, pick_pos, place_pos, "PLACE", pipeline, profiles
    )

Run it:

Bash
tesseract_car_seat_example

Full source: car_seat_example.py.

Descartes + TrajOpt chaining

chain_example, tesseract_robotics 0.35.0.9.dev554

The CartesianPipeline runs Descartes (sampling-based, ladder-graph IK selection) followed by TrajOpt (smoothing). The snippet below shows the program + pipeline assembly:

chain_example.py (descartes_then_trajopt)
    # Build motion program with FREESPACE and CARTESIAN segments
    # FREESPACE profile: uses OMPL sampling-based planner (handles obstacles)
    # CARTESIAN profile: uses Descartes ladder graph (optimal IK selection)
    program = (
        MotionProgram("manipulator", tcp_frame="tool0", working_frame="base_link")
        .set_joint_names(joint_names)
        # Segment 1: FREESPACE motion from home to first raster point
        .move_to(StateTarget(joint_start, names=joint_names, profile="FREESPACE"))
        .move_to(CartesianTarget(wp1, profile="FREESPACE"))
        # Segment 2: LINEAR raster pattern (Descartes optimizes IK redundancy)
        .move_to(CartesianTarget(wp2, profile="CARTESIAN"))  # Pass 1
        .move_to(CartesianTarget(wp3, profile="CARTESIAN"))  # Transition
        .move_to(CartesianTarget(wp4, profile="CARTESIAN"))  # Pass 2
        .move_to(CartesianTarget(wp5, profile="CARTESIAN"))  # Transition
        .move_to(CartesianTarget(wp6, profile="CARTESIAN"))  # Pass 3
        # Segment 3: FREESPACE return to home
        .move_to(StateTarget(joint_start, names=joint_names, profile="FREESPACE"))
    )

    # CartesianPipeline profiles configure:
    # - Descartes: IK sampling density, edge cost function
    # - TrajOpt: smoothing coefficients (coeff=1 for velocity), collision margin (0.025m)
    profiles = create_cartesian_pipeline_profiles()

    # TaskComposer loads pipeline from YAML config and manages execution
    if composer is None:
        composer = TaskComposer.from_config()

    # Execute planning - CartesianPipeline runs Descartes then TrajOpt
    print(f"Planning with {pipeline}...")
    start_time = time.time()

    result = composer.plan(robot, program, pipeline=pipeline, profiles=profiles)
    planning_time = time.time() - start_time

Run it:

Bash
tesseract_chain_example

Full source: chain_example.py.

9-DOF with external axis (puzzle piece)

puzzle_piece_auxillary_axes_example, tesseract_robotics 0.35.0.9.dev554

A Cartesian toolpath along a part's edge, planned for nine joints: a 7-DOF KUKA IIWA and a 2-DOF positioner (turntable and tilt) that holds the part. The toolpath is given in the part's frame, which moves with the positioner, and TrajOpt distributes the motion over all nine joints. The plan takes 33 to 34 s on an Apple M1 Max (10 cores).

Bash
tesseract_puzzle_piece_auxillary_axes_example

See the source at puzzle_piece_auxillary_axes_example.py.

Visualization

abb_irb2400_viewer, tesseract_robotics 0.35.0.9.dev554

All examples can be visualized:

Python
from tesseract_robotics.viewer import TesseractViewer

with TesseractViewer() as viewer:
    viewer.update_environment(robot.env)
    if result.successful:
        viewer.update_trajectory(result.raw_results)
    print(f"Viewer at {viewer.url}")
    input("Press Enter to exit...")

Next steps