Motion Planning¶

tesseract_robotics provides multiple motion planners for different use cases.

Planner Comparison¶

Planner	Type	Speed	Quality	Use Case
OMPL	Sampling	Medium	Good	Free-space, complex environments
TrajOpt	Optimization	Fast	Excellent	Cartesian paths, smoothing
TrajOptIfopt	SQP/OSQP	Very Fast	Excellent	Real-time replanning
Descartes	Graph Search	Slow	Optimal	Dense Cartesian toolpaths
Simple	Interpolation	Instant	N/A	Joint-space interpolation

Quick Planning¶

OMPL (Freespace)TrajOpt (Cartesian)Hybrid (OMPL + TrajOpt)

Python

from tesseract_robotics.planning import Robot, Planner
import numpy as np

robot = Robot.from_tesseract_support("abb_irb2400")
planner = Planner(robot)

start = np.zeros(6)
goal = np.array([0.5, -0.5, 0.5, 0.0, 0.5, 0.0])

trajectory = planner.plan(
    start=start,
    goal=goal,
    planner="ompl"
)

Python

from tesseract_robotics.tesseract_common import Isometry3d

target_pose = Isometry3d.Identity()
target_pose.translate([0.8, 0.2, 0.5])

trajectory = planner.plan(
    start=start,
    goal=target_pose,  # Cartesian goal
    planner="trajopt"
)

Python

# OMPL finds path, TrajOpt smooths it
trajectory = planner.plan(
    start=start,
    goal=goal,
    planner="freespace_hybrid"
)

OMPL Planner¶

Sampling-based planner for complex environments:

graph LR
    A[Start] --> B[Sample Random Config]
    B --> C{Collision Free?}
    C -->|Yes| D[Add to Tree]
    C -->|No| B
    D --> E{Goal Reached?}
    E -->|No| B
    E -->|Yes| F[Extract Path]

OMPL Profiles¶

Python

from tesseract_robotics.tesseract_motion_planners_ompl import (
    OMPLDefaultPlanProfile
)

profile = OMPLDefaultPlanProfile()
profile.planning_time = 5.0  # Max planning time (seconds)
profile.simplify = True      # Simplify path after planning
profile.collision_check_config.contact_margin = 0.02  # 2cm margin

Available OMPL Planners¶

Planner	Description
`RRTConnect`	Bidirectional RRT (default, fast)
`RRT`	Single-tree RRT
`RRTstar`	Asymptotically optimal RRT
`PRM`	Probabilistic Roadmap
`LazyPRM`	Lazy collision checking PRM
`BKPIECE`	Bi-directional KPIECE

TrajOpt Planner¶

Optimization-based planner using sequential convex optimization:

graph TD
    A[Initial Trajectory] --> B[Linearize Constraints]
    B --> C[Solve QP Subproblem]
    C --> D{Converged?}
    D -->|No| B
    D -->|Yes| E[Optimized Trajectory]

TrajOpt Costs and Constraints¶

Python

from tesseract_robotics.tesseract_motion_planners_trajopt import (
    TrajOptDefaultPlanProfile,
    TrajOptDefaultCompositeProfile,
)
from tesseract_robotics.tesseract_collision import CollisionEvaluatorType

# Plan profile (per-waypoint settings)
plan_profile = TrajOptDefaultPlanProfile()
plan_profile.cartesian_constraint_config.enabled = True
plan_profile.cartesian_constraint_config.coeff = [5, 5, 5, 5, 5, 5]  # Position/orientation weights

# Composite profile (trajectory-wide settings)
composite_profile = TrajOptDefaultCompositeProfile()
composite_profile.collision_cost_config.enabled = True
composite_profile.collision_cost_config.collision_margin_buffer = 0.025  # 2.5cm buffer
composite_profile.collision_cost_config.collision_coeff_data.setDefaultCollisionCoeff(20.0)
composite_profile.collision_cost_config.collision_check_config.type = CollisionEvaluatorType.DISCRETE
composite_profile.smooth_velocities = True
composite_profile.smooth_accelerations = True

Collision Configuration (0.33 API)¶

Python

from tesseract_robotics.tesseract_motion_planners_trajopt import TrajOptCollisionConfig
from tesseract_robotics.tesseract_collision import CollisionEvaluatorType

