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)	73 Hz	Fast replanning
Low-Level SQP (no collision)	128 Hz	Safe environments
Low-Level SQP (LVS continuous)	5-10 Hz	High precision

Numbers are from the reference online_planning_sqp_example.py on an 8-DOF gantry workcell. Run it on your machine to get local numbers.

Low-Level SQP Example

Real-time replanning at ~73 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_common import Isometry3d

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

Problem construction

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 IFOPT optimization problem using 0.34 Var/Node API.

    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
    )
    nlp = tsqp.IfoptProblem(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")
    nlp.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",
    )
    nlp.addConstraintSet(target_constraint)

    # Add velocity cost (smooth motion)
    vel_target = np.zeros(len(joint_names))
    vel_constraint = ti.JointVelConstraint(vel_target, vars_list, np.ones(1), "JointVelocity")
    nlp.addCostSet(vel_constraint)

    # Create QP problem from NLP
    problem = tsqp.IfoptQPProblem(nlp)

    # Add collision constraints
    margin = 0.1  # 10cm safety margin
    margin_coeff = 10.0
    collision_config = ti.TrajOptCollisionConfig(margin, margin_coeff)
    collision_config.collision_margin_buffer = 0.10
    collision_evaluators = []
    collision_constraints = []

    if use_continuous_collision:
        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],
                False,
                False,
                1,
                False,
                f"LVSCollision_{i - 1}_{i}",
            )
            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], 1, False, f"Collision_{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,
        "nlp": nlp,
        "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)...")

    n_joints = len(joint_names)

    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)

        # Extract joint values from solution
        joint_vals = x[: steps * n_joints]
        traj = joint_vals.reshape(steps, n_joints)

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

Text Only

Performance: ~13ms avg (~73 Hz discrete / ~128 Hz no-collision)

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

For higher precision (5-10 Hz) use the LVS continuous evaluator with ContinuousCollisionConstraint:

Python

from tesseract_robotics.trajopt_ifopt import (
    LVSContinuousCollisionEvaluator, ContinuousCollisionConstraint
)

evaluator = LVSContinuousCollisionEvaluator(
    manip, env, config,
    dynamic_environment=True,
)

# Add constraint between consecutive waypoints
for i in range(n_steps - 1):
    constraint = ContinuousCollisionConstraint(
        evaluator,
        variables[i], variables[i + 1],
        fixed0=False, fixed1=False,
        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 (~73 Hz discrete)
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

• Low-Level SQP Guide - Full API reference
• Collision Detection - Collision configuration