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`	`TrajOptQPProblem` (the QP problem), `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_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

Constraints and Costs¶

The build_optimization_problem function builds the problem tesseract_planning's online_planning_example.cpp builds: a TrajOptQPProblem over the NodesVariables, the start and target poses and one collision constraint per step as constraint sets, the joint velocity as a squared cost, then problem.setup():

Python

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

Available constraint types in tesseract_robotics.trajopt_ifopt. Each goes in as a constraint or as a cost with a penalty type its row bounds allow; the trajopt_ifopt reference lists the bounds and penalty types per class:

Constraint	Purpose
`JointPosConstraint`	Joint values, or per-joint bounds, at a waypoint
`JointVelConstraint`	Joint velocity toward a target (zero for smoothing)
`JointAccelConstraint`	Joint acceleration toward a target
`JointJerkConstraint`	Joint jerk (3^rd derivative) toward a target — 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. Each needs its own collision check type in the config, and its constructor raises otherwise:

Evaluator	Pairs with	Check type
`SingleTimestepCollisionEvaluator`	`DiscreteCollisionConstraint`	`DISCRETE` (the default)
`LVSDiscreteCollisionEvaluator`	`ContinuousCollisionConstraint` (LVS discrete)	`LVS_DISCRETE`
`LVSContinuousCollisionEvaluator`	`ContinuousCollisionConstraint` (LVS continuous)	`LVS_CONTINUOUS` or `CONTINUOUS`

Per-axis Cartesian constraints¶

CartPosConstraint has two constructors. The short one is an equality on all six axes at unit weight — the TCP is pinned to the target pose. That form cannot express a process constraint, which usually needs one or both of:

a free axis — a rotation the task is indifferent to (about an axis-symmetric tool, say), which must not be pulled back to the target;
an asymmetric bound — a standoff that may open but never close.

The second constructor takes coeffs (per-axis weights) and bounds, both length 6 in [x, y, z, rx, ry, rz] order, ahead of manip. Build the band with toBounds:

Python

import numpy as np
from tesseract_robotics import trajopt_ifopt
from tesseract_robotics.tesseract_common import Isometry3d

#                   x      y        z       rx      ry      rz
lower = np.array([-5e-4, -5e-4, -1.5e-3, -0.087, -0.087, -np.pi])
upper = np.array([ 5e-4,  5e-4,  0.0,     0.087,  0.087,  np.pi])

constraint = trajopt_ifopt.CartPosConstraint(
    var,
    np.array([1.0, 1.0, 1.0, 1.0, 1.0, 0.0]),  # rz free
    trajopt_ifopt.toBounds(lower, upper),
    manip,
    "tool0",
    "base_link",
    Isometry3d.Identity(),
    Isometry3d.Identity(),
    "CartPos",
)

Two semantics are worth knowing before you rely on the row count:

Behaviour	Effect
Zero coefficient	Drops that axis' row entirely — six rows become five. This is how an axis is freed, not merely de-weighted.
`RangeBoundHandling.KEEP_AS_IS`	Each range stays one row with `[lower, upper]` — the form that preserves an asymmetric band as written. `TrajOptQPProblem` takes no range row: `addConstraintSet` and `setup()` accept one, and the first `convexify()` raises `Unsupported bounds type!`.
`RangeBoundHandling.SPLIT_TO_TWO_INEQUALITIES` (default)	Each range row becomes two one-sided rows, `g(x) >= lower` and `g(x) <= upper`, so five constrained axes report ten rows. Equality and already one-sided bounds are unaffected.

Both are prerequisites for adding the term as a cost rather than a hard constraint: as an equality it only ever pulls straight back to the target pose, so a soft Cartesian preference is unexpressible and the free axis has to be given up entirely. TrajOptQPProblem.addCostSet checks every row against the penalty type:

Term	`SQUARED` / `ABSOLUTE`	`HINGE`
Equality on every constrained axis	accepted	raises
Band, split (default): one-sided rows	raises	accepted
Band, `KEEP_AS_IS`: range rows	raises	raises
Equality and band axes mixed	raises	raises

A band is a cost only as a hinge on its split rows. On trajopt 0.35.0, which every tesseract-robotics-nanobind 0.35.0.x wheel bundles (TrajOptQPProblem first ships in 0.35.0.9; earlier releases bind only IfoptQPProblem), the SQP's model reads a HINGE or ABSOLUTE cost as 0 at every QP solution, so it predicts the whole cost away at every step (a violation the trust box cannot cut by a quarter stalls at its seed), and the exact cost ignores the coefficient; trajopt#592 fixes both, unreleased (see the QP problem). Until a wheel is built on a trajopt that contains that fix, a band goes in as a constraint. A term that mixes equality and band axes goes in as a constraint, or as two terms, one per kind (the band term a constraint until then), freeing the other kind's axes with zero coefficients.

