Skip to content

Online Planning

Real-time trajectory replanning with moving obstacles.

Overview

Online planning continuously replans as the environment changes:

sequenceDiagram
    participant Sensor
    participant Planner
    participant Controller

    loop Every 10-15ms
        Sensor->>Planner: Obstacle position
        Planner->>Planner: Update environment
        Planner->>Planner: Replan trajectory
        Planner->>Controller: New waypoint
        Controller->>Controller: Execute
    end

Performance Comparison

Method Rate Use Case
Task Composer 1-5 Hz Moderate scene changes
Low-Level SQP (discrete) ~70 Hz (14 ms per replan) Fast replanning
Low-Level SQP (LVS continuous) ~20 Hz (52 ms per replan) Swept collision between waypoints

The low-level SQP rates come from the reference online_planning_sqp_example.py on its 8-DOF gantry workcell: the median over three runs of the mean replan step after the initial solve, on an Apple M1 Max with trajopt 0.35.0. The machine was shared at the time, so treat them as indicative, and run the example to get local numbers.

Low-Level SQP Example

Real-time replanning at about 70 Hz using discrete collision. The snippets below are the real shipped source, split into three regions: setup, problem, sqp_loop. For the accompanying conceptual guide see the Low-Level SQP API page.

Setup

online_planning_sqp_example.py (setup)
import sys
import time

import numpy as np

# Low-level SQP imports
from tesseract_robotics import trajopt_ifopt as ti
from tesseract_robotics import trajopt_sqp as tsqp
from tesseract_robotics.planning import Robot
from tesseract_robotics.tesseract_collision import CollisionEvaluatorType, ContactTestType
from tesseract_robotics.tesseract_common import Isometry3d

TesseractViewer = None
if "pytest" not in sys.modules:
    from tesseract_robotics.viewer import TesseractViewer

Problem construction

The problem is a TrajOptQPProblem, built as tesseract_planning's online_planning_example.cpp builds it. The start and target poses and one collision constraint per step are constraint sets, and the joint velocity is a squared cost. Continuous mode sets the collision check type to LVS_DISCRETE, which its evaluator requires; the C++ reference sets DISCRETE there and throws.

online_planning_sqp_example.py (problem)
def build_optimization_problem(
    robot, joint_names, start_pos, target_pos, steps=10, use_continuous_collision=True
):
    """Build the TrajOptQPProblem that online_planning_example.cpp builds.

    Args:
        robot: Robot instance with environment
        joint_names: List of joint names
        start_pos: Starting joint positions
        target_pos: Target joint positions
        steps: Number of trajectory waypoints
        use_continuous_collision: If True, use LVS continuous collision.
                                  If False, use discrete single-timestep collision.

    Returns:
        dict with problem and all objects that must stay alive
    """
    # Get kinematic group and joint limits
    manip = robot.env.getKinematicGroup("manipulator")
    joint_limits = manip.getLimits().joint_limits

    # Interpolate initial trajectory
    initial_states = ti.interpolate(start_pos, target_pos, steps)
    bounds = ti.toBounds(joint_limits)

    # Create NodesVariables — factory handles Node/Var creation on C++ side
    nodes_variables = ti.createNodesVariables(
        "trajectory", list(joint_names), list(initial_states), bounds
    )
    # One layer: constraint and cost sets go straight into the QP problem
    problem = tsqp.TrajOptQPProblem(nodes_variables)

    # Get Var references for constraints
    vars_list = []
    for node in nodes_variables.getNodes():
        vars_list.append(node.getVar("joints"))

    # Add start position constraint (first waypoint = current position)
    home_coeffs = np.ones(len(joint_names)) * 5.0
    home_constraint = ti.JointPosConstraint(start_pos, vars_list[0], home_coeffs, "Home_Position")
    problem.addConstraintSet(home_constraint)

    # Add target pose constraint (last waypoint = target in Cartesian space)
    target_tf = manip.calcFwdKin(target_pos)["tool0"]
    target_constraint = ti.CartPosConstraint(
        vars_list[-1],
        manip,
        "tool0",  # source_frame
        "world",  # target_frame
        Isometry3d.Identity(),  # source_frame_offset
        target_tf,  # target_frame_offset
        "Target_Pose",
    )
    problem.addConstraintSet(target_constraint)

    # Add velocity cost (smooth motion): squared, which needs the set's equality bounds
    vel_target = np.zeros(len(joint_names))
    vel_constraint = ti.JointVelConstraint(vel_target, vars_list, np.ones(1), "JointVelocity")
    problem.addCostSet(vel_constraint, tsqp.CostPenaltyType.SQUARED)

    # Add collision constraints, configured as the C++ reference configures them
    margin = 0.1  # 10cm safety margin
    margin_coeff = 10.0
    collision_config = ti.TrajOptCollisionConfig(margin, margin_coeff)
    collision_config.collision_check_config.contact_request.type = ContactTestType.ALL
    # LVSDiscreteCollisionEvaluator rejects any check type but LVS_DISCRETE. The C++ reference
    # sets DISCRETE for both modes, so its continuous mode throws on trajopt 0.35.0.
    collision_config.collision_check_config.type = (
        CollisionEvaluatorType.LVS_DISCRETE
        if use_continuous_collision
        else CollisionEvaluatorType.DISCRETE
    )
    collision_config.collision_margin_buffer = 0.10
    collision_evaluators = []
    collision_constraints = []

    if use_continuous_collision:
        # The first segment starts at the pinned current state, so its start is fixed
        fixed_start = True
        for i in range(1, steps):
            collision_evaluator = ti.LVSDiscreteCollisionEvaluator(
                manip,
                robot.env,
                collision_config,
                True,
            )
            collision_constraint = ti.ContinuousCollisionConstraint(
                collision_evaluator,
                vars_list[i - 1],
                vars_list[i],
                fixed_start,  # position_var0 fixed
                False,  # position_var1 fixed
                collision_config.max_num_cnt,
                False,  # fixed_sparsity
                f"LVSDiscreteCollision_{i}",
            )
            fixed_start = False
            problem.addConstraintSet(collision_constraint)
            collision_evaluators.append(collision_evaluator)
            collision_constraints.append(collision_constraint)
    else:
        for i in range(1, steps):
            collision_evaluator = ti.SingleTimestepCollisionEvaluator(
                manip,
                robot.env,
                collision_config,
                True,
            )
            collision_constraint = ti.DiscreteCollisionConstraint(
                collision_evaluator,
                vars_list[i],
                collision_config.max_num_cnt,
                False,  # fixed_sparsity
                f"SingleTimestepCollision_{i}",
            )
            problem.addConstraintSet(collision_constraint)
            collision_evaluators.append(collision_evaluator)
            collision_constraints.append(collision_constraint)

    # Setup the problem (must be called after adding all sets)
    problem.setup()

    return {
        "problem": problem,
        "nodes_variables": nodes_variables,
        "vars_list": vars_list,
        "target_constraint": target_constraint,
        "home_constraint": home_constraint,
        "vel_constraint": vel_constraint,
        "collision_config": collision_config,
        "collision_evaluators": collision_evaluators,
        "collision_constraints": collision_constraints,
        "manip": manip,
    }

