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¶
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.
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.
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:
Expected output (the timings vary by machine)
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¶
With --sliders the robot replans live, once per 0.1 s tick, while you move the person from the viewer's control panel.
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:
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:
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:
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 toSQP_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:
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:
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:
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:
Running the examples¶
# 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:
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¶
# 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¶
# 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¶
- Low-Level SQP Guide - Full API reference
- Collision Detection - Collision configuration