# TrajOptCollisionConfig constructor: (margin, coeff) or default
collision_config = TrajOptCollisionConfig(0.025, 20.0)  # margin=2.5cm, coeff=20
collision_config.collision_margin_buffer = 0.005  # Additional buffer
collision_config.collision_check_config.type = CollisionEvaluatorType.DISCRETE
collision_config.collision_check_config.longest_valid_segment_length = 0.05  # For LVS modes

Costs vs Constraints (Soft vs Hard)¶

TrajOpt distinguishes between costs (soft constraints) and constraints (hard constraints):

Type	Behavior	Solver Treatment	Use When
Cost	Penalized, can be violated	Added to objective function	Preferences, nice-to-haves
Constraint	Must be satisfied exactly	Strict equality/inequality	Safety-critical requirements

When to Use Each¶

Use Costs (Soft) for:

Collision avoidance during optimization (allows exploration)
Smoothness objectives (velocity, acceleration, jerk)
Preferred poses or configurations
Joint centering (stay near middle of range)

Use Constraints (Hard) for:

Final collision check (must be collision-free)
Exact Cartesian poses (pick/place positions)
Joint limits (physical limits)
Orientation requirements (e.g., tool must be vertical)

Example: Collision as Cost vs Constraint¶

Python

from tesseract_robotics.tesseract_motion_planners_trajopt import (
    TrajOptDefaultCompositeProfile,
)
from tesseract_robotics.tesseract_collision import CollisionEvaluatorType

profile = TrajOptDefaultCompositeProfile()

# SOFT: Collision as cost - allows temporary violations during optimization
# Good for finding paths through tight spaces
profile.collision_cost_config.enabled = True
profile.collision_cost_config.collision_margin_buffer = 0.025  # 2.5cm
profile.collision_cost_config.collision_coeff_data.setDefaultCollisionCoeff(20.0)
profile.collision_cost_config.collision_check_config.type = CollisionEvaluatorType.DISCRETE

# HARD: Collision as constraint - must be satisfied at solution
# Guarantees collision-free result (but may fail to find solution)
profile.collision_constraint_config.enabled = True
profile.collision_constraint_config.collision_margin_buffer = 0.01  # 1cm (tighter)
profile.collision_constraint_config.collision_check_config.type = CollisionEvaluatorType.DISCRETE

Recommended Pattern

Use both cost and constraint:

Cost with larger margin (2.5cm) guides optimization away from obstacles
Constraint with tighter margin (1cm) ensures final solution is collision-free

The cost helps the optimizer explore, while the constraint guarantees safety.

Example: Cartesian Pose as Cost vs Constraint¶

Python

from tesseract_robotics.tesseract_motion_planners_trajopt import TrajOptDefaultPlanProfile

plan_profile = TrajOptDefaultPlanProfile()

# SOFT: Cartesian cost - "try to reach this pose"
# Useful when exact pose isn't critical
plan_profile.cartesian_cost_config.enabled = True
plan_profile.cartesian_cost_config.coeff = [10, 10, 10, 5, 5, 5]  # xyz, rpy weights

# HARD: Cartesian constraint - "must reach this exact pose"
# Required for pick/place operations
plan_profile.cartesian_constraint_config.enabled = True
plan_profile.cartesian_constraint_config.coeff = [1, 1, 1, 1, 1, 1]

Coefficient Interpretation¶

The coeff values control how strongly violations are penalized:

Higher coefficient = stronger penalty = more important to satisfy
Lower coefficient = weaker penalty = more flexibility allowed

Python

# Position matters more than orientation
plan_profile.cartesian_cost_config.coeff = [100, 100, 100, 1, 1, 1]

# Rotation around Z is free (e.g., symmetric tool)
plan_profile.cartesian_cost_config.coeff = [10, 10, 10, 10, 10, 0]

Hard Constraints Can Cause Failures

Over-constraining a problem makes it harder (or impossible) to solve:

Start with costs only, verify solution quality
Add constraints incrementally
If planning fails, check if constraints are feasible

Descartes Planner¶

Graph-search planner for dense Cartesian toolpaths:

