Skip to content

tesseract_robotics.trajopt_sqp

Sequential Quadratic Programming (SQP) solver for trajectory optimization. See the Low-Level SQP guide for the user-guide walkthrough.

Problem hierarchy (0.34)

0.34 splits the problem into two layers:

  • IfoptProblem(nodes_variables) — the non-linear program (NLP). Holds the variables and accepts joint/cartesian/cost sets.
  • IfoptQPProblem(nlp) — the QP wrapper passed to the solver. Accepts collision constraints and must be setup() before solving.
Python
from tesseract_robotics import trajopt_ifopt as ti
from tesseract_robotics import trajopt_sqp as tsqp

nodes_variables = ti.createNodesVariables("trajectory", joint_names, states, bounds)

nlp = tsqp.IfoptProblem(nodes_variables)
nlp.addConstraintSet(joint_constraint)   # joint / cartesian constraints on NLP
nlp.addCostSet(vel_cost)                 # costs on NLP

problem = tsqp.IfoptQPProblem(nlp)
problem.addConstraintSet(collision_constraint)  # collision constraints on QP
problem.setup()                                 # must call before solving

IfoptQPProblem no longer has a default constructor — see changes for the full migration.

Solver

TrustRegionSQPSolver

Main SQP solver with trust-region globalization.

Python
qp_solver = tsqp.OSQPEigenSolver()
solver = tsqp.TrustRegionSQPSolver(qp_solver)
solver.verbose = False
solver.params.initial_trust_box_size = 0.01

# Solve
solver.solve(problem)
results = solver.getResults()

# Or single incremental step (for online/replanning)
solver.init(problem)
solver.stepSQPSolver()
solver.setBoxSize(0.01)          # trust region resets don't belong in the solve loop

Cost/constraint evaluation uses getTotalExactCost() / getExactCosts() on the QP problem (no arguments) — the 0.33 evaluate* methods are gone.

OSQPEigenSolver

QPSolver implementation backed by OSQP. Used by TrustRegionSQPSolver.

Parameters and Results

  • SQPParameters — configure iteration limits, trust region, penalty coefficients. Mutated via solver.params before solve().
  • SQPResults — the solver's iteration state. Notable attributes: best_var_vals, best_exact_merit, best_costs, best_constraint_violations, cost_names, constraint_names, plus the various iteration counters.
  • Solver status: solver.getStatus() returns the SQPStatus enum.

Enums

Python
from tesseract_robotics.trajopt_sqp import (
    SQPStatus, QPSolverStatus, CostPenaltyType,
)

SQPStatus.RUNNING
SQPStatus.NLP_CONVERGED
QPSolverStatus.UNITIALIZED
CostPenaltyType.SQUARED

Python names are unchanged from 0.33 — see changes. The 0.33 ConstraintType enum was removed; if you referenced it, drop the usage.

Callbacks

Override execute(problem, results) — return False to stop the solver.

Python
class MyCallback(tsqp.SQPCallback):
    def execute(self, problem, results):
        print(f"iter {results.overall_iteration}: best_merit={results.best_exact_merit}")
        return True

solver.registerCallback(MyCallback())

Module API

tesseract_robotics.trajopt_sqp

__all__ module-attribute

Python
__all__ = ['CostPenaltyType', 'SQPStatus', 'QPSolverStatus', 'SQPParameters', 'SQPResults', 'QPSolver', 'OSQPEigenSolver', 'QPProblem', 'IfoptQPProblem', 'TrustRegionSQPSolver', 'SQPCallback', 'IfoptProblem']

CostPenaltyType

Bases: Enum

Penalty type for cost terms

SQUARED class-attribute instance-attribute
Python
SQUARED = 0

Squared penalty (L2)

ABSOLUTE class-attribute instance-attribute
Python
ABSOLUTE = 1

Absolute penalty (L1)

HINGE class-attribute instance-attribute
Python
HINGE = 2

Hinge penalty

SQPStatus

Bases: Enum

Status codes for SQP optimization

RUNNING class-attribute instance-attribute
Python
RUNNING = 0

Optimization is currently running

NLP_CONVERGED class-attribute instance-attribute
Python
NLP_CONVERGED = 1

NLP successfully converged

ITERATION_LIMIT class-attribute instance-attribute
Python
ITERATION_LIMIT = 2

Reached iteration limit

