Motion Planning

tesseract_robotics provides multiple motion planners for different use cases. The high-level tesseract_robotics.planning API exposes three module-level entry points — plan_freespace, plan_ompl, and plan_cartesian — each accepting a MotionProgram describing the waypoints. Under the hood these dispatch to TaskComposer pipelines, so you can drop down to TaskComposer.plan(...) when you need a specific pipeline variant.

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

All three entry points take the same shape: a Robot plus a MotionProgram of JointTarget / CartesianTarget waypoints.

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

plan_ompl defaults to the FreespacePipeline — OMPL RRTConnect for path finding, TrajOpt for smoothing, and time parameterization.

Python

import numpy as np
from tesseract_robotics.planning import (
    Robot, MotionProgram, JointTarget, plan_ompl,
)

robot = Robot.from_tesseract_support("abb_irb2400")
program = (
    MotionProgram("manipulator")
    .move_to(JointTarget(np.zeros(6)))
    .move_to(JointTarget(np.array([0.5, -0.5, 0.5, 0.0, 0.5, 0.0])))
)

result = plan_ompl(robot, program)
if result.successful:
    for state in result.trajectory:
        print(state.positions)

plan_freespace drives the TrajOptPipeline — pure optimization-based freespace planning. Ideal when the direct path is roughly correct and just needs smoothing plus collision margin enforcement.

Python

from tesseract_robotics.planning import plan_freespace

result = plan_freespace(robot, program)

plan_cartesian drives the DescartesPipeline — Descartes graph search for precise Cartesian paths, with TrajOpt smoothing. Use this when the program contains CartesianTarget waypoints and you need tool-pose accuracy.

Python

from tesseract_robotics.planning import CartesianTarget, Pose, plan_cartesian

program = (
    MotionProgram("manipulator", tcp_frame="tool0")
    .move_to(CartesianTarget(Pose.from_xyz(0.5, -0.2, 0.5)))
    .linear_to(CartesianTarget(Pose.from_xyz(0.5, 0.2, 0.5)))
)

result = plan_cartesian(robot, program)

For hybrid planning (OMPL for global search, TrajOpt for local refinement) use the FreespacePipeline directly via TaskComposer — it chains OMPL, TrajOpt, and time parameterization in one call.

Python

from tesseract_robotics.planning import TaskComposer

composer = TaskComposer.from_config()
result = composer.plan(robot, program, pipeline="FreespacePipeline")

plan_ompl(robot, program) is a shortcut for exactly this pipeline.

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

Profiles control per-call planner behaviour. The create_ompl_default_profiles helper packages sensible defaults (parallel RRTConnect with a 10 s budget):

Python

from tesseract_robotics.planning.profiles import create_ompl_default_profiles

profiles = create_ompl_default_profiles(
    planning_time=5.0,   # Max planning time (seconds)
    num_planners=4,      # Parallel RRTConnect instances
    max_solutions=10,    # Stop after this many solutions
    optimize=True,       # Continue improving until timeout
)

result = plan_ompl(robot, program, profiles=profiles)

For freespace-pipeline use (OMPL + TrajOpt together), use create_freespace_pipeline_profiles — same arguments plus the TrajOpt smoothing profile in the same dictionary.

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

Use create_ompl_planner_configurators to mix planner types inside one profile.

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

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

TrajOptCollisionConfig lives in trajopt_ifopt (trajopt_common) and is re-exported from tesseract_motion_planners_trajopt, so either import works.

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:

1. Cost with larger margin (2.5cm) guides optimization away from obstacles
2. 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.planning.profiles import create_descartes_default_profiles

profiles = create_descartes_default_profiles(
    enable_collision=True,        # Check vertex (waypoint) collisions
    enable_edge_collision=False,  # Check transition collisions (slower)
    num_threads=4,                # Parallel IK solving
)

result = plan_cartesian(robot, program, profiles=profiles)

Custom Edge / State Evaluators in Python

When the default cost function (joint-space distance) does not fully satisfy your requirements, you can subclass the evaluator interfaces directly in Python to tweak how Descartes picks the right IK branch. Examples include penalizing wrist flips, weighting specific joints, or scoring edges using an external signal.

