Skip to content

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.

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", tcp_frame="tool0", working_frame="base_link")
    .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 DescartesDPipeline — 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", working_frame="base_link")
    .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.contact_manager_config.default_margin = 0.025  # 2.5cm margin
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  # collect contacts 5mm past the margin
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.

The margin is the clearance the term drives toward; it lives in contact_manager_config.default_margin, which the constructor's first argument sets. collision_margin_buffer only widens the distance within which contacts are collected and adds no error, so it cannot stand in for the margin. A default-constructed config has no margin (default_margin is None): the term then engages only at contact, and TrajOpt settles paths on the obstacle's surface, where a later contact check can reject them.

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.contact_manager_config.default_margin = 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.contact_manager_config.default_margin = 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.

On macOS, num_threads > 1 may not be available at all: a wheel installed into a conda or pixi environment brings a second OpenMP runtime, and the ladder solver segfaults above one thread. See macOS Wheel Gotchas.

With vertex collision on, a Descartes solve is dominated by setup, not search

Building each waypoint's sampler constructs a DescartesCollision, which clones the environment's Bullet contact manager — and a Bullet clone rebuilds and zero-fills its pool allocators. A sample profile of DescartesMotionPlannerD.solve on a 419-waypoint contour (abb_irb2400, redundant solutions on) puts ~97 % of the solve there, serially on the calling thread, before the parallel ladder build starts; the OpenMP workers wait throughout.

Vertex collision Threads Wall CPU / wall
on 1 435.4 ms 1.00
on 8 432.3 ms 1.02
off 1 2.7 ms 1.00
off 8 1.9 ms 1.78

So num_threads changes nothing measurable while vertex collision is on, and the graph search itself is cheap. Turning vertex collision off needs allow_collision = True as well, or the planner refuses for want of a checker — and then collisions must be checked elsewhere, for instance by the pipeline's own DiscreteContactCheckTask.

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.

Frames and the TCP

A MotionProgram names its frames; the planning API never picks one for you:

Argument Meaning ABB IRB2400 example
tcp_frame (required) the link the TCP is attached to "tool0"
working_frame (required) the link CartesianTarget poses are relative to "base_link"
tcp_offset (keyword, optional) offset from tcp_frame: a named TCP or an Isometry3d "laser"

The planner puts the tool, T(tcp_frame) * tcp_offset, on each Cartesian target. A named offset is resolved by the environment at plan time, first from the SRDF group_tcps of the program's group, then from find-TCP callbacks; a link name there is an error (use it as tcp_frame). The full rule, with upstream sources, is under TCP offsets; the SRDF side is under Named TCPs.

Python
import numpy as np
from tesseract_robotics.planning import CartesianTarget, MotionProgram, Pose
from tesseract_robotics.tesseract_common import Isometry3d

# a named TCP from abb_irb2400.srdf's <group_tcps group="manipulator">
welder = MotionProgram("manipulator", tcp_frame="tool0", working_frame="base_link", tcp_offset="welder")

# or an explicit offset: a tool 10 cm along tool0's z axis
offset = np.eye(4)
offset[2, 3] = 0.10
program = MotionProgram(
    "manipulator", tcp_frame="tool0", working_frame="base_link", tcp_offset=Isometry3d(offset)
).move_to(CartesianTarget(Pose.from_xyz(0.8, 0.0, 1.0)))

Robot.get_manipulator_info(group, tcp_frame, working_frame, tcp_offset=...) builds the same ManipulatorInfo for the low-level API. Robot.fk / Robot.ik take a link (tip_link), not a named TCP: apply or strip the offset yourself (see Tool Frames).

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

Each debug step below plans again, so hold one composer and pass it to every call; without composer=, each call reloads the task composer plugins.

Python
from tesseract_robotics.planning import (
    TaskComposer, plan_cartesian, plan_freespace, plan_ompl,
)

composer = TaskComposer.from_config()
result = plan_freespace(robot, program, composer=composer)

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,
    )
    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, composer=composer)       # sampling-based (OMPL + TrajOpt)
    result = plan_cartesian(robot, program, composer=composer)  # 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, composer=composer)

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 TaskComposer, plan_ompl

composer = TaskComposer.from_config()

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

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

Hold and Pre-Load One Composer

Every TaskComposer.from_config() loads the task composer plugins, about 4.3 s on macOS arm64, and a plan_*() call without composer= builds a new one each time. Build one composer at startup, warm the pipelines you will run, and pass it to every call. A warmup only pays off on the composer that then plans: a 419-waypoint Descartes solve takes 0.47 s through a held, warmed composer versus about 5 s through plan_cartesian without one (#137).

Python
from tesseract_robotics.planning import TaskComposer, plan_cartesian, plan_ompl

composer = TaskComposer.from_config(warmup=["FreespacePipeline", "DescartesDPipeline"])

result = plan_ompl(robot, program, composer=composer)       # FreespacePipeline
result = plan_cartesian(robot, program, composer=composer)  # DescartesDPipeline

Cache the Composer and the Profiles

For repeated plans, hold both the composer and the ProfileDictionary. The composer is the large saving: it skips the plugin load (about 4.3 s on macOS arm64) that every plan_*() call without composer= repeats. Caching profiles saves the work the profile factory helpers do to build them, which is far smaller; on its own it leaves the plugin load in every call.

Python
from tesseract_robotics.planning import TaskComposer, plan_ompl
from tesseract_robotics.planning.profiles import (
    create_freespace_pipeline_profiles,
)

# Create once
composer = TaskComposer.from_config(warmup=["FreespacePipeline"])
freespace_profiles = create_freespace_pipeline_profiles()

# Reuse for all plans
for program in programs:
    result = plan_ompl(robot, program, profiles=freespace_profiles, composer=composer)

Next Steps