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:
# 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:
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:
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 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:
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:
# === 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:
# === 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:
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.
# === 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:
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:
# 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:
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).
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:
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¶
- OMPL Examples - Sampling-based planning and optimization objectives
- Online Planning - Replanning examples
- Low-Level SQP Guide - Direct solver access