Skip to content

tesseract_robotics.tesseract_motion_planners_trajopt

TrajOpt trajectory optimization planner.

Overview

TrajOpt optimizes trajectories for smoothness while avoiding collisions. Best for Cartesian paths and trajectory refinement.

Python
from tesseract_robotics.tesseract_motion_planners_trajopt import (
    TrajOptMotionPlanner,
    TrajOptDefaultPlanProfile, TrajOptDefaultCompositeProfile,
    TrajOptPlanProfile, TrajOptCompositeProfile,
    TrajOptCollisionConfig,
    ProfileDictionary_addTrajOptPlanProfile,
    ProfileDictionary_addTrajOptCompositeProfile,
)
from tesseract_robotics.tesseract_collision import CollisionEvaluatorType

TrajOptCollisionConfig lives in trajopt_ifopt

It's defined in tesseract_robotics.trajopt_ifopt (trajopt_common) and re-exported from tesseract_motion_planners_trajopt for convenience. Import from either location.

TrajOptMotionPlanner

TrajOptMotionPlanner derives from MotionPlanner (tesseract_motion_planners).

Python
from tesseract_robotics.tesseract_motion_planners_trajopt import TrajOptMotionPlanner
from tesseract_robotics.tesseract_motion_planners import PlannerRequest

planner = TrajOptMotionPlanner()

request = PlannerRequest()
request.env = env
request.instructions = program
request.profiles = profiles

response = planner.solve(request)

Profiles

TrajOptDefaultPlanProfile

Per-waypoint configuration.

Python
from tesseract_robotics.tesseract_motion_planners_trajopt import TrajOptDefaultPlanProfile

profile = TrajOptDefaultPlanProfile()

# Cartesian waypoint config
profile.cartesian_constraint_config.enabled = True
profile.cartesian_constraint_config.coeff = np.array([5.0, 5.0, 5.0, 2.0, 2.0, 2.0])  # xyz, rpy weights

# Or as cost instead of constraint
profile.cartesian_cost_config.enabled = True
profile.cartesian_cost_config.coeff = np.array([5.0, 5.0, 5.0, 2.0, 2.0, 2.0])

# Joint waypoint config
profile.joint_constraint_config.enabled = True
profile.joint_constraint_config.coeff = np.ones(6)  # weight per joint

TrajOptDefaultCompositeProfile

Trajectory-wide configuration.

Python
from tesseract_robotics.tesseract_motion_planners_trajopt import TrajOptDefaultCompositeProfile
from tesseract_robotics.tesseract_collision import CollisionEvaluatorType

profile = TrajOptDefaultCompositeProfile()

# Collision avoidance — collision_*_config is a TrajOptCollisionConfig
profile.collision_cost_config.enabled = True
profile.collision_cost_config.contact_manager_config.default_margin = 0.025
profile.collision_cost_config.collision_coeff_data.setDefaultCollisionCoeff(20.0)
profile.collision_cost_config.collision_check_config.type = CollisionEvaluatorType.DISCRETE

profile.collision_constraint_config.enabled = False  # use cost, not constraint

# Smoothness costs
profile.smooth_velocities = True
profile.smooth_accelerations = True
profile.smooth_jerks = False

profile.velocity_coeff = np.ones(6)
profile.acceleration_coeff = np.ones(6)

The composite profile turns a collision config's longest-valid-segment fraction and length into the step its collision terms check at. The static computeLongestValidSegmentLength gives the same number from Python: min(fraction * extent, length) when both are positive, the positive one otherwise, and 0.01 * extent when neither is, with extent = ||upper - lower|| over the joint limits.

Python
limits = env.getJointGroup("manipulator").getLimits().joint_limits  # n x 2
cfg = profile.collision_cost_config.collision_check_config
step = TrajOptDefaultCompositeProfile.computeLongestValidSegmentLength(
    limits, cfg.longest_valid_segment_fraction, cfg.longest_valid_segment_length
)