PENALTY_ITERATION_LIMIT class-attribute instance-attribute
Python
PENALTY_ITERATION_LIMIT = 3

Reached penalty iteration limit

OPT_TIME_LIMIT class-attribute instance-attribute
Python
OPT_TIME_LIMIT = 4

Reached time limit

QP_SOLVER_ERROR class-attribute instance-attribute
Python
QP_SOLVER_ERROR = 5

QP solver failed

CALLBACK_STOPPED class-attribute instance-attribute
Python
CALLBACK_STOPPED = 6

Stopped by callback

QPSolverStatus

Bases: Enum

Status of QP solver

UNITIALIZED class-attribute instance-attribute
Python
UNITIALIZED = 0

Solver not initialized

INITIALIZED class-attribute instance-attribute
Python
INITIALIZED = 1

Solver initialized

QP_ERROR class-attribute instance-attribute
Python
QP_ERROR = 2

QP solver error

SQPParameters

Parameters controlling SQP optimization

improve_ratio_threshold property writable
Python
improve_ratio_threshold: float

Minimum ratio exact_improve/approx_improve to accept step (default: 0.25)

min_trust_box_size property writable
Python
min_trust_box_size: float

NLP converges if trust region smaller than this (default: 1e-4)

min_approx_improve property writable
Python
min_approx_improve: float

NLP converges if approx_merit_improve smaller than this (default: 1e-4)

min_approx_improve_frac property writable
Python
min_approx_improve_frac: float

NLP converges if approx_merit_improve/best_exact_merit < this

max_iterations property writable
Python
max_iterations: int

Max number of QP calls allowed (default: 50)

trust_shrink_ratio property writable
Python
trust_shrink_ratio: float

Trust region scale factor when shrinking (default: 0.1)

trust_expand_ratio property writable
Python
trust_expand_ratio: float

Trust region scale factor when expanding (default: 1.5)

cnt_tolerance property writable
Python
cnt_tolerance: float

Constraint violation tolerance (default: 1e-4)

max_merit_coeff_increases property writable
Python
max_merit_coeff_increases: float

Max times constraints will be inflated (default: 5)

max_qp_solver_failures property writable
Python
max_qp_solver_failures: int

Max QP solver failures before abort (default: 3)

merit_coeff_increase_ratio property writable
Python
merit_coeff_increase_ratio: float

Scale factor for constraint inflation (default: 10)

max_time property writable
Python
max_time: float

Max optimization time in seconds

initial_merit_error_coeff property writable
Python
initial_merit_error_coeff: float

Initial constraint scaling coefficient (default: 10)

inflate_constraints_individually property writable
Python
inflate_constraints_individually: bool

If true, only violated constraints are inflated (default: true)

initial_trust_box_size property writable
Python
initial_trust_box_size: float

Initial trust region size (default: 0.1)

log_results property writable
Python
log_results: bool

Enable logging (unused)

log_dir property writable
Python
log_dir: str

Log directory (unused)

__init__
Python
__init__() -> None

SQPResults

Results and state from SQP optimization

best_exact_merit property writable
Python
best_exact_merit: float

Lowest cost ever achieved

new_exact_merit property writable
Python
new_exact_merit: float

Cost achieved this iteration

best_approx_merit property writable
Python
best_approx_merit: float

Lowest convexified cost ever achieved

new_approx_merit property writable
Python
new_approx_merit: float

Convexified cost this iteration

best_var_vals property writable
Python
best_var_vals: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

Variable values for best_exact_merit

new_var_vals property writable
Python
new_var_vals: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

Variable values this iteration

approx_merit_improve property writable
Python
approx_merit_improve: float

Convexified cost improvement this iteration

exact_merit_improve property writable
Python
exact_merit_improve: float

Exact cost improvement this iteration

merit_improve_ratio property writable
Python
merit_improve_ratio: float

Cost improvement as ratio of total cost

box_size property writable
Python
box_size: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

Trust region box size (var_vals ± box_size)

merit_error_coeffs property writable
Python
merit_error_coeffs: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

Coefficients weighting constraint violations

best_constraint_violations property writable
Python
best_constraint_violations: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

Constraint violations for best solution (positive = violation)

new_constraint_violations property writable
Python
new_constraint_violations: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

Constraint violations this iteration

best_approx_constraint_violations property writable
Python
best_approx_constraint_violations: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

