Planning Examples

Motion planning examples from simple freespace to complex industrial tasks.

All examples install as console scripts. Run them with:

Bash

# Installed console script (preferred)
tesseract_freespace_ompl_example

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

Freespace OMPL

Basic OMPL planning between joint configurations using the ABB IRB2400:

Bash

tesseract_freespace_ompl_example

See the source at freespace_ompl_example.py.

Basic Cartesian

Cartesian straight-line motion with TrajOpt:

Bash

tesseract_basic_cartesian_example

See the source at basic_cartesian_example.py.

Glass Upright (Orientation Constraint)

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

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 (0.772m) + box_size + offset
    # - Rotation: Ry(180) = gripper pointing down
    # - Rotation matrix: [[-1,0,0], [0,1,0], [0,0,-1]]
    pick_z = BOX_SIZE + 0.772 + OFFSET
    pick_rotation = np.array([[-1, 0, 0], [0, 1, 0], [0, 0, -1]])
    pick_pose = Pose.from_matrix_position(pick_rotation, [box_pos[0], box_pos[1], pick_z])

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

    # 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)
    assert pick_result.successful, f"PICK failed: {pick_result.message}"
    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)

    # 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
    # Quaternion (w=0, x=0, y=0.7071, z=0.7071) = 90 deg around Z
    # Rotation matrix: [[-1,0,0], [0,0,1], [0,1,0]]
    place_rotation = np.array([[-1.0, 0.0, 0.0], [0.0, 0.0, 1.0], [0.0, 1.0, 0.0]])
    place_pos = [-0.148856, 0.73085, 1.16]  # middle_left_shelf coordinates
    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] - 0.25, 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)
    assert place_result.successful, f"PLACE failed: {place_result.message}"
    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

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 = 3

    program = MotionProgram("manipulator", tcp_frame="tool0", 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})...")
    composer = TaskComposer.from_config()
    profiles = create_trajopt_default_profiles()
    result = composer.plan(robot, program, pipeline=pipeline, profiles=profiles)

    assert result.successful, f"Planning failed: {result.message}"
    print(f"Planning successful! {len(result)} waypoints")

Run it:

Bash

tesseract_raster_example

Full source: raster_example.py.

Hybrid Planning (OMPL + TrajOpt)

OMPL finds a feasible path, TrajOpt smooths and optimizes it:

Bash

tesseract_freespace_hybrid_example

See the source at freespace_hybrid_example.py.

Multi-Step with Task Composer (Car Seat)

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"])
    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

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")
        .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
    composer = TaskComposer.from_config()

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

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

        if result.successful:
            print(f"Success in {planning_time:.2f}s, {len(result)} waypoints")
        else:
            print(f"Failed in {planning_time:.2f}s: {result.message}")

    except Exception as e:
        planning_time = time.time() - start_time
        print(f"Exception after {planning_time:.2f}s: {e}")
        result = None

Run it:

Bash

tesseract_chain_example

Full source: chain_example.py.

9-DOF with External Axis (Puzzle Piece)

Cartesian toolpath planning with a 9-DOF system (robot + positioner + rail):

Bash

tesseract_puzzle_piece_auxillary_axes_example

See the source at puzzle_piece_auxillary_axes_example.py.

Visualization

All examples can be visualized:

Python

from tesseract_robotics.viewer import TesseractViewer

viewer = TesseractViewer()
viewer.update_environment(robot.env, [0, 0, 0])

# Show trajectory
if result.successful:
    viewer.update_trajectory(result.raw_results)

# Start server
viewer.start_serve_background()
print("Open http://localhost:8000 in browser")
input("Press Enter to exit...")

Next Steps

• Online Planning - Real-time replanning examples
• Low-Level SQP Guide - Direct solver access