The three extension points are:

Class	Override	When it runs
DescartesWaypointSamplerD	sample() -> list[DescartesStateSampleD]	Generating IK candidates for a waypoint
DescartesEdgeEvaluatorD	evaluate(start, end) -> (valid, cost)	Edge between two graph states
DescartesStateEvaluatorD	evaluate(state) -> (valid, cost)	Per-state cost

To inject them into the planner, subclass DescartesMoveProfileD and return your custom objects from the three factory methods. Delegate the ones you don't want to customize to an inner DescartesDefaultMoveProfileD:

Python

import numpy as np
from tesseract_robotics.tesseract_motion_planners_descartes import (
    DescartesDefaultMoveProfileD,
    DescartesEdgeEvaluatorD,
    DescartesMoveProfileD,
    cast_DescartesMoveProfileD,
)

class WristFlipPenaltyEvaluator(DescartesEdgeEvaluatorD):
    """Cost = L2 joint delta + large penalty for >90 deg wrist swing."""

    def evaluate(self, start, end):
        delta = end.values - start.values
        cost = float(np.linalg.norm(delta))
        if abs(delta[-1]) > np.pi / 2:   # wrist joint flip
            return (False, cost)         # reject this edge
        return (True, cost)


class MyMoveProfile(DescartesMoveProfileD):
    def __init__(self):
        super().__init__()
        self._inner = DescartesDefaultMoveProfileD()
        # Hold evaluators on self so the planner sees the same instance
        # across all edge evaluations.
        self._edge_evaluator = WristFlipPenaltyEvaluator()

    def createWaypointSampler(self, move_instruction, composite_manip_info, env):
        return self._inner.createWaypointSampler(move_instruction, composite_manip_info, env)

    def createEdgeEvaluator(self, move_instruction, composite_manip_info, env):
        return self._edge_evaluator

    def createStateEvaluator(self, move_instruction, composite_manip_info, env):
        return self._inner.createStateEvaluator(move_instruction, composite_manip_info, env)

The start and end arguments to evaluate are DescartesStateD objects with a .values attribute that is a 1-D numpy array of joint positions in the manipulator's joint order.

Then, register the profile in the ProfileDictionary the same way you would a C++ profile, using cast_DescartesMoveProfileD to coerce it to the base Profile type:

Python

from tesseract_robotics.tesseract_command_language import ProfileDictionary

profiles = ProfileDictionary()
profiles.addProfile(
    "DescartesMotionPlannerTask",          # namespace
    "MY_PROFILE",                          # profile name (referenced by MoveInstruction)
    cast_DescartesMoveProfileD(MyMoveProfile()),
)

Threading and the GIL

Each call back into Python acquires the GIL, so OpenMP-parallel edge evaluation inside Descartes is serialized through your Python evaluator. If you set num_threads > 1, the cost computation itself runs single-threaded — only IK sampling parallelizes. Keep the Python evaluate body tight (numpy ops, no allocations) for best throughput.

Use for joint-continuity across rasters

A custom edge evaluator is the cleanest way to globally enforce IK-branch consistency across an entire raster program — the raster pipeline itself does not backtrack on a downstream transition failure, so picking compatible IK solutions during Descartes is the only reliable solution.

Motion Types

Motion type is set via the MotionProgram builder methods:

Freespace Motion

Any collision-free path between configurations. Use move_to(...):

Python

program.move_to(JointTarget(joint_values))
program.move_to(CartesianTarget(pose))  # freespace to pose

Linear Motion

Straight-line Cartesian path. Use linear_to(...):

Python

program.linear_to(CartesianTarget(pose))

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). Use circular_to(...):

Python

program.circular_to(CartesianTarget(pose))

Program Structure

Use the MotionProgram builder — it handles all the poly-type wrapping automatically. For reference, the equivalent low-level call would be:

Python

from tesseract_robotics.tesseract_command_language import (
    CartesianWaypoint, CartesianWaypointPoly_wrap_CartesianWaypoint,
    CompositeInstruction, MoveInstruction, MoveInstructionType_LINEAR,
    MoveInstructionPoly_wrap_MoveInstruction,
)

