Low-Level SQP API¶

Real-time trajectory optimization via Sequential Quadratic Programming. Use this when you need sub-10ms optimizer steps for online replanning — lower-level than TaskComposer, higher-control than plan_cartesian.

When to reach for this¶

Situation	Use
Offline, one-shot plan	`plan_freespace` / `plan_cartesian`
Multi-stage pipeline (sampling → optimize → time-param)	`TaskComposer`
Online replanning with moving obstacle, ~100 Hz	Low-level SQP (this page)

Measured step rates for a reference 8-DOF gantry problem are printed at runtime by online_planning_sqp_example.py — run it locally to see numbers on your machine.

Modules¶

Module	Purpose
`tesseract_robotics.trajopt_ifopt`	Variables (`Var`, `Node`, `NodesVariables`), constraints (joint, Cartesian, collision), factory helpers
`tesseract_robotics.trajopt_sqp`	`IfoptProblem` (NLP), `IfoptQPProblem` (QP wrapper), `TrustRegionSQPSolver`, `OSQPEigenSolver`

The standalone tesseract_robotics.ifopt module was removed in 0.34. All types merged into tesseract_robotics.trajopt_ifopt. See the 0.33 → 0.34 migration guide for details.

Variable Hierarchy (0.34+)¶

The JointPosition class is gone. Variables now use a three-level hierarchy:

Var — a single variable (e.g., joint values at one waypoint)
Node — groups multiple Vars (e.g., joints + velocities at one waypoint)
NodesVariables — container of Nodes passed to the problem constructor

Build the full hierarchy with createNodesVariables:

Python

from tesseract_robotics.trajopt_ifopt import Bounds, createNodesVariables

bounds = Bounds(-3.14, 3.14)
nodes_variables = createNodesVariables(
    "trajectory", joint_names, initial_states, bounds,
)

Get Var references for passing to constraints:

Python

vars_list = [node.getVar("joints") for node in nodes_variables.getNodes()]

Problem Setup¶

Python

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

Constraints and Costs¶

The build_optimization_problem function shows the full 0.34 pattern — NodesVariables factory, IfoptProblem (NLP) for joint/Cartesian constraints, then IfoptQPProblem(nlp) wrapping for collision constraints, followed by problem.setup():

Python

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,
    }

Available constraint types in tesseract_robotics.trajopt_ifopt:

Constraint	Purpose
`JointPosConstraint`	Target joint values at specific waypoints
`JointVelConstraint`	Velocity limits across waypoints
`JointAccelConstraint`	Acceleration limits
`JointJerkConstraint`	Jerk (3^rd derivative) limits — smoother motion
`CartPosConstraint`	TCP pose at a waypoint
`CartLineConstraint`	TCP on a line segment
`DiscreteCollisionConstraint`	Collision at single timesteps
`DiscreteCollisionNumericalConstraint`	Alt. collision jacobian
`ContinuousCollisionConstraint`	Collision across segments (LVS)
`InverseKinematicsConstraint`	IK-based optimization

Available collision evaluators:

Evaluator	Pairs with
`SingleTimestepCollisionEvaluator`	`DiscreteCollisionConstraint`
`LVSDiscreteCollisionEvaluator`	`ContinuousCollisionConstraint` (LVS discrete)
`LVSContinuousCollisionEvaluator`	`ContinuousCollisionConstraint` (LVS continuous)

SQP Solver Loop¶

The solver loop: initial global solve, then per-tick setVariables + stepSQPSolver for warm-started incremental optimization. setBoxSize resets the trust region each tick (trust-region shrinking can drive the box to zero):

Python

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

Warm-Starting¶

Between solver steps, update NodesVariables with the previous solution:

Python

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

The example above rebuilds the problem each tick because the obstacle pose is baked into collision constraints. If your scene is static, you can reuse the same problem and only call setVariables + stepSQPSolver.

Python Subclassing¶

You can subclass ConstraintSet from Python to define custom constraints — the trampoline landed in 0.34.1.1 (see the CHANGELOG). scipy is required at runtime because nanobind's sparse-matrix conversion goes through scipy.sparse.

Migrating from 0.33¶

If you have 0.33 SQP code, see the full migration guide — critical items:

from tesseract_robotics import ifopt → from tesseract_robotics import trajopt_ifopt
JointPosition(...) → createNodesVariables(...) + Var refs
IfoptQPProblem() → IfoptProblem(nodes_variables) + IfoptQPProblem(nlp)
CartPosInfo struct removed — CartPosConstraint takes args directly
CollisionCache removed — caching is internal
getTotalExactCost() / getExactCosts() take no arguments
getStaticKey() → getKey() (instance method on profiles)
Call problem.setup() after adding all constraint/cost sets

Low-Level SQP API¶

When to reach for this¶

Modules¶

Variable Hierarchy (0.34+)¶

Problem Setup¶

Constraints and Costs¶

SQP Solver Loop¶

Warm-Starting¶

Python Subclassing¶

Migrating from 0.33¶

See also¶