tesseract_robotics.trajopt_ifopt¶
Variables and constraint sets for the low-level SQP solver, and how each set becomes a
constraint or a cost of a TrajOptQPProblem.
See the Low-Level SQP guide for the user-guide walkthrough.
Variables (0.34+)¶
The JointPosition class from 0.33 is removed. Variables now use a three-level hierarchy.
Var¶
A single variable — typically one joint value at one waypoint. Var(index, name, value, bounds)
and Var(index, identifier, names, values, bounds) build a standalone one (no parent Node) at
index of the decision vector. It belongs to no variable set, so a constraint built on it
raises VarNotInVariableSetError when linked (below); a collision constraint already when built.
Node¶
Groups multiple Vars per waypoint (e.g., joints + velocities at one waypoint).
NodesVariables¶
Container of Nodes. Passed to TrajOptQPProblem. NodesVariables(name, nodes) takes
ownership of the Nodes: after the call the caller's Node objects are unusable (read the nodes
back with getNode / getNodes), and each Var reports its Node (Var.getParent()) and the
Node its set (Node.getParent()). A Node given twice, or one already in a set, raises
NodeOwnershipError. A Var or Node read out of a set keeps the set alive, as does a handed-over
Node, so a Var taken from a Node before the handover stays valid after the set is dropped.
Node.getParent() returns the set only when Python built it (NodesVariables(...) or
createNodesVariables), it is alive and it holds that Node; the result keeps the set alive. A
Node of a set C++ owns, such as the planner's nodes a Python composite profile's create receives,
raises NodeParentUnavailableError (a RuntimeError), during create too: the planner frees that
set when its solve returns, and a Node holds only a raw pointer to it. For a set of a Python
subclass of NodesVariables, getParent() returns that very object. A reference cycle through a Node on such an object
(s.n = s.getNode(0)) is not collected by the garbage collector: break it by hand.
from tesseract_robotics.trajopt_ifopt import Bounds, Node, NodesVariables
joint_bounds = [Bounds(-3.14, 3.14)] * len(joint_names)
nodes = []
for k, q in enumerate(initial_states):
node = Node(f"waypoint_{k}")
node.addVar("joints", joint_names, q, joint_bounds)
node.addVar("time", 0.1, Bounds(0.01, 1.0)) # a second var per node
nodes.append(node)
nodes_variables = NodesVariables("trajectory", nodes)
createNodesVariables¶
Factory helper. Builds the full hierarchy from a list of initial states.
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
)
# Iterate waypoints to pull Var refs for constraints
vars_list = [node.getVar("joints") for node in nodes_variables.getNodes()]
Constraints¶
Every class below is a ConstraintSet. It goes into a TrajOptQPProblem either as a
constraint, with addConstraintSet, or as a cost, with addCostSet(set, penalty_type). Every
class is accepted as a constraint and as a cost with any penalty type: every row must be an
equality or bounded on one side, and a set may mix the two. A two-sided range row is refused by
addConstraintSet and addCostSet alike. There is no cost without a penalty type. The SQP
models every penalty type as it charges it: an ABSOLUTE or HINGE cost on its linearized rows,
weighted by its coefficients (see the QP problem).
| Class | Purpose | Row bounds |
|---|---|---|
JointPosConstraint (target) |
Joint values at a waypoint | equality |
JointPosConstraint (bounds) |
Per-joint bounds at a waypoint | a range splits into two one-sided rows (the default) |
JointVelConstraint (target) |
Joint velocity toward a target (zero for smoothing) | equality |
JointVelConstraint (bounds) |
Per-joint velocity band, the same every step | a range splits into two one-sided rows (the default) |
JointAccelConstraint (target) |
Joint acceleration toward a target | equality |
JointAccelConstraint (bounds) |
Per-joint acceleration band | a range splits into two one-sided rows (the default) |
JointJerkConstraint (target) |
Joint jerk toward a target | equality |
JointJerkConstraint (bounds) |
Per-joint jerk band | a range splits into two one-sided rows (the default) |
CartPosConstraint (pose) |
TCP pose at a waypoint | equality |
CartPosConstraint (per-axis) |
TCP pose with free axes and bands | per axis |
CartLineConstraint |
TCP on a line segment between two poses | equality |
CartAxisConeConstraint |
A link axis within a half angle of a target axis; rotation about it free | one-sided (upper), in radians |
CartAxisAlignConstraint |
A link axis along a target axis; rotation about it free | equality (two rows), in radians |
DiscreteCollisionConstraint |
Collision at a waypoint | one-sided (upper) |
DiscreteCollisionNumericalConstraint |
The same, with a numerical Jacobian | one-sided (upper) |
ContinuousCollisionConstraint |
Collision along the segment between two waypoints | one-sided (upper) |
InverseKinematicsConstraint |
Joints toward an IK solution of a target pose | equality |
DiscreteCollisionConstraintD |
Collision at a waypoint, one row per contact (the TrajOpt-Ifopt planner's term) | one-sided (upper) |
ContinuousCollisionConstraintD |
Collision along a segment, one row per contact | one-sided (upper) |
ContinuousCollisionNumericalConstraint |
ContinuousCollisionConstraint with a numerical Jacobian |
one-sided (upper) |
The …D sets are dynamic: update() recounts their rows from the contacts it finds, and resets
their bounds to one upper bound per row.
A range row kept whole (RangeBoundHandling.KEEP_AS_IS) goes into neither: both add calls raise
rows must have equality or one-sided bounds!. Measured on trajopt master (a504b22): sets with
equality, one-sided, mixed and range rows were added to a TrajOptQPProblem as a constraint and
with each penalty type, then set up and convexified. That measures acceptance, not how the SQP
models the cost. trajopt 0.35.0 accepted less: SQUARED and ABSOLUTE costs only with equality
rows, HINGE only with one-sided rows, and a range constraint failed only at the first
convexify().
A constraint has a Jacobian only once it is linked to the optimization variables:
addConstraintSet / addCostSet link it, or call linkWithVariables(nodes) yourself.
getJacobian() on an unlinked set raises ConstraintNotLinkedError (a RuntimeError); upstream
would dereference a null pointer. The collision constraints also need update() before
getValues() or getJacobian() (and calcValues() / calcJacobianBlock() on
DiscreteCollisionNumericalConstraint), which TrajOptQPProblem.setup() calls for every set.
Before the first update() they raise RuntimeError("<name>: collision data not computed, call
update() first") (trajopt#617; trajopt 0.35.0 dereferenced a null pointer).
An exception raised in a Python ConstraintSet subclass reaches the caller with its own type,
also when trajopt calls the set from TrajOptQPProblem or TrustRegionSQPSolver.solve(): an
IndexError in getValues() is an IndexError, not trajopt's RuntimeError. On macOS it
used to arrive as RuntimeError with the Python traceback as its message
(#156).
CartLineConstraint takes 1 to 6 CartLineInfo.indices, in any order: getValues(),
getJacobian() and calcJacobianBlock() return one row per index, the rows of the 6-index result
that the indices name. trajopt 0.35.0 filled 6 Jacobian rows whatever their number, so fewer
corrupted memory, and the binding raised CartLineIndicesUnsupportedError until the pinned trajopt
had the fix (trajopt#612,
#237).
setBounds(bounds) replaces a set's row bounds; it raises BoundsSizeError (a ValueError)
unless there is one bound per row (trajopt checks with assert only).
calcJacobianBlock(joint_vals) on CartPosConstraint, CartLineConstraint,
DiscreteCollisionNumericalConstraint and InverseKinematicsConstraint evaluates the Jacobian
at given joint values, sized like getJacobian(), so it also needs linked variables; a wrong
joint_vals length raises JointValuesSizeError. trajopt 0.35.0 declared the IK one without
defining it; trajopt#626 defines it.
The list[Bounds] constructors of JointVelConstraint, JointAccelConstraint and
JointJerkConstraint take one bound per joint, repeated for every step. The rows are undivided
finite differences of the positions, so a velocity limit v (rad/s) over a step dt is the band
Bounds(-v * dt, v * dt). As with JointPosConstraint, a range splits into a lower-bound row and
an upper-bound row by default; RangeBoundHandling.KEEP_AS_IS keeps it as one row, which
TrajOptQPProblem refuses. trajopt 0.35.0 declared these constructors without defining them
(trajopt#606).
from tesseract_robotics import trajopt_ifopt as ti
n_dof = len(joint_names)
from tesseract_robotics import trajopt_sqp as tsqp
problem = tsqp.TrajOptQPProblem(nodes_variables)
band = [ti.Bounds(-0.1, 0.1)] * n_dof # rad per step
problem.addConstraintSet(ti.JointVelConstraint(band, vars_list, np.ones(n_dof), "VelocityLimit"))
A constraint writes its Jacobian columns at its Vars' indices, so linking it to a variable set
(linkWithVariables, or TrajOptQPProblem.addConstraintSet / addCostSet) raises
VarNotInVariableSetError unless every Var it was built on is in that set (the set holds the
Var's Node, and that Node the Var); the constraint stays unlinked. The collision constraints read the variable set's hash through var.getParent().getParent().
They raise VarNotInVariableSetError (a ValueError) at construction for a Var that is not in
a NodesVariables; upstream would dereference null on the first update(). A linked collision
constraint's update() reads the set it is linked to. Unlinked, update() needs each Var's set
to be one built from Python and still alive, and holds it for the call; for a Var kept from a
planner's set (a profile's create) it raises NodeParentUnavailableError, since the planner frees
that set when its solve returns.
0.34 constraint constructors take Var references directly (not
JointPosition lists). See changes for the migration details.
Collision Evaluators¶
| Class | Pairs with | Collision check type in the config |
|---|---|---|
SingleTimestepCollisionEvaluator |
DiscreteCollisionConstraint |
DISCRETE (the default); the constructor raises otherwise |
LVSDiscreteCollisionEvaluator |
ContinuousCollisionConstraint (LVS discrete mode) |
LVS_DISCRETE; the constructor raises otherwise |
LVSContinuousCollisionEvaluator |
ContinuousCollisionConstraint (LVS continuous mode) |
LVS_CONTINUOUS or CONTINUOUS; the constructor raises otherwise |
DiscreteCollisionEvaluator |
Base class | |
ContinuousCollisionEvaluator |
Base class |
Set the type with config.collision_check_config.type = CollisionEvaluatorType.LVS_DISCRETE
(CollisionEvaluatorType is in tesseract_robotics.collision).
CollisionCache was removed in 0.34 — caching is internal to each evaluator.
The two bases expose what a collision constraint works from. calcCollisions(dof_vals,
max_allowed) (discrete) and calcCollisionData(dof_vals0, dof_vals1, vars0_fixed, vars1_fixed,
max_allowed) (continuous) return a CollisionCacheData: the ContactResultMap and one
GradientResultsSet per link pair. getGradient(dofvals, contact) /
calcGradientData(dof_vals0, dof_vals1, contact) return the GradientResults of one contact,
and getJointGroup() the evaluator's joint group.
data = evaluator.calcCollisions(q, 3)
for results_set in data.gradient_results_sets:
print(results_set.key, results_set.getMaxError())
Info Structs (for Cartesian / IK constraints)¶
| Struct | Used by |
|---|---|
CartLineInfo |
CartLineConstraint |
InverseKinematicsInfo |
InverseKinematicsConstraint |
Both have field constructors: CartLineInfo(manip, source_frame, target_frame,
target_frame_offset1, target_frame_offset2[, source_frame_offset[, indices]]) and
InverseKinematicsInfo(manip, working_frame, tcp_frame[, tcp_offset]). The bracketed arguments
default to the identity and all 6 indices. CartLineConstraint.getInfo(), getLine() and
InverseKinematicsConstraint.getKinematicInfo() read them back; setTargetPose moves the IK
target.
CartPosInfo from 0.33 is gone — CartPosConstraint now takes parameters
directly (see changes).
Config / Bounds¶
TrajOptCollisionConfig(margin, coeff, request, type, longest_valid_segment_length, check_program_mode)— the four trailing parameters default as in C++.CollisionCoeffData— per-pair collision coefficient data.getCollisionCoeffPairData()returns{(link_a, link_b): coeff},getPairsWithZeroCoeff()the switched-off pairs.CollisionCoeffDataandTrajOptCollisionConfigcompare by value and are unhashable.Bounds(lower, upper)— single-variable bounds.b += s/b -= sshift both limits and reclassifygetType().- Enums:
BoundsType,RangeBoundHandling.
Utilities¶
interpolate(start, end, steps)— linear joint interpolation.toBounds(joint_limits)— convert a limits matrix into aBoundslist.getClosestValidPoint(x, bounds),calcBoundsErrors(x, bounds),calcBoundsViolations(x, bounds)— clamp into bounds, the signed error and the violation per entry; one bound per entry orBoundsSizeError.toTrajArray(vars)/toJointTrajectory(vars)— the vars' values as a waypoints × joints matrix, or aJointTrajectorywith one state per var.calcNumericalConstraintGradient(variables, constraint_set, epsilon=1e-8)andcalcForwardNumJac(f, x, epsilon)— forward-difference Jacobians, to check an analyticgetJacobian()(a PythonConstraintSetsubclass's included).
from tesseract_robotics import trajopt_ifopt as ti
from tesseract_robotics.trajopt_ifopt import calcNumericalConstraintGradient
vars_list = [node.getVar("joints") for node in nodes_variables.getNodes()]
my_constraint = ti.JointPosConstraint(
np.zeros(len(joint_names)), vars_list[0], np.ones(len(joint_names)), "mine"
)
my_constraint.linkWithVariables(nodes_variables)
numerical = calcNumericalConstraintGradient(nodes_variables, my_constraint)
assert np.allclose(numerical.toarray(), my_constraint.getJacobian().toarray(), atol=1e-6)
Python-defined rules¶
tesseract_robotics.trajopt_ifopt.python_constraints holds PyConstraint, a ConstraintSet
base for a rule written as f(q) on one waypoint's joints, and the rules built on it; see
PyConstraint: a rule from f(q).
Planning rules written in Python, solved by TrajOpt's SQP in C++.
PyConstraint is a thin base on the ConstraintSet trampoline: a subclass writes
f(q), the values of the rule at one waypoint's joint values q, and gets bounds,
coefficients and a Jacobian for free. The default Jacobian is a forward difference over that
waypoint's joints only (calcForwardNumJac, n_dof + 1 calls to f), placed in the
waypoint's columns of the problem; override jacobian(q) where an analytic one is cheap.
The rules below use only what Tesseract already computes: forward kinematics, the Jacobian,
manipulability and the discrete contact manager. Two of them have a C++ counterpart, which is
the one to use in production: CartAxisConeConstraint for PointsDown and
CartAxisAlignConstraint for HoldAxis at zero tolerance. They stay here as the worked
examples of a rule with an analytic Jacobian, and the tests check them against the C++ ones.
Example
BoundsShapeError
¶
InvertedBoundsError
¶
ClearanceMarginError
¶
Bases: ValueError
Clearance's d_max is not below its contact margin, so the upper bound is never seen.
ConstraintOptions
¶
Bases: TypedDict
The PyConstraint options every rule passes through.
Attributes:
| Name | Type | Description |
|---|---|---|
epsilon |
float
|
Forward-difference step of the default Jacobian [joint units]. |
coeff |
float | Sequence[float]
|
Merit coefficient per row (scalar or one per row). |
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
| Python | |
|---|---|
PyConstraint
¶
Bases: ConstraintSet
A constraint lower <= f(q) <= upper on one waypoint's joint values q.
A row bounded on both sides with lower < upper (a range) is split into two one-sided rows,
as trajopt's RangeBoundHandling.SPLIT_TO_TWO_INEQUALITIES does: TrajOptQPProblem
takes equality and one-sided rows only, and addConstraintSet / addCostSet raise on
a range row.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
name
|
str
|
name of the constraint set (shown in the solver's output) |
required |
var
|
Var
|
the waypoint's |
required |
lower
|
ArrayLike
|
lower bound per row; |
required |
upper
|
ArrayLike
|
upper bound per row; |
required |
epsilon
|
float
|
forward-difference step of the default Jacobian |
DEFAULT_EPSILON
|
coeff
|
float | Sequence[float]
|
merit coefficient per row (scalar or one per row): how hard the SQP pushes on a violation relative to the other sets (the online example weights collision at 10) |
1.0
|
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
| Python | |
|---|---|
81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 | |
__init__
¶
__init__(name: str, var: Var, lower: ArrayLike, upper: ArrayLike, epsilon: float = DEFAULT_EPSILON, coeff: float | Sequence[float] = 1.0) -> None
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
f
¶
jacobian
¶
d f / d q, shape (rows, n_dof). Default: forward difference over q.
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
getValues
¶
getBounds
¶
getJacobian
¶
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
update
¶
PointsDown
¶
Bases: PyConstraint
The tool axis (axis of tcp) stays within max_tilt of down.
Value: cos of the angle between the tool axis and down, at least cos(max_tilt). There
is no upper bound: a cosine cannot pass 1, and a row that sits on its bound where its
gradient vanishes (the axis aligned) only costs the QP its conditioning.
Analytic Jacobian from Tesseract's: d(a . down)/dq = down . (omega x a).
CartAxisConeConstraint is the C++ form of this rule, on the angle itself.
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
__init__
¶
__init__(var: Var, manip: JointGroup, tcp: str, max_tilt: float, down: ArrayLike = (0.0, 0.0, -1.0), axis: int = 2, analytic: bool = True, name: str = 'points_down', **options: Unpack[ConstraintOptions]) -> None
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
f
¶
jacobian
¶
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
| Python | |
|---|---|
HoldAxis
¶
Bases: PyConstraint
The tool axis (axis of tcp) stays within tol of direction, for small tol.
PointsDown bounds cos(angle), whose gradient vanishes at zero angle: fine for a tilt limit
of tens of degrees, too flat to hold a fraction of a degree. This rule bounds the two
components of the axis perpendicular to direction (each +-sin(tol), linear in the angle),
plus axis . direction >= 0 so the opposite direction is not a solution. The cone is the
square one: up to sqrt(2) * tol on the diagonal.
Analytic Jacobian from Tesseract's: d(P a)/dq = P (omega x a).
CartAxisAlignConstraint is the C++ rule for the equality (no tolerance band).
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
__init__
¶
__init__(var: Var, manip: JointGroup, tcp: str, direction: ArrayLike, tol: float, axis: int = 2, analytic: bool = True, name: str = 'hold_axis', **options: Unpack[ConstraintOptions]) -> None
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
f
¶
jacobian
¶
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
StayDexterous
¶
Bases: PyConstraint
Manipulability of tcp at least w_min: keeps every waypoint off singularities.
Value: Yoshikawa's measure sqrt(det(J J^T)), the volume of Tesseract's manipulability
ellipsoid (calcManipulability(J).m.volume).
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
__init__
¶
__init__(var: Var, manip: JointGroup, tcp: str, w_min: float, name: str = 'stay_dexterous', **options: Unpack[ConstraintOptions]) -> None
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
ElbowUp
¶
Bases: PyConstraint
upper_link stays at least delta above lower_link (one IK branch).
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
__init__
¶
__init__(var: Var, manip: JointGroup, upper_link: str, lower_link: str, delta: float = 0.0, axis: int = 2, name: str = 'elbow_up', **options: Unpack[ConstraintOptions]) -> None
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
f
¶
WatchTheTool
¶
Bases: PyConstraint
tcp stays in camera's cone of view, half-angle alpha, both links in one group.
Value: cos of the angle between the optical axis and the line to tcp, at least
cos(alpha); no upper bound, as for PointsDown.
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
__init__
¶
__init__(var: Var, manip: JointGroup, camera: str, tcp: str, alpha: float, optical_axis: int = 2, name: str = 'watch_the_tool', **options: Unpack[ConstraintOptions]) -> None
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
f
¶
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Clearance
¶
Bases: PyConstraint
Closest distance from links to the rest of the scene stays in [d_min, d_max].
Uses a clone of the environment's discrete contact manager, so the planner's collision
engine answers. Distances beyond margin are reported as margin; keep d_max below it.
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
margin
instance-attribute
¶
__init__
¶
__init__(var: Var, env: Environment, joint_ids: Sequence[str], links: Sequence[str], d_min: float, d_max: float, margin: float | None = None, name: str = 'clearance', **options: Unpack[ConstraintOptions]) -> None
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
f
¶
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
WorldHold
¶
Bases: PyConstraint
tcp holds target in the world while the base moves, within tol per axis.
base_pose is the base's world pose for this waypoint: an Isometry3d, or a callable
returning one (e.g. a localisation reader evaluated at the waypoint's time).
Analytic Jacobian: R_base @ J_linear.
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
__init__
¶
__init__(var: Var, manip: JointGroup, tcp: str, target: ArrayLike, base_pose: Callable[[], Isometry3d] | Isometry3d, tol: ArrayLike = WORLD_HOLD_TOL_M, analytic: bool = True, name: str = 'world_hold', **options: Unpack[ConstraintOptions]) -> None
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
f
¶
jacobian
¶
Reference¶
trajopt_ifopt Python bindings: variables, constraints and costs for TrajOpt's SQP.
Rules written in Python live in tesseract_robotics.trajopt_ifopt.python_constraints.
DegenerateAxisError
¶
Bases: ValueError
A CartAxis axis argument is zero or not finite.
ConstraintNotLinkedError
¶
Bases: RuntimeError
getJacobian() on a constraint set that is not linked to variables yet.
BoundsSizeError
¶
Bases: ValueError
A bounds list whose length is not the number of rows or values it bounds.
JointValuesSizeError
¶
Bases: ValueError
joint_vals whose length is not the constraint's number of joints.
VarSizeMismatchError
¶
Bases: ValueError
toTrajArray over Vars of different sizes.
VarNotInVariableSetError
¶
Bases: ValueError
A constraint linked to a variable set that does not hold its Vars, or a collision constraint built on a Var that is not in a NodesVariables.
NodeOwnershipError
¶
Bases: ValueError
A Node handed to NodesVariables that is in a variable set already, or twice.
NodeParentUnavailableError
¶
Bases: RuntimeError
Node.getParent on a Node of a variable set C++ owns (a planner's).
NumericalJacobianSizeError
¶
Bases: ValueError
calcForwardNumJac over a function whose output length changes between calls.
BoundsType
¶
Bases: Enum
RangeBoundHandling
¶
Bounds
¶
Variables
¶
Differentiable
¶
ConstraintSet
¶
Bases: Differentiable
__init__
¶
Create a constraint set with name and number of constraints
linkWithVariables
¶
Connect the constraint with the optimization variables
getVariables
¶
Get the linked optimization variables (protected in C++, exposed via trampoline)
getJacobian
¶
Get the constraint Jacobian (sparse matrix)
getCoefficients
¶
Get the coefficient vector
Var
¶
Node
¶
getParent
¶
The NodesVariables holding this node (kept alive by the result), or None before it is handed to one. Raises NodeParentUnavailableError for a Node of a set C++ owns (a planner's, handed to a profile's create), even during that call
getValues
¶
Get all variable values as a single vector
setVariables
¶
setVariables(x: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> None
Update all variable values from the full decision vector
NodesVariables
¶
CartPosConstraint
¶
Bases: ConstraintSet
use_numeric_differentiation
property
writable
¶
If true, use numeric differentiation (default: true)
calcValues
¶
calcValues(joint_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]
Calculate error values for given joint values
calcJacobianBlock
¶
calcJacobianBlock(joint_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> scipy.sparse.csr_matrix
The constraint Jacobian at joint_vals (sparse matrix, sized like getJacobian()); needs linked variables
getValues
¶
Get current constraint values
setTargetPose
¶
Set the target pose - critical for online planning!
getTargetPose
¶
Get the target pose for the constraint
getCurrentPose
¶
Get current TCP pose in world frame
JointPosConstraint
¶
Bases: ConstraintSet
getValues
¶
Get current constraint values
JointVelConstraint
¶
Bases: ConstraintSet
getValues
¶
Get current constraint values
JointAccelConstraint
¶
Bases: ConstraintSet
getValues
¶
Get current constraint values
CollisionCoeffData
¶
TrajOptCollisionConfig
¶
LinkGradientResults
¶
GradientResults
¶
LinkMaxError
¶
GradientResultsSet
¶
CollisionCacheData
¶
DiscreteCollisionEvaluator
¶
calcCollisions
¶
calcCollisions(dof_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float], max_allowed: int) -> CollisionCacheData
Check collisions at dof_vals, keeping at most max_allowed contacts per link pair; returns the contacts and their gradient result sets
getCollisionMarginData
¶
SingleTimestepCollisionEvaluator
¶
Bases: DiscreteCollisionEvaluator
__init__
¶
__init__(manip: JointGroup, env: Environment, collision_config: TrajOptCollisionConfig, dynamic_environment: bool = False) -> None
DiscreteCollisionConstraint
¶
Bases: ConstraintSet
ContinuousCollisionEvaluator
¶
calcCollisionData
¶
calcCollisionData(dof_vals0: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float], dof_vals1: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float], vars0_fixed: bool, vars1_fixed: bool, max_allowed: int) -> CollisionCacheData
Check collisions along the segment dof_vals0 -> dof_vals1, keeping at most max_allowed contacts per link pair; returns the contacts and their gradient result sets
getCollisionMarginData
¶
LVSDiscreteCollisionEvaluator
¶
Bases: ContinuousCollisionEvaluator
__init__
¶
__init__(manip: JointGroup, env: Environment, collision_config: TrajOptCollisionConfig, dynamic_environment: bool = False) -> None
LVSContinuousCollisionEvaluator
¶
Bases: ContinuousCollisionEvaluator
__init__
¶
__init__(manip: JointGroup, env: Environment, collision_config: TrajOptCollisionConfig, dynamic_environment: bool = False) -> None
ContinuousCollisionConstraint
¶
Bases: ConstraintSet
__init__
¶
__init__(collision_evaluator: ContinuousCollisionEvaluator, position_var0: Var, position_var1: Var, fixed0: bool = False, fixed1: bool = False, max_num_cnt: int = 1, fixed_sparsity: bool = False, name: str = 'LVSCollision') -> None
setBounds
¶
Set the bounds, one per row; raises BoundsSizeError otherwise
DiscreteCollisionConstraintD
¶
Bases: ConstraintSet
ContinuousCollisionConstraintD
¶
Bases: ConstraintSet
__init__
¶
__init__(collision_evaluator: ContinuousCollisionEvaluator, position_var0: Var, position_var1: Var, fixed0: bool = False, fixed1: bool = False, name: str = 'LVSCollision') -> None
Create a continuous collision constraint with one row per contact (rows change on update())
setBounds
¶
Set the bounds, one per row; raises BoundsSizeError otherwise
ContinuousCollisionNumericalConstraint
¶
Bases: ConstraintSet
__init__
¶
__init__(collision_evaluator: ContinuousCollisionEvaluator, position_var0: Var, position_var1: Var, fixed0: bool = False, fixed1: bool = False, max_num_cnt: int = 1, fixed_sparsity: bool = False, name: str = 'LVSCollision') -> None
Create continuous collision constraint with numerical jacobians
setBounds
¶
Set the bounds, one per row; raises BoundsSizeError otherwise
DiscreteCollisionNumericalConstraint
¶
Bases: ConstraintSet
__init__
¶
__init__(collision_evaluator: DiscreteCollisionEvaluator, position_var: Var, max_num_cnt: int = 1, fixed_sparsity: bool = False, name: str = 'DiscreteCollisionNumerical') -> None
Create discrete collision constraint with numerical jacobians
setBounds
¶
Set the bounds, one per row; raises BoundsSizeError otherwise
calcValues
¶
calcValues(joint_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]
calcJacobianBlock
¶
calcJacobianBlock(joint_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> scipy.sparse.csr_matrix
The constraint Jacobian at joint_vals (sparse matrix, sized like getJacobian()); needs linked variables and an update() since the last variable change
JointJerkConstraint
¶
Bases: ConstraintSet
getValues
¶
Get current constraint values
CartLineConstraint
¶
Bases: ConstraintSet
use_numeric_differentiation
property
writable
¶
If true, use numeric differentiation (default: true)
__init__
¶
__init__(info: CartLineInfo, position_var: Var, coeffs: Annotated[NDArray[float64], dict(shape=(None,), order=C)] | Sequence[float], name: str = 'CartLine') -> None
Create Cartesian line constraint
getValues
¶
Get current constraint values
setBounds
¶
Set the bounds, one per row; raises BoundsSizeError otherwise
getLine
¶
getLine() -> tuple[tesseract_robotics.common.Isometry3d, tesseract_robotics.common.Isometry3d]
The line's two end poses (target_frame_offset1, target_frame_offset2)
getJacobian
¶
Get the constraint Jacobian (sparse matrix), one row per index
calcJacobianBlock
¶
calcJacobianBlock(joint_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> scipy.sparse.csr_matrix
The constraint Jacobian at joint_vals (sparse matrix, sized like getJacobian()); needs linked variables
calcValues
¶
calcValues(joint_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]
Calculate error values for given joint values
getLinePoint
staticmethod
¶
getLinePoint(source_tf: Isometry3d, target_tf1: Isometry3d, target_tf2: Isometry3d) -> tesseract_robotics.common.Isometry3d
Find nearest point on line (uses SLERP for orientation)
CartAxisKinematics
¶
A source link axis expressed in a target link's coordinates, v(q) = R_target(q)^T R_source(q) a, and its 3 x n Jacobian.
__init__
¶
__init__(manip: JointGroup, source_frame: LinkId | str, source_axis: Annotated[NDArray[float64], dict(shape=3, order=C)] | Sequence[float], target_frame: LinkId | str) -> None
source_axis is in source link coordinates and normalized. Raises on a link that is not in manip or an invalid axis.
calcAxis
¶
calcAxis(joint_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> Annotated[NDArray[numpy.float64], dict(shape=3, order=C)]
v at joint_vals
calcAxisAndJacobian
¶
calcAxisAndJacobian(joint_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> tuple[Annotated[NDArray[numpy.float64], dict(shape=3, order=C)], Annotated[NDArray[numpy.float64], dict(shape=(3, None), order=F)]]
(v, dv/dq) at joint_vals; dv/dq is 3 x numJoints()
getSourceAxis
¶
The unit source axis, in source link coordinates
mostReachableDirection
staticmethod
¶
mostReachableDirection(tangent_jacobian: Annotated[NDArray[float64], dict(shape=(2, None), order=F)] | Sequence[Sequence[float]]) -> Annotated[NDArray[numpy.float64], dict(shape=2, order=C)]
The unit 2D direction the 2 x n tangent Jacobian moves along most per unit joint step
toUnitAxis
staticmethod
¶
toUnitAxis(axis: Annotated[NDArray[float64], dict(shape=3, order=C)] | Sequence[float], what: str) -> Annotated[NDArray[numpy.float64], dict(shape=3, order=C)]
axis normalized; raises DegenerateAxisError (a ValueError) naming what if it is zero or not finite
CartAxisConeConstraint
¶
Bases: ConstraintSet
Keeps the angle between a source link axis and a target link axis at most half_angle: one one-sided row, phi(q) = atan2(|a_tgt x v|, a_tgt . v) <= half_angle, in radians. Rotation about the source axis stays free. For half_angle near 0 use CartAxisAlignConstraint.
__init__
¶
__init__(position_var: Var, manip: JointGroup, source_frame: LinkId | str, source_axis: Annotated[NDArray[float64], dict(shape=3, order=C)] | Sequence[float], target_frame: LinkId | str, target_axis: Annotated[NDArray[float64], dict(shape=3, order=C)] | Sequence[float], half_angle: float, coeff: float = 1.0, name: str = 'CartAxisCone') -> None
Axes are in their links' coordinates and normalized; 0 < half_angle < pi. Raises on an invalid link, axis, half angle or coefficient.
calcValues
¶
calcValues(joint_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]
The angle phi between the axes for the given joint values
calcJacobianBlock
¶
calcJacobianBlock(joint_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> scipy.sparse.csr_matrix
The constraint Jacobian at joint_vals (sparse matrix, sized like getJacobian()); needs linked variables
setTargetAxis
¶
setTargetAxis(target_axis: Annotated[NDArray[float64], dict(shape=3, order=C)] | Sequence[float]) -> None
Set the cone axis, in target link coordinates
getTargetAxis
¶
The cone axis, normalized
CartAxisAlignConstraint
¶
Bases: ConstraintSet
Points a source link axis along a target link axis; rotation about the axis is free. Two equality rows, in radians: the tilt angle times the unit tilt direction, zero only when the axes coincide.
__init__
¶
__init__(position_var: Var, manip: JointGroup, source_frame: LinkId | str, source_axis: Annotated[NDArray[float64], dict(shape=3, order=C)] | Sequence[float], target_frame: LinkId | str, target_axis: Annotated[NDArray[float64], dict(shape=3, order=C)] | Sequence[float], coeff: float = 1.0, name: str = 'CartAxisAlign') -> None
Axes are in their links' coordinates and normalized. Raises on an invalid link, axis or coefficient.
calcValues
¶
calcValues(joint_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]
The two tilt rows for the given joint values
calcJacobianBlock
¶
calcJacobianBlock(joint_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> scipy.sparse.csr_matrix
The constraint Jacobian at joint_vals (sparse matrix, sized like getJacobian()); needs linked variables
setTargetAxis
¶
setTargetAxis(target_axis: Annotated[NDArray[float64], dict(shape=3, order=C)] | Sequence[float]) -> None
Set the axis to align with, in target link coordinates
getTargetAxis
¶
The axis to align with, normalized
InverseKinematicsInfo
¶
InverseKinematicsConstraint
¶
Bases: ConstraintSet
__init__
¶
__init__(target_pose: Isometry3d, kinematic_info: InverseKinematicsInfo, constraint_var: Var, seed_var: Var, name: str = 'InverseKinematics') -> None
Create IK constraint (constraint_var constrained to IK solution seeded from seed_var)
getValues
¶
Get current constraint values
setBounds
¶
Set the bounds, one per row; raises BoundsSizeError otherwise
calcJacobianBlock
¶
calcJacobianBlock(joint_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> scipy.sparse.csr_matrix
The constraint Jacobian at joint_vals, the constraint var's values (sparse matrix, sized like getJacobian()); needs linked variables
setTargetPose
¶
Set the target pose the IK solution is computed for
getKinematicInfo
¶
The constraint's InverseKinematicsInfo
createNodesVariables
¶
createNodesVariables(variable_name: str, joint_names: Sequence[str], initial_values: Sequence[Annotated[NDArray[float64], dict(shape=(None,), order=C)] | Sequence[float]], bounds: Sequence[Bounds]) -> NodesVariables
Create NodesVariables with one vector Var per node.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
variable_name
|
str
|
Name for the variable set |
required |
joint_names
|
Sequence[str]
|
Joint name for each DOF |
required |
initial_values
|
Sequence[Annotated[NDArray[float64], dict(shape=(None,), order=C)] | Sequence[float]]
|
List of initial joint value vectors (one per waypoint) |
required |
bounds
|
Sequence[Bounds]
|
Joint bounds (shared across all waypoints) |
required |
interpolate
¶
interpolate(start: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float], end: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float], steps: int) -> list[Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]]
Interpolate between two joint vectors, returning 'steps' waypoints
getClosestValidPoint
¶
getClosestValidPoint(input: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float], bounds: Sequence[Bounds]) -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]
input clamped into bounds, entry by entry
calcBoundsErrors
¶
calcBoundsErrors(input: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float], bounds: Sequence[Bounds]) -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]
Signed bound error per entry: input - lower below the lower bound (negative), input - upper above the upper bound (positive), else 0
calcBoundsViolations
¶
calcBoundsViolations(input: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float], bounds: Sequence[Bounds]) -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]
Bound violation per entry: the magnitude of calcBoundsErrors
calcNumericalConstraintGradient
¶
calcNumericalConstraintGradient(variables: Variables, constraint_set: ConstraintSet, epsilon: float = 1e-08) -> scipy.sparse.csr_matrix
Forward-difference Jacobian of constraint_set's values over variables (sparse matrix). Perturbs each variable in turn through variables.setVariables, calling update() and getValues(), and restores the variables afterwards.
calcForwardNumJac
¶
calcForwardNumJac(f: Callable[[Annotated[NDArray[float64], dict(shape=(None,), writable=False)]], Annotated[NDArray[float64], dict(shape=(None,), order=C)] | Sequence[float]], x: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float], epsilon: float) -> scipy.sparse.csr_matrix
Forward-difference Jacobian of f at x (sparse matrix, len(f(x)) x len(x)); raises NumericalJacobianSizeError if f's output length changes
toTrajArray
¶
toTrajArray(joint_positions: Sequence[Var]) -> Annotated[NDArray[numpy.float64], dict(shape=(None, None), order=C)]
The Vars' values as a waypoints x joints matrix, one row per Var
toJointTrajectory
¶
toJointTrajectory(joint_positions: Sequence[Var]) -> tesseract_robotics.common.JointTrajectory
A JointTrajectory with one JointState (the Var's names and values) per Var
See also¶
- Low-Level SQP API — user-guide walkthrough
- 0.33 → 0.34 migration — if you're porting from the old
ifoptmodule