Skip to content

Online planning

Two examples replan a gantry robot's trajectory while a person walks up to its arm. They plan the same task. online_planning_sqp_example follows tesseract_planning's C++ online_planning_example.cpp: it builds one TrajOptQPProblem and, every tick, moves the person and re-solves it with TrustRegionSQPSolver, starting from the previous solution. online_planning_example does the same job through the task composer, running a whole pipeline per plan.

The task

Both examples take their task from online_planning_scene.py. The robot runs 5.5 m down the cell on the gantry's x axis, its arm at zero at both ends and pointing across the cell, towards the person. The person, a cylinder 0.5 m in radius and 2 m tall on two prismatic joints, walks 0.8 m straight towards the arm and back at 1.3 m/s, a comfortable adult walking speed, halfway down the robot's run; it never crosses the run. One replan every 0.1 s, the spacing of the C++ example's planned waypoints, so a lap takes 12 ticks; the examples run four laps. The person stays in the arm's way: the straight plan would come within the margin on 9 ticks of every 12, and at the near end run the arm 0.39 m into the person. In the C++ example someone drags the person's joints by hand, and the target also moves the gantry 3 m across the cell.

To generate more impressive arm motion, gantry steps cost more than an arm step. Each joint's velocity cost is weighted by the inverse of its velocity limit (velocity_weights). The URDF rates the gantry at 0.2 and 0.5 m/s against 2.6 to 7.9 rad/s for the arm.

Overview

Each tick of the loop:

sequenceDiagram
    participant Sensor
    participant Planner
    participant Controller

    loop Every control cycle
        Sensor->>Planner: Obstacle position
        Planner->>Planner: Update environment
        Planner->>Planner: Replan trajectory
        Planner->>Controller: New waypoint
        Controller->>Controller: Execute
    end

Performance comparison

Method Use Case
Task Composer Moderate scene changes; each plan runs a whole pipeline
Low-Level SQP (discrete) Replanning from the last solution; collisions checked at the waypoints
Low-Level SQP (LVS continuous) Swept collision between waypoints; slower per step

The reference online_planning_sqp_example.py prints the mean replan step after the initial solve on its 8-DOF gantry workcell, for each mode. Run it to get numbers for your machine and build; no rate or latency bound is claimed here.

Low-level SQP example

Replanning with 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 argparse
import sys
import threading
import time

import numpy as np

# Low-level SQP imports
from tesseract_robotics import get_tesseract_resource_path
from tesseract_robotics import trajopt_ifopt as ti
from tesseract_robotics import trajopt_sqp as tsqp
from tesseract_robotics.collision import CollisionEvaluatorType, ContactTestType
from tesseract_robotics.common import GeneralResourceLocator
from tesseract_robotics.examples import online_planning_scene as scene
from tesseract_robotics.planning import Robot

Problem construction

The problem is a TrajOptQPProblem, built as tesseract_planning's online_planning_example.cpp builds it, with the target as joints. The start and target joints and one collision constraint per step are constraint sets, and the joint velocity is a squared cost, weighted per joint. Continuous mode sets the collision check type to LVS_DISCRETE, which its evaluator requires; the C++ reference sets DISCRETE there and throws. Each collision constraint keeps 10 contacts as rows, not the C++ default 3, because several of the arm's links can touch the person and the floor at once.