graph LR
    A[Waypoint 1<br/>IK Solutions] --> B[Waypoint 2<br/>IK Solutions]
    B --> C[Waypoint 3<br/>IK Solutions]
    C --> D[Waypoint N<br/>IK Solutions]

    A1[Sol 1] --> B1[Sol 1]
    A1 --> B2[Sol 2]
    A2[Sol 2] --> B1
    A2 --> B2

Best for:

Welding paths
Painting trajectories
Machining toolpaths

Python

from tesseract_robotics.tesseract_motion_planners_descartes import (
    DescartesDefaultPlanProfile
)

profile = DescartesDefaultPlanProfile()
profile.num_threads = 4  # Parallel IK solving

Motion Types¶

Freespace Motion¶

Any collision-free path between configurations:

Python

from tesseract_robotics.tesseract_command_language import (
    MoveInstruction, MoveInstructionType
)

move = MoveInstruction(waypoint, MoveInstructionType.FREESPACE, "DEFAULT")

Linear Motion¶

Straight-line Cartesian path:

Python

move = MoveInstruction(waypoint, MoveInstructionType.LINEAR, "DEFAULT")

Linear Motion Requirements

Linear motion requires:

Reachable start and end poses
Collision-free swept volume
Within joint velocity limits

Circular Motion¶

Arc motion (specialized planners):

Python

move = MoveInstruction(waypoint, MoveInstructionType.CIRCULAR, "DEFAULT")

Program Structure¶

Complex motions are defined as programs:

Python

from tesseract_robotics.tesseract_command_language import (
    CompositeInstruction, MoveInstruction,
    StateWaypointPoly, CartesianWaypointPoly
)

# Create program
program = CompositeInstruction("DEFAULT")

# Add start state
start_wp = StateWaypointPoly.wrap_StateWaypoint(
    StateWaypoint(joint_names, start_joints)
)
program.appendMoveInstruction(
    MoveInstruction(start_wp, MoveInstructionType.START, "DEFAULT")
)

# Add goal waypoints
for pose in waypoints:
    cart_wp = CartesianWaypointPoly.wrap_CartesianWaypoint(
        CartesianWaypoint(pose)
    )
    program.appendMoveInstruction(
        MoveInstruction(cart_wp, MoveInstructionType.LINEAR, "DEFAULT")
    )

Planning with Constraints¶

Orientation Constraints¶

Keep end-effector upright (e.g., carrying a glass):

Python

# In TrajOpt profile
profile.cartesian_coeff = [1, 1, 1, 10, 10, 1]  # High rotation weights

Joint Constraints¶

Limit specific joints:

Python

# Lock joint 1
profile.joint_coeff = [100, 1, 1, 1, 1, 1]  # High weight on joint 1

Handling Planning Failures¶

Python

trajectory = planner.plan(start, goal, planner="ompl")

if trajectory is None:
    print("Planning failed!")

    # Debug strategies:
    # 1. Check if start/goal are collision-free
    print(f"Start collision-free: {robot.check_collision(start)}")
    print(f"Goal collision-free: {robot.check_collision(goal)}")

    # 2. Increase planning time
    trajectory = planner.plan(
        start, goal,
        planner="ompl",
        planning_time=10.0
    )

    # 3. Try different planner
    trajectory = planner.plan(start, goal, planner="trajopt")

Trajectory Output¶

Python

if trajectory:
    print(f"Waypoints: {len(trajectory)}")

    for i, waypoint in enumerate(trajectory):
        print(f"  [{i}] positions: {waypoint.positions}")
        print(f"       velocities: {waypoint.velocities}")
        print(f"       time: {waypoint.time}")

Performance Tips¶

Warm-Start TrajOpt

Provide an initial trajectory for faster convergence:

Python

# Use OMPL solution as TrajOpt initial guess
initial = planner.plan(start, goal, planner="ompl")
refined = planner.refine(initial, planner="trajopt")

Reduce Search Space

Python

# Tighter joint limits for faster planning
profile.joint_limits_scale = 0.8  # 80% of full limits

Profile Caching

Profiles are expensive to create. Cache and reuse:

Python

# Create once
self.ompl_profile = OMPLDefaultPlanProfile()

# Reuse for all plans
trajectory = planner.plan(..., profile=self.ompl_profile)

Next Steps¶

Task Composer - Complex multi-step planning
Low-Level SQP - Real-time trajectory optimization