Convexified constraint violations for best solution

new_approx_constraint_violations property writable
Python
new_approx_constraint_violations: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

Convexified constraint violations this iteration

best_costs property writable
Python
best_costs: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

Cost values for best solution

new_costs property writable
Python
new_costs: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

Cost values this iteration

best_approx_costs property writable
Python
best_approx_costs: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

Convexified costs for best solution

new_approx_costs property writable
Python
new_approx_costs: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

Convexified costs this iteration

constraint_names property writable
Python
constraint_names: list[str]

Names of constraint sets

cost_names property writable
Python
cost_names: list[str]

Names of cost terms

penalty_iteration property writable
Python
penalty_iteration: int
convexify_iteration property writable
Python
convexify_iteration: int
trust_region_iteration property writable
Python
trust_region_iteration: int
overall_iteration property writable
Python
overall_iteration: int
print
Python
print() -> None

Print results to console

QPSolver

Abstract base class for QP solvers

verbosity property writable
Python
verbosity: int

Verbosity level (0 = silent)

init
Python
init(num_vars: int, num_cnts: int) -> bool

Initialize the QP solver

clear
Python
clear() -> bool

Clear the QP solver

solve
Python
solve() -> bool

Solve the QP

getSolution
Python
getSolution() -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]

Get the solution vector

getSolverStatus
Python
getSolverStatus() -> QPSolverStatus

Get solver status

OSQPEigenSolver

Bases: QPSolver

OSQP-based QP solver

__init__
Python
__init__() -> None
init
Python
init(num_vars: int, num_cnts: int) -> bool
clear
Python
clear() -> bool
solve
Python
solve() -> bool
getSolution
Python
getSolution() -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]
getSolverStatus
Python
getSolverStatus() -> QPSolverStatus
updateGradient
Python
updateGradient(gradient: Annotated[NDArray[float64], dict(shape=(None,), writable=False)]) -> bool
updateLowerBound
Python
updateLowerBound(lower_bound: Annotated[NDArray[float64], dict(shape=(None,), writable=False)]) -> bool
updateUpperBound
Python
updateUpperBound(upper_bound: Annotated[NDArray[float64], dict(shape=(None,), writable=False)]) -> bool
updateBounds
Python
updateBounds(lower_bound: Annotated[NDArray[float64], dict(shape=(None,), writable=False)], upper_bound: Annotated[NDArray[float64], dict(shape=(None,), writable=False)]) -> bool

QPProblem

Abstract base class for QP problems (convexified NLP)

addConstraintSet
Python
addConstraintSet(constraint_set: ConstraintSet) -> None

Add a set of constraints

addCostSet
Python
addCostSet(constraint_set: ConstraintSet, penalty_type: CostPenaltyType) -> None

Add a cost term with specified penalty type

setup
Python
setup() -> None

Setup the QP problem (call after adding all sets)

getVariableValues
Python
getVariableValues() -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]

Get current optimization variable values

convexify
Python
convexify() -> None

Run the full convexification routine

evaluateTotalConvexCost
Python
evaluateTotalConvexCost(var_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)]) -> float

Evaluate convexified cost at given point

evaluateConvexCosts
Python
evaluateConvexCosts(var_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)]) -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]

Evaluate individual convexified costs

getTotalExactCost
Python
getTotalExactCost() -> float

Get exact (non-convexified) total cost

getExactCosts
Python
getExactCosts() -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]

Get current exact costs

evaluateConvexConstraintViolations
Python
evaluateConvexConstraintViolations(var_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)]) -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]

Evaluate convexified constraint violations

getExactConstraintViolations
Python
getExactConstraintViolations() -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]

Get current exact constraint violations

scaleBoxSize
Python
scaleBoxSize(scale: float) -> None

Uniformly scale the trust region box size

setBoxSize
Python
setBoxSize(box_size: Annotated[NDArray[float64], dict(shape=(None,), writable=False)]) -> None

Set trust region box size

setConstraintMeritCoeff
Python
setConstraintMeritCoeff(merit_coeff: Annotated[NDArray[float64], dict(shape=(None,), writable=False)]) -> None

Set constraint merit coefficients

getBoxSize
Python
getBoxSize() -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]

Get trust region box size

print
Python
print() -> None

Print problem to console

getNumNLPVars
Python
getNumNLPVars() -> int