composite = CompositeInstruction("DEFAULT")
for pose in waypoints:
    wp = CartesianWaypointPoly_wrap_CartesianWaypoint(CartesianWaypoint(pose))
    instr = MoveInstructionPoly_wrap_MoveInstruction(
        MoveInstruction(wp, MoveInstructionType_LINEAR, "DEFAULT")
    )
    composite.appendMoveInstruction(instr)

The MotionProgram fluent API builds exactly the same structure with far less ceremony, and the planning entry points accept either form.

Planning with Constraints

Orientation Constraints

Keep end-effector upright (e.g., carrying a glass). The UPRIGHT profile is pre-baked by create_trajopt_upright_profiles:

Python

from tesseract_robotics.planning.profiles import create_trajopt_upright_profiles

profiles = create_trajopt_upright_profiles()
result = plan_freespace(robot, program, profiles=profiles)

Equivalent manual setup:

Python

# In TrajOpt plan profile — position free (0), orientation constrained (5)
plan_profile.cartesian_constraint_config.coeff = [0, 0, 0, 5, 5, 5]

Joint Constraints

Weight individual joints to lock or favour them:

Python

# High weight on joint 1 - strongly discouraged from moving
plan_profile.joint_constraint_config.coeff = [100, 1, 1, 1, 1, 1]

Handling Planning Failures

Python

result = plan_freespace(robot, program)

if not result.successful:
    print(f"Planning failed: {result.message}")

    # Debug strategies:

    # 1. Check if start/goal are collision-free
    from tesseract_robotics.tesseract_collision import (
        ContactRequest, ContactResultMap, ContactTestType_ALL,
    )
    manager = robot.env.getDiscreteContactManager()
    manager.setActiveCollisionObjects(robot.env.getActiveLinkNames())
    manager.setCollisionObjectsTransform(robot.env.getState().link_transforms)
    contacts = ContactResultMap()
    manager.contactTest(contacts, ContactRequest(ContactTestType_ALL))
    print(f"Collision-free: {contacts.size() == 0}")

    # 2. Try a different backend
    result = plan_ompl(robot, program)       # sampling-based (OMPL + TrajOpt)
    result = plan_cartesian(robot, program)  # graph search (Descartes)

    # 3. Increase planning time on OMPL
    from tesseract_robotics.planning.profiles import create_ompl_default_profiles
    profiles = create_ompl_default_profiles(planning_time=30.0)
    result = plan_ompl(robot, program, profiles=profiles)

Trajectory Output

Python

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

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

Each TrajectoryPoint carries joint_names, positions, and optionally velocities, accelerations, and time (time from start, seconds). Velocities and timing are populated by pipelines that include time parameterization (FreespacePipeline, CartesianPipeline, etc.); pure OMPL output has positions only. result.to_numpy() returns an (N, num_joints) array of positions.

Performance Tips

Warm-Start via FreespacePipeline

The FreespacePipeline (what plan_ompl runs by default) already does warm-start internally: OMPL finds a feasible path, TrajOpt then refines it in the same call. If you only need a sampling solution, pass pipeline="OMPLPipeline" to drop the TrajOpt step.

Python

from tesseract_robotics.planning import plan_ompl

# Default: OMPL + TrajOpt refinement
result = plan_ompl(robot, program)

# Raw OMPL output (no refinement)
result = plan_ompl(robot, program, pipeline="OMPLPipeline")

Pre-Load Plugins

First plan_*() call takes ~10–15 s due to plugin dlopen. In interactive or long-running sessions, warm up once at startup:

Python

from tesseract_robotics.planning import TaskComposer

composer = TaskComposer.from_config(warmup=True)  # ~10–15s once
# Subsequent composer.plan() calls are fast

Profile Caching

Profile factory helpers do real work — cache and reuse the returned ProfileDictionary:

Python

from tesseract_robotics.planning.profiles import (
    create_freespace_pipeline_profiles,
)

# Create once
self.freespace_profiles = create_freespace_pipeline_profiles()

# Reuse for all plans
result = plan_ompl(robot, program, profiles=self.freespace_profiles)

Next Steps

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