online_planning_sqp_example.py (problem)
def build_optimization_problem(
    robot,
    joint_names,
    start_pos,
    target_pos,
    steps=10,
    use_continuous_collision=True,
    extra_constraints=None,
    initial_trajectory=None,
):
    """Build the TrajOptQPProblem online_planning_example.cpp builds, with a joint target.

    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.
        extra_constraints: Optional callable (vars_list, manip, env) -> constraint sets,
                           added before setup(), e.g. Python-defined planning rules.
        initial_trajectory: `(steps, joints)` seed, e.g. the last solution; default the straight
                            interpolation from start to target. The C++ setupProblem seeds the
                            variables this way, when it creates them. `NodesVariables.
                            setVariables` after setup() only writes the values: the collision
                            constraints keep evaluating the interpolation, and the solver
                            reports convergence without moving, the person inside the margin.

    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

    # Seed: the given trajectory, or the straight interpolation
    if initial_trajectory is None:
        initial_states = ti.interpolate(start_pos, target_pos, steps)
    else:
        initial_states = [np.asarray(row, dtype=np.float64) for row in initial_trajectory]
    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)
    joint_coeffs = np.ones(len(joint_names)) * 5.0
    home_constraint = ti.JointPosConstraint(start_pos, vars_list[0], joint_coeffs, "Home_Position")
    problem.addConstraintSet(home_constraint)

    # Add target position constraint (last waypoint = target joints), as the start's
    target_constraint = ti.JointPosConstraint(
        target_pos, vars_list[-1], joint_coeffs, "Target_Position"
    )
    problem.addConstraintSet(target_constraint)

    # Add velocity cost (smooth motion), squared: weight times squared step, per joint, a
    # gantry step costing more than an arm step
    vel_target = np.zeros(len(joint_names))
    vel_weights = scene.velocity_weights(robot)
    vel_constraint = ti.JointVelConstraint(vel_target, vars_list, vel_weights, "JointVelocity")
    problem.addCostSet(vel_constraint, tsqp.CostPenaltyType.SQUARED)

    # Add collision constraints, configured as the C++ reference configures them
    margin = scene.COLLISION_MARGIN_M  # 0.1 m safety margin
    margin_coeff = 10.0
    collision_config = ti.TrajOptCollisionConfig(margin, margin_coeff)
    # More rows than the C++ default 3: several arm links can be in contact at once
    collision_config.max_num_cnt = COLLISION_ROWS_PER_WAYPOINT
    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)

    # Extra constraint sets, e.g. Python-defined rules (python_constraints.PyConstraint)
    extra_sets = list(extra_constraints(vars_list, manip, robot.env)) if extra_constraints else []
    for extra in extra_sets:
        problem.addConstraintSet(extra)

    # 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,
        "extra_sets": extra_sets,
    }

SQP loop

initial_solve solves the first plan from scratch. Then, every tick, run moves the person one step along its walk and calls replan, which rebuilds the problem seeded with the last plan and steps the solver from there.

online_planning_sqp_example.py (sqp_loop)
def initial_solve(problem, verbose=False):
    """Create the SQP solver, and solve `problem` from scratch with it, before the first tick.

    Returns:
        The solver, for `replan` to step each tick

    Raises:
        SolverDidNotConvergeError: the solve did not end in `SQPStatus.CONVERGED`.
    """
    qp_solver = tsqp.OSQPEigenSolver()
    solver = tsqp.TrustRegionSQPSolver(qp_solver)
    solver.verbose = verbose
    solver.params.initial_trust_box_size = TRUST_BOX_SIZE
    solver.solve(problem)
    if solver.getStatus() != tsqp.SQPStatus.CONVERGED:
        raise SolverDidNotConvergeError(
            f"initial solve ended {tsqp.toString(solver.getStatus())}, expected SQP_CONVERGED"
        )
    return solver


def replan(
    solver,
    robot,
    joint_names,
    plan,
    target_pos,
    *,
    use_continuous_collision=False,
    extra_constraints=None,
    sqp_steps_per_tick=SQP_STEPS_PER_TICK,
):
    """One tick: rebuild the problem seeded with `plan`, the last solution, and step from it.

    The start stays pinned at `plan[0]`. Takes SQP steps until one converges, at most
    `sqp_steps_per_tick`; the solver leaves the new problem's variables at its best values.

    Returns:
        The new problem's data (see build_optimization_problem): keep it while the plan is used
    """
    problem_data = build_optimization_problem(
        robot,
        joint_names,
        plan[0],
        target_pos,
        len(plan),
        use_continuous_collision,
        extra_constraints,
        initial_trajectory=plan,
    )
    # Incremental optimization: a few SQP steps, from the seed
    solver.init(problem_data["problem"])
    for _ in range(sqp_steps_per_tick):
        converged = solver.stepSQPSolver()
        # Reset box size (trust region loop can shrink it to zero)
        solver.setBoxSize(TRUST_BOX_SIZE)
        if converged:
            break
    return problem_data

Run the complete example:

Bash
tesseract_online_planning_sqp_example
Expected output (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=35.9975, time=<ms>
Online replanning (48 iterations)...
Replan timing: avg=<ms> (<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 prints zero counts and uninitialized bounds there.

Move the person yourself

Bash
tesseract_online_planning_sqp_example --sliders

With --sliders the robot replans live, once per 0.1 s tick, while you move the person from the viewer's control panel.

online_planning_sqp_example.py (sliders)
def drive_with_slider(viewer, robot, joint_names, solver, plan, stop: threading.Event) -> None:
    """Replan once a tick with the person where its slider sends it, until `stop` is set.

    The slider runs along the person's walk, from its far end towards the arm. The person walks
    there at walking speed, one tick's step at a time, and the robot sweeps along the plan in
    force, a waypoint per tick, as the replay shows it.

    Args:
        viewer: A `TesseractViewer` showing `robot.env`.
        robot, joint_names: From `start_cell`.
        solver: From `initial_solve`, which solved `plan`.
        plan: The initial `(steps, joints)` plan, with the person at the walk's far end.
        stop: Set to end the loop.
    """
    gui = viewer.server.gui
    walk_m = scene.WALK_FAR_Y_M - scene.WALK_NEAR_Y_M
    person = gui.add_slider(
        "Person, towards the arm [m]",
        min=0.0,
        max=walk_m,
        step=PERSON_SLIDER_STEP_M,
        initial_value=0.0,
    )
    status = gui.add_text("Replan", initial_value="", disabled=True)
    target_pos = np.array(scene.TARGET_JOINTS)
    names = [*joint_names, *scene.HUMAN_JOINTS]
    y_m = scene.WALK_FAR_Y_M
    tick = 0
    while not stop.is_set():
        started = time.perf_counter()
        y_m = scene.walk_towards(y_m, scene.WALK_FAR_Y_M - float(person.value))
        position = (scene.WALK_X_M, y_m)
        scene.place_person(robot.env, position)
        problem_data = replan(solver, robot, joint_names, plan, target_pos)
        plan = ti.toTrajArray(problem_data["vars_list"])
        replan_ms = (time.perf_counter() - started) * MS_PER_S
        waypoint = scene.sweep_waypoint(tick, len(plan))
        viewer.update_joint_positions(names, [*plan[waypoint], *scene.human_joints_at(position)])
        status.value = f"{replan_ms:.0f} ms of the {scene.TICK_S * MS_PER_S:.0f} ms tick"
        tick += 1
        stop.wait(max(0.0, scene.TICK_S - (time.perf_counter() - started)))

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)] * len(joint_names)  # one Bounds per joint
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, rebuild the problem with the previous trajectory as the variables' initial values, as the C++ setupProblem does:

Python
from tesseract_robotics.trajopt_ifopt import createNodesVariables

seeded = createNodesVariables("trajectory", joint_names, list(trajectory), bounds)
# ... a new TrajOptQPProblem(seeded) with the constraint and cost sets, setup(), then:
solver.init(problem)
solver.stepSQPSolver()

Writing the values into an existing problem with NodesVariables.setVariables is not the same: it only writes the values, and the collision constraints keep evaluating the trajectory they were set up with. The solver then reports convergence without moving the plan.

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. The C++ example takes one step per loop, as fast as the solver runs. At a 0.1 s tick and walking speed one step of the 0.01 trust box runs the arm into the person, so the example takes up to SQP_STEPS_PER_TICK (10) steps a tick and stops at the first that converges.

5. Sparse collision checking

Check collision every N waypoints for speed:

Python
from tesseract_robotics.trajopt_ifopt import DiscreteCollisionConstraint

# Every other waypoint (faster)
for i in range(0, n_steps, 2):
    collision = DiscreteCollisionConstraint(evaluator, vars_list[i], name=f"collision_{i}")

# Every waypoint (slower, safer)
for i in range(n_steps):
    collision = DiscreteCollisionConstraint(evaluator, vars_list[i], name=f"collision_{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), and LVSContinuousCollisionEvaluator needs LVS_CONTINUOUS or CONTINUOUS:

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

from tesseract_robotics.trajopt_sqp import TrajOptQPProblem

problem = TrajOptQPProblem(nodes_variables)  # over the variables built above
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,
        vars_list[i], vars_list[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, Trajectory

# Rows of joint values plus a time column
dt = 0.1
rows = [wp.tolist() + [i * dt] for i, wp in enumerate(trajectory)]

with TesseractViewer() as viewer:
    viewer.update_environment(robot.env)
    viewer.update_trajectory(Trajectory.from_rows(joint_names, rows))
    print(f"Viewer at {viewer.url}")
    input("Press Enter to exit...")

Task composer alternative

online_planning_example.py replans through the task composer instead: each plan runs a whole pipeline, with profiles and the high-level API, at the cost of rebuilding the problem every time. See online_planning_example.py:

Bash
tesseract_online_planning_example

Running the examples

Bash
# Low-level SQP (discrete collision)
tesseract_online_planning_sqp_example

# The same, moving the person yourself with a slider
tesseract_online_planning_sqp_example --sliders

# 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_iter = 5

Better quality

Python
# More waypoints
n_steps = 20

# Continuous collision
# (use ContinuousCollisionConstraint + LVSContinuousCollisionEvaluator)

# More iterations
solver.params.max_iter = 50

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

Next steps