TrajOptOSQPSolverProfile

The solver profile holds the SQP parameters (opt_params, a BasicTrustRegionSQPParameters) and the QP solver's settings, an OSQPSettings. A new profile holds trajopt's OSQP defaults (eps_abs=1e-4, eps_rel=1e-6, max_iter=8192, polishing and adaptive rho on); OSQPSettings() on its own holds OSQP's defaults.

Python
from tesseract_robotics.tesseract_motion_planners_trajopt import (
    ProfileDictionary_addTrajOptSolverProfile, TrajOptOSQPSolverProfile,
)

solver = TrajOptOSQPSolverProfile()
solver.opt_params.max_iter = 200
settings = solver.settings
settings.eps_abs = 1e-5
settings.time_limit = 0.5
solver.settings = settings

ProfileDictionary_addTrajOptSolverProfile(profiles, "TrajOptMotionPlannerTask", "DEFAULT", solver)

createOptimizationParameters() returns the SQP parameters the planner uses for that profile (a copy of opt_params). The OSQP enum fields linsys_solver and cg_precond are not bound.

Collision Configuration

TrajOptCollisionConfig

Replaces the 0.33 CollisionCostConfig / CollisionConstraintConfig. See changes for the full migration notes.

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

# Constructor: TrajOptCollisionConfig(margin, coeff) or default
config = TrajOptCollisionConfig(0.025, 20.0)  # margin=2.5cm, coeff=20
config.enabled = True
config.collision_margin_buffer = 0.005  # collect contacts 5mm past the margin
config.collision_check_config.type = CollisionEvaluatorType.DISCRETE
config.collision_check_config.longest_valid_segment_length = 0.05  # for LVS modes