SQP loop

online_planning_sqp_example.py (sqp_loop)
    # Create SQP solver with OSQP
    # Match C++ defaults: box_size=0.01
    box_size = 0.01
    qp_solver = tsqp.OSQPEigenSolver()
    solver = tsqp.TrustRegionSQPSolver(qp_solver)
    solver.verbose = verbose
    solver.params.initial_trust_box_size = box_size

    # Initial global solve
    print("Running initial global solve...")
    t0 = time.perf_counter()
    solver.solve(problem)
    solve_time = time.perf_counter() - t0
    x = solver.getResults().best_var_vals
    cost = problem.getTotalExactCost()
    print(f"Initial solve: cost={cost:.4f}, time={solve_time * 1000:.1f}ms")

    trajectories = [x.copy()]
    timings = [solve_time]

    # Online replanning loop
    num_replan = 10
    print(f"\nOnline replanning ({num_replan} iterations)...")

    for iteration in range(num_replan):
        # Move obstacle
        human_x = 0.5 + 0.3 * np.sin(iteration * 0.3)
        update_human_position(robot, human_x, 0.0)

        # The solver leaves the problem's variables at best_var_vals: one row per waypoint
        traj = ti.toTrajArray(problem_data["vars_list"])

        # Rebuild problem with warm-start
        t0 = time.perf_counter()
        problem_data = build_optimization_problem(
            robot,
            joint_names,
            traj[0],
            target_pos,
            steps,
            use_continuous_collision,
        )
        problem = problem_data["problem"]

        # Set warm-start values — flatten trajectory into decision vector
        problem_data["nodes_variables"].setVariables(traj.flatten())

        # Single SQP step (incremental optimization)
        solver.init(problem)
        solver.stepSQPSolver()

        # Reset box size (trust region loop can shrink it to zero)
        solver.setBoxSize(box_size)

        x = solver.getResults().best_var_vals
        dt = time.perf_counter() - t0
        timings.append(dt)
        trajectories.append(x.copy())

        if verbose:
            cost = problem.getTotalExactCost()
            print(f"  Iter {iteration + 1}: cost={cost:.4f}, time={dt * 1000:.1f}ms")

Run the complete example:

