tesseract_robotics.trajopt_sqp¶
Sequential Quadratic Programming (SQP) solver for trajectory optimization. See the Low-Level SQP guide for the user-guide walkthrough.
The QP problem¶
TrajOptQPProblem(nodes_variables) is the problem the solver takes: the one tesseract_planning's TrajOpt-Ifopt planner and its online_planning_example.cpp build. Constraint and cost sets go straight in, and setup() runs after the last one:
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)
vars_list = [node.getVar("joints") for node in nodes_variables.getNodes()]
joint_constraint = ti.JointPosConstraint(states[0], vars_list[0], np.ones(len(joint_names)), "Joints")
collision_constraint = ti.DiscreteCollisionConstraint(evaluator, vars_list[1])
vel_cost = ti.JointVelConstraint(np.zeros(len(joint_names)), vars_list, np.ones(1), "Velocity")
problem = tsqp.TrajOptQPProblem(nodes_variables)
problem.addConstraintSet(joint_constraint) # joint / Cartesian constraints
problem.addConstraintSet(collision_constraint) # collision constraints
problem.addCostSet(vel_cost, tsqp.CostPenaltyType.SQUARED) # any penalty type
problem.setup() # must call before solving
Every row of a cost or constraint set must be an equality or bounded on one side, with any penalty type; addCostSet and addConstraintSet raise on a two-sided range row. (trajopt 0.35.0 required equality rows for squared and absolute costs and one-sided rows for hinge costs.) Each set is linked to the variables when it is added, and dynamic sets (isDynamic()) are accepted. The exact merit weights each cost row by getCoefficients(), as the convex model does, and counts every cost term once; it charges each constraint set its violation weighted the same way (ConstraintViolations.weighted). So the trust-region ratio compares like with like for every penalty type. The problem keeps one merit coefficient, one violation and one name per constraint set, not per row, and counts cost sets the same way.
After convexify() the problem exposes the QP it built: minimise x'Hx + g'x over
getNumQPVars() variables (the NLP variables, then the slacks) subject to
lower ≤ A x ≤ upper over getNumQPConstraints() rows. getHessian() and
getConstraintMatrix() return scipy.sparse copies; getGradient(), getBoundsLower(),
getBoundsUpper() and getConstraintMeritCoeff() vectors.
problem.setVariables(x) sets the NLP variables and then updates every component, so a
dynamic set recounts its rows; NodesVariables.setVariables only writes the values. It raises
VariablesSizeError (a ValueError) unless len(x) == getNumNLPVars().
evaluateConvexCosts, evaluateTotalConvexCost and evaluateConvexConstraintViolations take a
point of the QP the last convexify() built: getNumQPVars() entries, the NLP variables followed
by the slacks. They raise QPSolutionSizeError for any other size and NotConvexifiedError
before the first convexify(), both ValueErrors. trajopt reads only the NLP block, so a shorter
vector would be read past its end, and before convexify() there is no model to read.
An ABSOLUTE or HINGE cost is modelled on its linearized rows, without the slacks: the
model charges each row its coefficient times the row's linearized violation, and the exact cost
charges the same coefficient times the exact violation. A violation larger than the trust box is
then reduced step by step, as a squared one is. #149's development builds on trajopt 0.35.0 read
such a cost as 0 at every QP solution and dropped its coefficient from the exact cost (#151);
trajopt#592 fixed both before any wheel shipped TrajOptQPProblem.
IfoptQPProblem and IfoptProblem are no longer bound
IfoptQPProblem(IfoptProblem(nodes_variables)), the two-layer form of 0.34, is gone; trajopt
is removing IfoptQPProblem (trajopt#595). It is not a rename: see
Breaking API changes for
the migration and the five ways the problems differ.
Solver¶
TrustRegionSQPSolver¶
Main SQP solver with trust-region globalization.
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, or driven by hand
through the QPSolver interface: init(num_vars, num_cnts), then updateHessianMatrix(H),
updateGradient(g), updateLinearConstraintsMatrix(A) and updateBounds(lower, upper), then
solve() and getSolution(). The objective is x'Hx + g'x (the solver doubles H for OSQP's
½ x'Px). Each update raises QPSizeError (a ValueError) unless its size matches init().
Before the first solve() only updateBounds stores bounds; updateLowerBound and
updateUpperBound return False until OSQP is set up.
import scipy.sparse
qp_solver = tsqp.OSQPEigenSolver()
qp_solver.init(2, 2)
qp_solver.updateHessianMatrix(scipy.sparse.identity(2, format="csr"))
qp_solver.updateGradient([-2.0, -4.0])
qp_solver.updateLinearConstraintsMatrix(scipy.sparse.identity(2, format="csr"))
qp_solver.updateBounds([-10.0, -10.0], [0.5, 10.0])
qp_solver.solve()
qp_solver.getSolution() # [0.5, 2.0]
setWarmStart has two forms: setWarmStart(True) / setWarmStart(False) sets the OSQP
option, and setWarmStart(problem) seeds the next solve from a convexified problem's variables
and slacks (a no-op while the option is off; the solver must be init()-ed at the problem's QP
size, else QPSizeError).
Parameters and Results¶
SQPParameters— configure iteration limits, trust region, penalty coefficients. Mutated viasolver.paramsbeforesolve().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 theSQPStatusenum;toString(status)gives trajopt's text for it ("SQP_CONVERGED"). SQPParameterscompares by value and is unhashable.
Enums¶
from tesseract_robotics.trajopt_sqp import (
SQPStatus, QPSolverStatus, CostPenaltyType,
)
SQPStatus.RUNNING
SQPStatus.CONVERGED
QPSolverStatus.UNINITIALIZED
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.
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())
Reference¶
trajopt_sqp Python bindings - SQP solver for trajectory optimization
QPProblemExpiredError
¶
Bases: RuntimeError
A QPProblemView used after the SQPCallback call it was passed to returned.
VariablesSizeError
¶
Bases: ValueError
QPProblem.setVariables with a vector that is not getNumNLPVars() long.
QPSizeError
¶
Bases: ValueError
QPSolver input whose size is not the (num_vars, num_cnts) of its init().
QPSolutionSizeError
¶
Bases: ValueError
A convex evaluator's var_vals that is not getNumQPVars() long.
NotConvexifiedError
¶
Bases: ValueError
A convex evaluator called before the problem's first convexify().
CostPenaltyType
¶
SQPStatus
¶
QPSolverStatus
¶
SQPParameters
¶
Parameters controlling SQP optimization
improve_ratio_threshold
property
writable
¶
Minimum ratio exact_improve/approx_improve to accept step (default: 0.25)
min_trust_box_size
property
writable
¶
NLP converges if trust region smaller than this (default: 1e-4)
min_approx_improve
property
writable
¶
NLP converges if approx_merit_improve smaller than this (default: 1e-4)
min_approx_improve_frac
property
writable
¶
NLP converges if approx_merit_improve/best_exact_merit < this
max_iter
property
writable
¶
Max convexifications per penalty iteration; rejected trust-region steps do not count (default: 50)
trust_shrink_ratio
property
writable
¶
Trust region scale factor when shrinking (default: 0.1)
trust_expand_ratio
property
writable
¶
Trust region scale factor when expanding (default: 1.5)
cnt_tolerance
property
writable
¶
Constraint violation tolerance (default: 1e-4)
max_merit_coeff_increases
property
writable
¶
Max times constraints will be inflated (default: 5)
max_qp_solver_failures
property
writable
¶
Max QP solver failures before abort (default: 3)
merit_coeff_increase_ratio
property
writable
¶
Scale factor for constraint inflation (default: 10)
initial_merit_error_coeff
property
writable
¶
Initial constraint scaling coefficient (default: 10)
inflate_constraints_individually
property
writable
¶
If true, only violated constraints are inflated (default: true)
sum_constraint_set_violations
property
writable
¶
If true, cnt_tolerance is tested against a set's summed row violations; if false, against its largest row. Applied by TrustRegionSQPSolver.init() (default: true)
initial_trust_box_size
property
writable
¶
Initial trust region size (default: 0.1)
ConstraintViolations
¶
SQPResults
¶
Results and state from SQP optimization
best_approx_merit
property
writable
¶
Lowest convexified cost ever achieved
best_var_vals
property
writable
¶
Variable values for best_exact_merit
new_var_vals
property
writable
¶
Variable values this iteration
approx_merit_improve
property
writable
¶
Convexified cost improvement this iteration
exact_merit_improve
property
writable
¶
Exact cost improvement this iteration
merit_improve_ratio
property
writable
¶
Cost improvement as ratio of total cost
box_size
property
writable
¶
Trust region box size (var_vals ± box_size)
merit_error_coeffs
property
writable
¶
Coefficients weighting constraint violations
best_constraint_violations
property
writable
¶
Constraint violations for best solution (positive = violation)
new_constraint_violations
property
writable
¶
Constraint violations this iteration
best_approx_constraint_violations
property
writable
¶
Convexified constraint violations for best solution
new_approx_constraint_violations
property
writable
¶
Convexified constraint violations this iteration
best_costs
property
writable
¶
Cost values for best solution
new_costs
property
writable
¶
Cost values this iteration
best_approx_costs
property
writable
¶
Convexified costs for best solution
new_approx_costs
property
writable
¶
Convexified costs this iteration
QPSolver
¶
Abstract base class for QP solvers
getSolution
¶
Get the solution vector
updateHessianMatrix
¶
Set the Hessian H (sparse, num_vars square) of the objective x'Hx + g'x
updateGradient
¶
updateGradient(gradient: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> bool
Set the gradient g (num_vars)
updateLowerBound
¶
updateLowerBound(lower_bound: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> bool
Set the constraint lower bounds (num_cnts)
updateUpperBound
¶
updateUpperBound(upper_bound: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> bool
Set the constraint upper bounds (num_cnts)
updateBounds
¶
updateBounds(lower_bound: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float], upper_bound: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> bool
Set both constraint bounds (num_cnts each)
updateLinearConstraintsMatrix
¶
Set the constraint matrix A (sparse, num_cnts x num_vars)
setWarmStart
¶
Seed the next solve from the problem's variables (and its slacks); a no-op while the solver's warm-start option is off
OSQPEigenSolver
¶
Bases: QPSolver
OSQP-based QP solver
getSolution
¶
setAdaptiveRho
¶
Adapt the ADMM step size rho (OSQP adaptive_rho).
setAdaptiveRhoInterval
¶
Adapt rho every interval iterations. OSQP's 0 adapts on wall-clock timing, which makes solutions differ run to run; a fixed interval (e.g. 25) makes them reproducible.
setMaxIteration
¶
Maximum ADMM iterations per QP solve.
setAbsoluteTolerance
¶
Absolute convergence tolerance (OSQP eps_abs).
setRelativeTolerance
¶
Relative convergence tolerance (OSQP eps_rel).
QPProblem
¶
Abstract base class for QP problems (convexified NLP)
addConstraintSet
¶
Add a set of constraints
addCostSet
¶
Add a cost term with specified penalty type
getVariableValues
¶
Get current optimization variable values
setVariables
¶
setVariables(x: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> None
Set the NLP variables and update every component (a TrajOptQPProblem also recounts its dynamic rows). Raises VariablesSizeError unless len(x) == getNumNLPVars().
evaluateTotalConvexCost
¶
evaluateTotalConvexCost(var_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> float
Evaluate the convexified total cost at var_vals.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
var_vals
|
Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]
|
A point of the QP built by the last convexify(): getNumQPVars() entries, the NLP variables followed by the slack variables, as in a QP solution. To evaluate at an NLP point, append zeros for the slacks. |
required |
Returns:
| Type | Description |
|---|---|
float
|
The sum of evaluateConvexCosts(var_vals). |
Raises:
| Type | Description |
|---|---|
QPSolutionSizeError
|
if var_vals does not have getNumQPVars() entries. |
NotConvexifiedError
|
before the first convexify(). |
evaluateConvexCosts
¶
evaluateConvexCosts(var_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]
Evaluate each cost term of the convexified problem at var_vals.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
var_vals
|
Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]
|
A point of the QP built by the last convexify(): getNumQPVars() entries, the NLP variables followed by the slack variables, as in a QP solution. To evaluate at an NLP point, append zeros for the slacks. |
required |
Returns:
| Type | Description |
|---|---|
Annotated[NDArray[float64], dict(shape=(None,), order=C)]
|
One convexified cost per cost term, in getNLPCostNames() order. |
Raises:
| Type | Description |
|---|---|
QPSolutionSizeError
|
if var_vals does not have getNumQPVars() entries. |
NotConvexifiedError
|
before the first convexify(). |
getExactCosts
¶
Get current exact costs
evaluateConvexConstraintViolations
¶
evaluateConvexConstraintViolations(var_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> ConstraintViolations
Evaluate the convexified constraint violations at var_vals.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
var_vals
|
Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]
|
A point of the QP built by the last convexify(): getNumQPVars() entries, the NLP variables followed by the slack variables, as in a QP solution. To evaluate at an NLP point, append zeros for the slacks. |
required |
Returns:
| Type | Description |
|---|---|
ConstraintViolations
|
One violation per constraint set, in getNLPConstraintNames() order; 0 where |
ConstraintViolations
|
satisfied. |
Raises:
| Type | Description |
|---|---|
QPSolutionSizeError
|
if var_vals does not have getNumQPVars() entries. |
NotConvexifiedError
|
before the first convexify(). |
getExactConstraintViolations
¶
Get current exact constraint violations
setSumConstraintSetViolations
¶
Reduce a merit unit's row violations to their sum (True) or to the largest (False) in ConstraintViolations.raw. TrustRegionSQPSolver.init() overwrites this with SQPParameters.sum_constraint_set_violations.
setBoxSize
¶
setBoxSize(box_size: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> None
Set trust region box size
setConstraintMeritCoeff
¶
setConstraintMeritCoeff(merit_coeff: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> None
Set constraint merit coefficients
getBoxSize
¶
Get trust region box size
getConstraintMeritCoeff
¶
Get the constraint merit coefficients, one per constraint set
getHessian
¶
The QP Hessian (sparse, getNumQPVars() square), from the last convexify()
getGradient
¶
The QP gradient (getNumQPVars()), from the last convexify()
getConstraintMatrix
¶
The QP constraint matrix (sparse, getNumQPConstraints() x getNumQPVars()), from the last convexify()
getBoundsLower
¶
The QP constraint lower bounds (getNumQPConstraints()), from the last convexify()
getBoundsUpper
¶
The QP constraint upper bounds (getNumQPConstraints()), from the last convexify()
TrajOptQPProblem
¶
QPProblemView
¶
Bases: QPProblem
The problem an SQPCallback receives from a solve whose problem has no Python object (one a planner built in C++). Valid only during that call; afterwards every method raises QPProblemExpiredError.
SQPCallback
¶
Base class for SQP optimization callbacks
execute
¶
Called during SQP. Return false to stop optimization. problem is valid only during the call: a problem passed to solve() from Python arrives as that object; one a planner built in C++ arrives as a QPProblemView, which raises QPProblemExpiredError once the call has returned. sqp_results is a copy the callback may keep.
PIQPKKTSolver
¶
Bases: Enum
PIQP's KKT system solver
PIQPSettings
¶
PIQP solver settings (piqp::Settings
eps_duality_gap_abs
property
writable
¶
Absolute duality gap tolerance
eps_duality_gap_rel
property
writable
¶
Relative duality gap tolerance
infeasibility_threshold
property
writable
¶
Threshold for the infeasibility detection
reg_finetune_lower_limit
property
writable
¶
Fine-tune lower limit of the regularization
reg_finetune_primal_update_threshold
property
writable
¶
Primal updates before fine-tuning the regularization
reg_finetune_dual_update_threshold
property
writable
¶
Dual updates before fine-tuning the regularization
max_factor_retires
property
writable
¶
Maximum number of factorization retries
preconditioner_scale_cost
property
writable
¶
Scale the cost in the preconditioner
preconditioner_reuse_on_update
property
writable
¶
Reuse the preconditioner when the problem is updated
iterative_refinement_always_enabled
property
writable
¶
Always run iterative refinement
iterative_refinement_eps_abs
property
writable
¶
Iterative refinement absolute tolerance
iterative_refinement_eps_rel
property
writable
¶
Iterative refinement relative tolerance
iterative_refinement_max_iter
property
writable
¶
Iterative refinement maximum iterations
iterative_refinement_min_improvement_rate
property
writable
¶
Iterative refinement minimum improvement rate
iterative_refinement_static_regularization_eps
property
writable
¶
Iterative refinement static regularization
iterative_refinement_static_regularization_rel
property
writable
¶
Iterative refinement relative static regularization
PIQPSolver
¶
TrustRegionSQPSolver
¶
Trust region SQP solver for trajectory optimization.
Key methods for 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
params
property
writable
¶
SQP parameters (modify before calling init/solve)
__init__
¶
Construct with a QP solver (e.g., OSQPEigenSolver)
init
¶
Initialize the solver with a QP problem for incremental solving
stepSQPSolver
¶
Run a SINGLE SQP convexification step.
Used for online planning, where the caller updates the problem between steps.
Returns True when no further step should follow at the current penalty: the step converged, the QP failure budget was spent, or a callback stopped the solve; getStatus() tells which. It does not mean the SQP solver has converged.
Example:
verifySQPSolverConvergence
¶
Check if SQP constraints are satisfied
adjustPenalty
¶
Increase penalty on constraints when SQP reports convergence but constraints violated
solveQPProblem
¶
Solve current QP problem, store results, call callbacks
setBoxSize
¶
Set trust region box size (for online planning control)
callCallbacks
¶
Call all registered callbacks. Returns false if any callback returned false.
registerCallback
¶
Register an optimization callback
toString
¶
trajopt's text for a solver status (e.g. 'SQP_CONVERGED')
See also¶
trajopt_ifopt— variables and constraints- Low-Level SQP API
- 0.33 → 0.34 migration