The margin (contact_manager_config.default_margin, the constructor's first argument) is the clearance the term drives toward. collision_margin_buffer only widens contact collection and adds no error. A default-constructed config has no margin: the term engages only at contact and TrajOpt settles paths on the obstacle's surface.

CollisionEvaluatorType

Module Location

CollisionEvaluatorType lives in tesseract_robotics.tesseract_collision (re-exported from the TrajOpt module for convenience).

Type Description Speed
DISCRETE Check at waypoints only Fast
LVS_DISCRETE Interpolate between waypoints, discrete checks Medium
LVS_CONTINUOUS Swept volume check, LVS interpolation Slow
CONTINUOUS Full continuous collision Slowest

Waypoint Configurations

TrajOptCartesianWaypointConfig

Python
from tesseract_robotics.tesseract_motion_planners_trajopt import TrajOptCartesianWaypointConfig

config = TrajOptCartesianWaypointConfig()
config.enabled = True
config.coeff = np.array([5.0, 5.0, 5.0, 2.0, 2.0, 2.0])
config.lower_tolerance = np.zeros(6)
config.upper_tolerance = np.zeros(6)

TrajOptJointWaypointConfig

Python
from tesseract_robotics.tesseract_motion_planners_trajopt import TrajOptJointWaypointConfig

config = TrajOptJointWaypointConfig()
config.enabled = True
config.coeff = np.ones(6)
config.lower_tolerance = np.zeros(6)
config.upper_tolerance = np.zeros(6)

Value equality

The waypoint configs, the default move/composite profiles, TrajOptOSQPSolverProfile, BasicTrustRegionSQPParameters and OSQPSettings compare by value (==, !=) and are unhashable.

Adding Profiles

Python
from tesseract_robotics.tesseract_command_language import ProfileDictionary
from tesseract_robotics.tesseract_motion_planners_trajopt import (
    TrajOptDefaultPlanProfile, TrajOptDefaultCompositeProfile,
    ProfileDictionary_addTrajOptPlanProfile,
    ProfileDictionary_addTrajOptCompositeProfile,
)
from tesseract_robotics.tesseract_collision import CollisionEvaluatorType

# Plan profile (per waypoint)
plan_profile = TrajOptDefaultPlanProfile()
plan_profile.cartesian_constraint_config.enabled = True
plan_profile.cartesian_constraint_config.coeff = np.array([10.0, 10.0, 10.0, 5.0, 5.0, 5.0])

# Composite profile (trajectory-wide)
composite_profile = TrajOptDefaultCompositeProfile()
composite_profile.collision_cost_config.enabled = True
composite_profile.collision_cost_config.contact_manager_config.default_margin = 0.025
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

# Add to dictionary
profiles = ProfileDictionary()
ProfileDictionary_addTrajOptPlanProfile(profiles, "TrajOptMotionPlannerTask", "DEFAULT", plan_profile)
ProfileDictionary_addTrajOptCompositeProfile(profiles, "TrajOptMotionPlannerTask", "DEFAULT", composite_profile)

ProfileDictionary.hasProfile / getProfile take a profile key. Each profile kind has one: TrajOptMoveProfile.createKey(), TrajOptCompositeProfile.createKey() and TrajOptSolverProfile.createKey(), inherited by their concrete profiles.

Python
composite = profiles.getProfile(
    TrajOptDefaultCompositeProfile.createKey(), "TrajOptMotionPlannerTask", "DEFAULT"
)

Complete Example

Python
from tesseract_robotics.tesseract_motion_planners_trajopt import (
    TrajOptMotionPlanner, TrajOptDefaultPlanProfile, TrajOptDefaultCompositeProfile,
    ProfileDictionary_addTrajOptPlanProfile, ProfileDictionary_addTrajOptCompositeProfile,
)
from tesseract_robotics.tesseract_motion_planners import PlannerRequest
from tesseract_robotics.tesseract_command_language import ProfileDictionary
from tesseract_robotics.tesseract_collision import CollisionEvaluatorType
import numpy as np

# Configure profiles
plan_profile = TrajOptDefaultPlanProfile()
plan_profile.cartesian_constraint_config.enabled = True
plan_profile.cartesian_constraint_config.coeff = np.array([10, 10, 10, 5, 5, 5])

composite_profile = TrajOptDefaultCompositeProfile()
composite_profile.collision_cost_config.enabled = True
composite_profile.collision_cost_config.contact_manager_config.default_margin = 0.025
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.velocity_coeff = np.ones(6)

profiles = ProfileDictionary()
ProfileDictionary_addTrajOptPlanProfile(profiles, "TrajOptMotionPlannerTask", "DEFAULT", plan_profile)
ProfileDictionary_addTrajOptCompositeProfile(profiles, "TrajOptMotionPlannerTask", "DEFAULT", composite_profile)

# Create request
request = PlannerRequest()
request.env = env
request.instructions = program
request.profiles = profiles

# Solve
planner = TrajOptMotionPlanner()
response = planner.solve(request)

if response.successful:
    trajectory = response.results
else:
    print(f"TrajOpt failed: {response.message}")

Logging

Control TrajOpt logging via environment variable:

Bash
export TRAJOPT_LOG_THRESH=ERROR  # FATAL, ERROR, WARN, INFO, DEBUG, TRACE

Tips

  1. Use as refinement - TrajOpt works best refining OMPL paths
  2. Start with collision cost - easier to tune than hard constraints
  3. Increase the margin (contact_manager_config.default_margin) if paths pass too close; the buffer only widens contact collection
  4. Use LVS_DISCRETE for better collision coverage
  5. Tune coefficients - higher = stricter enforcement

Auto-generated API Reference

tesseract_motion_planners_trajopt Python bindings

TrajOptPlanProfile module-attribute

Python
TrajOptPlanProfile: TypeAlias = TrajOptMoveProfile

TrajOptDefaultPlanProfile module-attribute

Python
TrajOptDefaultPlanProfile: TypeAlias = TrajOptDefaultMoveProfile

TrajOptCartesianWaypointConfig

enabled property writable

Python
enabled: bool

use_tolerance_override property writable

Python
use_tolerance_override: bool

lower_tolerance property writable

Python
lower_tolerance: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

upper_tolerance property writable

Python
upper_tolerance: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

coeff property writable

Python
coeff: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

__hash__ class-attribute instance-attribute

Python
__hash__: None = None

__init__

Python
__init__() -> None

__eq__

Python
__eq__(arg: TrajOptCartesianWaypointConfig) -> bool

__ne__

Python
__ne__(arg: TrajOptCartesianWaypointConfig) -> bool

TrajOptJointWaypointConfig

enabled property writable

Python
enabled: bool

use_tolerance_override property writable

Python
use_tolerance_override: bool

lower_tolerance property writable

Python
lower_tolerance: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

upper_tolerance property writable

Python
upper_tolerance: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

coeff property writable

Python
coeff: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

__hash__ class-attribute instance-attribute

Python
__hash__: None = None

__init__

Python
__init__() -> None

__eq__

Python
__eq__(arg: TrajOptJointWaypointConfig) -> bool

__ne__

Python
__ne__(arg: TrajOptJointWaypointConfig) -> bool

TrajOptMoveProfile

Bases: Profile

getKey

Python
getKey() -> int

createKey staticmethod

Python
createKey() -> int

TrajOptCompositeProfile

Bases: Profile

getKey

Python
getKey() -> int

createKey staticmethod

Python
createKey() -> int

TrajOptDefaultMoveProfile

Bases: TrajOptMoveProfile

cartesian_cost_config property writable

Python
cartesian_cost_config: TrajOptCartesianWaypointConfig

cartesian_constraint_config property writable

Python
cartesian_constraint_config: TrajOptCartesianWaypointConfig

joint_cost_config property writable

Python
joint_cost_config: TrajOptJointWaypointConfig

joint_constraint_config property writable

Python
joint_constraint_config: TrajOptJointWaypointConfig

__hash__ class-attribute instance-attribute

Python
__hash__: None = None

__init__

Python
__init__() -> None

__eq__

Python
__eq__(arg: TrajOptDefaultMoveProfile) -> bool

__ne__

Python
__ne__(arg: TrajOptDefaultMoveProfile) -> bool

from_yaml staticmethod

Python
from_yaml(config: str) -> TrajOptDefaultMoveProfile

Construct the profile from its YAML config text; malformed YAML raises ValueError.

Nested sub-configs are replaced, not merged: a key naming one rebuilds it from that struct's own defaults, so fields left out lose the profile's values. This is per struct; here cartesian_cost_config: and joint_cost_config: come back enabled (the waypoint config default), where the profile's are disabled.

TrajOptDefaultCompositeProfile

Bases: TrajOptCompositeProfile

collision_cost_config property writable

Python
collision_cost_config: TrajOptCollisionConfig

collision_constraint_config property writable

Python
collision_constraint_config: TrajOptCollisionConfig

smooth_velocities property writable

Python
smooth_velocities: bool

velocity_coeff property writable

Python
velocity_coeff: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

smooth_accelerations property writable

Python
smooth_accelerations: bool

acceleration_coeff property writable

Python
acceleration_coeff: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

smooth_jerks property writable

Python
smooth_jerks: bool

jerk_coeff property writable

Python
jerk_coeff: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

avoid_singularity property writable

Python
avoid_singularity: bool

avoid_singularity_coeff property writable

Python
avoid_singularity_coeff: float

__hash__ class-attribute instance-attribute

Python
__hash__: None = None

__init__

Python
__init__() -> None

computeLongestValidSegmentLength staticmethod

Python
computeLongestValidSegmentLength(joint_limits: Annotated[NDArray[float64], dict(shape=(None, 2), order=F)] | Sequence[Sequence[float]], longest_valid_segment_fraction: float, longest_valid_segment_length: float) -> float

Collision-check step length from the joint limits (n x 2, lower/upper) and a longest-valid-segment fraction and length: the smaller of fraction * extent and length where both are positive, else the positive one, else 0.01 * extent, with extent = ||upper - lower||.

__eq__

Python
__eq__(arg: TrajOptDefaultCompositeProfile) -> bool

__ne__

Python
__ne__(arg: TrajOptDefaultCompositeProfile) -> bool

from_yaml staticmethod

Python
from_yaml(config: str) -> TrajOptDefaultCompositeProfile

Construct the profile from its YAML config text; malformed YAML raises ValueError.

Nested sub-configs are replaced, not merged: a key naming one rebuilds it from that struct's own defaults, so fields left out lose the profile's values. This is per struct; here collision_cost_config: and collision_constraint_config: start from TrajOptCollisionConfig() defaults.

BasicTrustRegionSQPParameters

improve_ratio_threshold property writable

Python
improve_ratio_threshold: float

Minimum ratio exact_improve/approx_improve to accept step

min_trust_box_size property writable

Python
min_trust_box_size: float

If trust region gets any smaller, exit and report convergence

min_approx_improve property writable

Python
min_approx_improve: float

If model improves less than this, exit and report convergence

min_approx_improve_frac property writable

Python
min_approx_improve_frac: float

If model improves less than this fraction, exit and report convergence

max_iter property writable

Python
max_iter: int

The max number of iterations

trust_shrink_ratio property writable

Python
trust_shrink_ratio: float

If improvement is less than improve_ratio_threshold, shrink trust region by this ratio

trust_expand_ratio property writable

Python
trust_expand_ratio: float

If improvement is greater than improve_ratio_threshold, expand trust region by this ratio

cnt_tolerance property writable

Python
cnt_tolerance: float

After convergence of penalty subproblem, if constraint violation is less than this, we're done

max_merit_coeff_increases property writable

Python
max_merit_coeff_increases: float

Max number of times that the constraints' cost will be increased

max_qp_solver_failures property writable

Python
max_qp_solver_failures: int

Max number of times the QP solver can fail before optimization is aborted

merit_coeff_increase_ratio property writable

Python
merit_coeff_increase_ratio: float

Ratio that we increase coeff each time

max_time property writable

Python
max_time: float

Max time in seconds that the optimizer will run

initial_merit_error_coeff property writable

Python
initial_merit_error_coeff: float

Initial coefficient that is used to scale the constraints

inflate_constraints_individually property writable

Python
inflate_constraints_individually: bool

If true, merit coeffs will only be inflated for the constraints that failed

trust_box_size property writable

Python
trust_box_size: float

Current size of trust region (component-wise)

log_results property writable

Python
log_results: bool

Log results to file

log_dir property writable

Python
log_dir: str

Directory to store log results

num_threads property writable

Python
num_threads: int

If greater than one, multi threaded functions are called

__hash__ class-attribute instance-attribute

Python
__hash__: None = None

__init__

Python
__init__() -> None

__eq__

Python
__eq__(arg: BasicTrustRegionSQPParameters) -> bool

__ne__

Python
__ne__(arg: BasicTrustRegionSQPParameters) -> bool

TrajOptSolverProfile

Bases: Profile

opt_params property writable

Python
opt_params: BasicTrustRegionSQPParameters

Optimization parameters

getKey

Python
getKey() -> int

createKey staticmethod

Python
createKey() -> int

createOptimizationParameters

Python
createOptimizationParameters() -> BasicTrustRegionSQPParameters

The optimization parameters the planner uses for this profile

OSQPSettings

device property writable

Python
device: int

allocate_solution property writable

Python
allocate_solution: int

verbose property writable

Python
verbose: int

profiler_level property writable

Python
profiler_level: int

warm_starting property writable

Python
warm_starting: int

scaling property writable

Python
scaling: int

polishing property writable

Python
polishing: int

rho property writable

Python
rho: float

rho_is_vec property writable

Python
rho_is_vec: int

sigma property writable

Python
sigma: float

alpha property writable

Python
alpha: float

cg_max_iter property writable

Python
cg_max_iter: int

cg_tol_reduction property writable

Python
cg_tol_reduction: int

cg_tol_fraction property writable

Python
cg_tol_fraction: float

adaptive_rho property writable

Python
adaptive_rho: int

adaptive_rho_interval property writable

Python
adaptive_rho_interval: int

adaptive_rho_fraction property writable

Python
adaptive_rho_fraction: float

adaptive_rho_tolerance property writable

Python
adaptive_rho_tolerance: float

max_iter property writable

Python
max_iter: int

eps_abs property writable

Python
eps_abs: float

eps_rel property writable

Python
eps_rel: float

eps_prim_inf property writable

Python
eps_prim_inf: float

eps_dual_inf property writable

Python
eps_dual_inf: float

scaled_termination property writable

Python
scaled_termination: int

check_termination property writable

Python
check_termination: int

check_dualgap property writable

Python
check_dualgap: int

time_limit property writable

Python
time_limit: float

delta property writable

Python
delta: float

polish_refine_iter property writable

Python
polish_refine_iter: int

__hash__ class-attribute instance-attribute

Python
__hash__: None = None

__init__

Python
__init__() -> None

OSQP's own defaults (osqp_set_default_settings). TrajOptOSQPSolverProfile().settings holds trajopt's defaults instead.

__eq__

Python
__eq__(arg: OSQPSettings) -> bool

__ne__

Python
__ne__(arg: OSQPSettings) -> bool

TrajOptOSQPSolverProfile

Bases: TrajOptSolverProfile

settings property writable

Python
settings: OSQPSettings

OSQP settings; trajopt's defaults (sco::OSQPModelConfig::setDefaultOSQPSettings)

update_workspace property writable

Python
update_workspace: bool

Update the OSQP workspace for subsequent optimizations, instead of recreating it each time

__hash__ class-attribute instance-attribute

Python
__hash__: None = None

__init__

Python
__init__() -> None

__eq__

Python
__eq__(arg: TrajOptOSQPSolverProfile) -> bool

__ne__

Python
__ne__(arg: TrajOptOSQPSolverProfile) -> bool

from_yaml staticmethod

Python
from_yaml(config: str) -> TrajOptOSQPSolverProfile

Construct the profile from its YAML config text; malformed YAML raises ValueError.

settings: and opt_params: are merged into the profile's values: keys left out keep them (settings: {max_iter: 321} keeps eps_abs 1e-4). This is per struct; other profiles' from_yaml may replace a nested sub-config instead.

TrajOptMotionPlanner

Bases: MotionPlanner

__init__

Python
__init__(name: str) -> None

ProfileDictionary_addTrajOptMoveProfile

Python
ProfileDictionary_addTrajOptMoveProfile(dict: ProfileDictionary, ns: str, profile_name: str, profile: TrajOptMoveProfile) -> None

Add TrajOpt move profile to ProfileDictionary

ProfileDictionary_addTrajOptPlanProfile

Python
ProfileDictionary_addTrajOptPlanProfile(dict: ProfileDictionary, ns: str, profile_name: str, profile: TrajOptMoveProfile) -> None

Add TrajOpt plan profile to ProfileDictionary (legacy alias)

ProfileDictionary_addTrajOptCompositeProfile

Python
ProfileDictionary_addTrajOptCompositeProfile(dict: ProfileDictionary, ns: str, profile_name: str, profile: TrajOptCompositeProfile) -> None

Add TrajOpt composite profile to ProfileDictionary

ProfileDictionary_addTrajOptSolverProfile

Python
ProfileDictionary_addTrajOptSolverProfile(dict: ProfileDictionary, ns: str, profile_name: str, profile: TrajOptSolverProfile) -> None

Add TrajOpt solver profile to ProfileDictionary