Bash
tesseract_online_planning_sqp_example
Expected output (indicative; the timings vary by machine)
Text Only
Manipulator joints (8): ['gantry_axis_1', 'gantry_axis_2', 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
Building optimization problem (collision: discrete)...
Running initial global solve...
Initial solve: cost=3.9284, time=31.5ms
Online replanning (10 iterations)...
Replan timing: avg=14.7ms (68 Hz)
Trajectory: 12 waypoints x 8 joints

Between the second and third line, problem.print() dumps the problem's state, as the C++ reference does. Before the first solve, trajopt 0.35.0 prints zero counts and uninitialized bounds there.

Key Concepts

1. Variable hierarchy (0.34+)

Use createNodesVariables to build NodesVariables → Node → Var:

Python
from tesseract_robotics.trajopt_ifopt import Bounds, createNodesVariables

bounds = Bounds(-3.14, 3.14)
nodes_variables = createNodesVariables(
    "trajectory", joint_names, initial_states, bounds,
)
vars_list = [node.getVar("joints") for node in nodes_variables.getNodes()]

2. Warm-start from the previous solution

Between ticks, push the previous trajectory back into the variables:

Python
nodes_variables.setVariables(trajectory.flatten())
solver.init(problem)
solver.stepSQPSolver()

3. Reset the trust region each tick

stepSQPSolver shrinks the trust region — if you don't reset it, successive ticks get smaller and smaller steps:

Python
solver.stepSQPSolver()
solver.setBoxSize(0.01)  # Reset trust region

4. Single step vs full solve

  • Use solver.solve(problem) on the first tick for global convergence.
  • Use solver.stepSQPSolver() per tick afterwards for incremental optimization.

5. Sparse collision checking

Check collision every N waypoints for speed:

Python
# Every other waypoint (faster)
for i in range(0, n_steps, 2):
    collision = DiscreteCollisionConstraint(evaluator, variables[i], ...)

# Every waypoint (slower, safer)
for i in range(n_steps):
    collision = DiscreteCollisionConstraint(evaluator, variables[i], ...)

Continuous Collision

To check the motion between waypoints, not only the waypoints, use ContinuousCollisionConstraint with an LVS evaluator. Each evaluator requires its own check type in the collision config, and its constructor raises otherwise: LVSDiscreteCollisionEvaluator needs LVS_DISCRETE (the example's continuous mode, about 20 Hz), and LVSContinuousCollisionEvaluator needs LVS_CONTINUOUS or CONTINUOUS:

Python
from tesseract_robotics.tesseract_collision import CollisionEvaluatorType
from tesseract_robotics.trajopt_ifopt import (
    ContinuousCollisionConstraint,
    LVSContinuousCollisionEvaluator,
    TrajOptCollisionConfig,
)

config = TrajOptCollisionConfig(0.1, 10.0)  # margin (m), coefficient
config.collision_check_config.type = CollisionEvaluatorType.LVS_CONTINUOUS

# One constraint per segment between consecutive waypoints. The first segment starts at the
# pinned current state, so its start is fixed.
for i in range(n_steps - 1):
    evaluator = LVSContinuousCollisionEvaluator(manip, env, config, dynamic_environment=True)
    constraint = ContinuousCollisionConstraint(
        evaluator,
        variables[i], variables[i + 1],
        fixed0=(i == 0), fixed1=False,
        max_num_cnt=config.max_num_cnt,
        name=f"cont_collision_{i}",
    )
    problem.addConstraintSet(constraint)

Visualization

Animate a trajectory in the viewer:

Python
from tesseract_robotics.viewer import TesseractViewer

viewer = TesseractViewer()
viewer.update_environment(robot.env, [0, 0, 0])

# Convert trajectory to viewer format (joints + time column)
dt = 0.1
trajectory_list = [wp.tolist() + [i * dt] for i, wp in enumerate(trajectory)]

viewer.update_trajectory_list(joint_names, trajectory_list)
viewer.start_serve_background()

Task Composer Alternative

For slower updates (1-5 Hz) with higher-level abstractions, use Task Composer. See online_planning_example.py:

Bash
tesseract_online_planning_example

Running the Examples

Bash
# Low-level SQP (discrete collision)
tesseract_online_planning_sqp_example

# Task Composer based
tesseract_online_planning_example

Or via module invocation:

Bash
pixi run python -m tesseract_robotics.examples.online_planning_sqp_example
pixi run python -m tesseract_robotics.examples.online_planning_example

Performance Tuning

Faster planning

Python
# Fewer waypoints
n_steps = 8  # Instead of 12

# Fewer collision checks
for i in range(0, n_steps, 3):  # Every third waypoint
    ...

# Smaller trust region (faster convergence)
solver.params.initial_trust_box_size = 0.01

# Fewer iterations per step
solver.params.max_iterations = 5

Better quality

Python
# More waypoints
n_steps = 20

# Continuous collision
# (use ContinuousCollisionConstraint + LVSContinuousCollisionEvaluator)

# More iterations
solver.params.max_iterations = 50

# Tighter convergence
solver.params.min_approx_improve = 1e-5

Next Steps