Eigen arguments are required

coeffs, source_frame_offset, and target_frame_offset have no Python-side defaults. Eigen default arguments raise std::bad_cast in this module, so pass them explicitly — Isometry3d.Identity() where you want no offset.

Per-joint bounds¶

JointPosConstraint has two constructors. The target one pins every joint of a waypoint to a value. The bounds one takes one Bounds per joint, ahead of the variable: an equality, a one-sided limit, or a range.

Python

import numpy as np
from tesseract_robotics import trajopt_ifopt

bounds = [
    trajopt_ifopt.Bounds(0.0, 0.0),      # joint 0 pinned
    trajopt_ifopt.Bounds(-np.inf, 1.2),  # joint 1 at most 1.2 rad
    trajopt_ifopt.Bounds(-0.5, 0.5),     # joint 2 within a band
]
constraint = trajopt_ifopt.JointPosConstraint(bounds, var, np.array([5.0]), "band")

By default a range becomes two one-sided rows (RangeBoundHandling.SPLIT_TO_TWO_INEQUALITIES), because the QP problems accept only equality and one-sided rows; pass KEEP_AS_IS to keep it as one. A length-1 coeffs weights every row.

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

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

Warm-Starting¶

Between solver steps, set the previous solution through the problem. problem.setVariables writes the variables, then updates every component and recounts the dynamic rows; NodesVariables.setVariables only writes them. A wrong length raises VariablesSizeError:

Python

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

A built problem does not see the environment change. Each collision evaluator keeps its own contact manager, and the pose of every link outside the manipulator is fixed in it when the evaluator is built: env.setState(...) afterwards moves the obstacle in the environment, not in the problem. Measured on the example's scene with the obstacle moved onto the planned path (summed constraint violation, discrete and LVS continuous alike):

Problem	Before the move	After the move
Held from before the move	0.0	0.0
Rebuilt after the move	—	1.92

Perturbing the variables after the move leaves the held problem at 0.002, which is the start-position constraint alone, so this is not the evaluator's cache.

That is why the example rebuilds the problem every tick, as upstream's online_planning_example.cpp (0.35.0, lines 351–357) does. Only when nothing outside the manipulator moves can you hold one problem and call setVariables + stepSQPSolver.

Starting values and failure¶

The example uses upstream's values. They are a starting point for this scene, not tuned guidance:

Term	Kind	Value
`JointPosConstraint` on the first node	constraint	coefficient 5 per joint
`CartPosConstraint` on the last node	constraint	default coefficients
`JointVelConstraint`, zero target	`SQUARED` cost	coefficient 1
Collision (`TrajOptCollisionConfig(0.1, 10)`)	constraint	0.1 m margin, coefficient 10, `collision_margin_buffer` 0.10 m

After solve, solver.getStatus() returns an SQPStatus: NLP_CONVERGED is the only success. ITERATION_LIMIT, PENALTY_ITERATION_LIMIT, OPT_TIME_LIMIT, QP_SOLVER_ERROR and CALLBACK_STOPPED each name why it stopped. A run can stop with constraints still violated, so check problem.getExactConstraintViolations() before using the result.

What the TrajOpt-Ifopt planner builds¶

TrajOptIfoptMotionPlanner builds a TrajOptQPProblem the same way, from its profiles. Each profile's create hook is bound, so Python can see the terms the planner would add without running a plan:

Hook	Returns
`TrajOptIfoptMoveProfile.create(move_instruction, composite_manip_info, env, index)`	`TrajOptIfoptWaypointInfo`: the waypoint's `node`, its `term_infos` and whether it is `fixed`
`TrajOptIfoptCompositeProfile.create(composite_manip_info, env, nodes, fixed_indices)`	`TrajOptIfoptTermInfos` over every waypoint: collision, velocity, acceleration, jerk
`TrajOptIfoptSolverProfile.create(verbose=False)`	the `TrustRegionSQPSolver`, with `opt_params` and `callbacks` applied

TrajOptIfoptTermInfos holds four lists of constraint sets. The planner adds constraints with addConstraintSet and the others with addCostSet and the matching penalty type (SQUARED, ABSOLUTE, HINGE).

Python

