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,  # 0.33 API
    ProfileDictionary_addTrajOptPlanProfile,
    ProfileDictionary_addTrajOptCompositeProfile,
)
from tesseract_robotics.tesseract_collision import CollisionEvaluatorType

TrajOptMotionPlanner

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 (0.33 API)
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 (0.33 API: TrajOptCollisionConfig)
profile.collision_cost_config.enabled = True
profile.collision_cost_config.collision_margin_buffer = 0.025  # was safety_margin
profile.collision_cost_config.collision_coeff_data.setDefaultCollisionCoeff(20.0)  # was .coeff
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)

Collision Configuration (0.33 API)

TrajOptCollisionConfig

Replaces the old CollisionCostConfig and CollisionConstraintConfig.

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  # additional buffer beyond margin
config.collision_check_config.type = CollisionEvaluatorType.DISCRETE
config.collision_check_config.longest_valid_segment_length = 0.05  # for LVS modes

CollisionEvaluatorType

Module Location

CollisionEvaluatorType moved to tesseract_collision in 0.33 API.

Type Description Speed
DISCRETE Check at waypoints only (was SINGLE_TIMESTEP) Fast
LVS_DISCRETE Interpolate between waypoints Medium
LVS_CONTINUOUS Swept volume check (was DISCRETE_CONTINUOUS) 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)

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.collision_margin_buffer = 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)

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 (0.33 API)
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.collision_margin_buffer = 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 collision_margin_buffer if collisions occur
  4. Use LVS_DISCRETE for better collision coverage
  5. Tune coefficients - higher = stricter enforcement

Auto-generated API Reference

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=6, order=C)]

upper_tolerance property writable

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

coeff property writable

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

__init__

Python
__init__() -> None

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)]

__init__

Python
__init__() -> None

TrajOptMoveProfile

Bases: Profile

getKey

Python
getKey() -> int

getStaticKey staticmethod

Python
getStaticKey() -> int

TrajOptCompositeProfile

Bases: Profile

getKey

Python
getKey() -> int

getStaticKey staticmethod

Python
getStaticKey() -> 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

__init__

Python
__init__() -> None

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

__init__

Python
__init__() -> None

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

__init__

Python
__init__() -> None

TrajOptSolverProfile

Bases: Profile

opt_params property writable

Python
opt_params: BasicTrustRegionSQPParameters

Optimization parameters

getKey

Python
getKey() -> int

getStaticKey staticmethod

Python
getStaticKey() -> int

TrajOptOSQPSolverProfile

Bases: TrajOptSolverProfile

update_workspace property writable

Python
update_workspace: bool

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

__init__

Python
__init__() -> None

TrajOptMotionPlanner

__init__

Python
__init__(name: str) -> None

getName

Python
getName() -> str

solve

Python
solve(request: 'tesseract_planning::PlannerRequest') -> 'tesseract_planning::PlannerResponse'

terminate

Python
terminate() -> bool

clear

Python
clear() -> 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