Number of NLP variables

getNumNLPConstraints
Python
getNumNLPConstraints() -> int

Number of NLP constraints

getNumNLPCosts
Python
getNumNLPCosts() -> int

Number of NLP cost terms

getNumQPVars
Python
getNumQPVars() -> int

Number of QP variables (includes slack)

getNumQPConstraints
Python
getNumQPConstraints() -> int

Number of QP constraints

getNLPConstraintNames
Python
getNLPConstraintNames() -> list[str]

Get constraint names

getNLPCostNames
Python
getNLPCostNames() -> list[str]

Get cost names

IfoptQPProblem

Bases: QPProblem

QP problem wrapper for trajopt_ifopt::Problem (general NLP)

__init__
Python
__init__(nlp: IfoptProblem) -> None

Construct from trajopt_ifopt Problem

addConstraintSet
Python
addConstraintSet(constraint_set: ConstraintSet) -> None
addCostSet
Python
addCostSet(constraint_set: ConstraintSet, penalty_type: CostPenaltyType) -> None
setup
Python
setup() -> None
convexify
Python
convexify() -> None
print
Python
print() -> None

SQPCallback

Base class for SQP optimization callbacks

__init__
Python
__init__() -> None
execute
Python
execute(problem: QPProblem, sqp_results: SQPResults) -> bool

Called during SQP. Return false to stop optimization.

TrustRegionSQPSolver

Trust region SQP solver for trajectory optimization.

Key methods for real-time/incremental optimization: - init(qp_prob): Initialize solver with problem - stepSQPSolver(): Run ONE SQP iteration (for real-time control) - getResults(): Get current optimization results - setBoxSize(): Control trust region size

verbose property writable
Python
verbose: bool

If true, print debug info to console

params property writable
Python
params: SQPParameters

SQP parameters (modify before calling init/solve)

qp_solver property writable
Python
qp_solver: QPSolver

The QP solver used internally

qp_problem property writable
Python
qp_problem: QPProblem

The current QP problem

__init__
Python
__init__(qp_solver: QPSolver) -> None

Construct with a QP solver (e.g., OSQPEigenSolver)

init
Python
init(qp_prob: QPProblem) -> bool

Initialize the solver with a QP problem for incremental solving

solve
Python
solve(qp_prob: QPProblem) -> None

Run complete SQP optimization

stepSQPSolver
Python
stepSQPSolver() -> bool

Run a SINGLE SQP convexification step.

This is the key method for real-time/online planning. Returns True if QP solve converged (does not mean SQP converged).

Typical usage

solver.init(problem) while solver.getStatus() == SQPStatus.RUNNING: solver.stepSQPSolver() # Check constraints, update environment, etc.

verifySQPSolverConvergence
Python
verifySQPSolverConvergence() -> bool

Check if SQP constraints are satisfied

adjustPenalty
Python
adjustPenalty() -> None

Increase penalty on constraints when SQP reports convergence but constraints violated

runTrustRegionLoop
Python
runTrustRegionLoop() -> None

Run trust region loop (adjusts box size)

solveQPProblem
Python
solveQPProblem() -> SQPStatus

Solve current QP problem, store results, call callbacks

setBoxSize
Python
setBoxSize(box_size: float) -> None

Set trust region box size (for online planning control)

callCallbacks
Python
callCallbacks() -> bool

Call all registered callbacks. Returns false if any callback returned false.

printStepInfo
Python
printStepInfo() -> None

Print info about current optimization state

registerCallback
Python
registerCallback(callback: SQPCallback) -> None

Register an optimization callback

getStatus
Python
getStatus() -> SQPStatus

Get current SQP status

getResults
Python
getResults() -> SQPResults

Get current SQP results

IfoptProblem

trajopt_ifopt::Problem - generic NLP with variables, costs, constraints

__init__
Python
__init__(variables: Variables) -> None
addConstraintSet
Python
addConstraintSet(constraint_set: ConstraintSet) -> None
addCostSet
Python
addCostSet(cost_set: ConstraintSet) -> None
getNumberOfOptimizationVariables
Python
getNumberOfOptimizationVariables() -> int
getNumberOfConstraints
Python
getNumberOfConstraints() -> int
getVariableValues
Python
getVariableValues() -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]
printCurrent
Python
printCurrent() -> None

See also