from tesseract_robotics.tesseract_motion_planners_trajopt_ifopt import (
    TrajOptIfoptDefaultCompositeProfile,
    TrajOptIfoptDefaultMoveProfile,
)

move_profile = TrajOptIfoptDefaultMoveProfile()
infos = [move_profile.create(move, manip_info, env, i) for i, move in enumerate(moves)]
nodes = [info.node for info in infos]
fixed = [i for i, info in enumerate(infos) if info.fixed]

terms = TrajOptIfoptDefaultCompositeProfile().create(manip_info, env, nodes, fixed)
len(terms.constraints), len(terms.squared_costs), len(terms.hinge_costs)

TrajOptIfoptWaypointInfo owns its Node, so info.node is read-only and keeps info alive. A constraint set from term_infos is shared and outlives the info. The planner moves every node into one NodesVariables; Python cannot, so the nodes a hook returns are for inspection, and a problem you solve yourself is built over createNodesVariables(...).

The planner's term factories¶

The planner builds its terms with five factories, bound under their C++ names. A hand-built problem that uses them gets the planner's coefficients and sparsity choices:

Factory	Builds
`createJointPositionConstraint(joint_waypoint, var, coeffs)`	`JointPosConstraint`: the waypoint's position, or its tolerance band
`createCollisionConstraints(vars, env, manip_info, config, fixed_indices, fixed_sparsity)`	One collision set per unfixed step (`DISCRETE`) or per segment (`LVS_DISCRETE`, `LVS_CONTINUOUS`, `CONTINUOUS`); none for `NONE`
`createJointVelocityConstraint(target, vars, coeffs)`	`JointVelConstraint`
`createJointAccelerationConstraint(target, vars, coeffs)`	`JointAccelConstraint`
`createJointJerkConstraint(target, vars, coeffs)`	`JointJerkConstraint`

Python

import numpy as np
from tesseract_robotics.tesseract_motion_planners_trajopt_ifopt import (
    createCollisionConstraints,
    createJointVelocityConstraint,
)
from tesseract_robotics.trajopt_ifopt import TrajOptCollisionConfig
from tesseract_robotics.trajopt_sqp import CostPenaltyType

vars_list = [node.getVar("joints") for node in nodes_variables.getNodes()]
for constraint in createCollisionConstraints(
    vars_list, env, manip_info, TrajOptCollisionConfig(0.1, 10.0), [0], False
):
    problem.addConstraintSet(constraint)
problem.addCostSet(
    createJointVelocityConstraint(np.zeros(n_joints), vars_list, np.ones(n_joints)),
    CostPenaltyType.SQUARED,
)

An empty vars raises ValueError, a None in it TypeError.

Progress callbacks through the planner¶

A Python SQPCallback set in TrajOptIfoptSolverProfile.callbacks is registered on the solver the planner builds. execute runs once per SQP iteration, with the GIL taken for the call:

Python

from tesseract_robotics.trajopt_sqp import SQPCallback


class Progress(SQPCallback):
    def execute(self, problem, sqp_results):
        print(sqp_results.overall_iteration, sqp_results.best_exact_merit)
        return True


solver_profile = TrajOptIfoptOSQPSolverProfile()
solver_profile.callbacks = [Progress()]  # assign the whole list; it is copied

problem is valid only during the call

The planner owns the problem it passes to execute and destroys it when the solve ends. Read what you need from problem inside the callback; do not store it, or anything taken from it, for use after execute returns.

Returning False does not stop the solve on trajopt 0.35.0

The callback's False reaches the solver, which drops it. In trajopt 0.35.0's trust_region_sqp_solver.cpp, runTrustRegionLoop returns on kStoppedByCallback, but stepSQPSolver does not check that status, so solve keeps stepping until opt_params.max_iterations, and verifySQPSolverConvergence can then set kConverged. Through TrajOptIfoptMotionPlanner, a callback that always returns False is called once per iteration up to max_iterations and the response can still be successful. To bound a solve, set opt_params.max_iterations or opt_params.max_time.

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() → TrajOptQPProblem(nodes_variables), with every constraint and cost set added to it; 0.34's IfoptQPProblem(IfoptProblem(nodes_variables)) is no longer bound (see Breaking API changes)
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¶

Per-axis Cartesian constraints¶

Per-joint bounds¶

SQP Solver Loop¶

Warm-Starting¶

Starting values and failure¶

What the TrajOpt-Ifopt planner builds¶

The planner's term factories¶

Progress callbacks through the planner¶

Python Subclassing¶

Migrating from 0.33¶

See also¶