Skip to content

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.

Python
from tesseract_robotics.trajopt_ifopt import Bounds, Node, NodesVariables

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.

Python
from tesseract_robotics.trajopt_ifopt import Bounds, createNodesVariables

bounds = Bounds(-3.14, 3.14)
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. As a cost, the set's row bounds decide the penalty type: SQUARED and ABSOLUTE need equality bounds on every row, HINGE one-sided bounds, and addCostSet raises otherwise. There is no cost without a penalty type. The last column lists what addCostSet accepts. 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 models only SQUARED costs as it charges them: it reads an ABSOLUTE or HINGE cost as 0 at every QP solution, and that cost's exact value ignores its coefficient (see the QP problem). trajopt#592 fixes both; the fix arrives only with a wheel built on a newer trajopt (none yet).

Class Purpose Row bounds As a cost
JointPosConstraint (target) Joint values at a waypoint equality SQUARED, ABSOLUTE
JointPosConstraint (bounds) Per-joint bounds at a waypoint a range splits into two one-sided rows (the default) HINGE if every row is one-sided; SQUARED, ABSOLUTE if every row is an equality
JointVelConstraint Joint velocity toward a target (zero for smoothing) equality SQUARED, ABSOLUTE
JointAccelConstraint Joint acceleration toward a target equality SQUARED, ABSOLUTE
JointJerkConstraint Joint jerk toward a target equality SQUARED, ABSOLUTE
CartPosConstraint (pose) TCP pose at a waypoint equality SQUARED, ABSOLUTE
CartPosConstraint (per-axis) TCP pose with free axes and bands per axis see per-axis Cartesian constraints
CartLineConstraint TCP on a line segment between two poses equality SQUARED, ABSOLUTE
DiscreteCollisionConstraint Collision at a waypoint one-sided (upper) HINGE
DiscreteCollisionNumericalConstraint The same, with a numerical Jacobian one-sided (upper) HINGE
ContinuousCollisionConstraint Collision along the segment between two waypoints one-sided (upper) HINGE
InverseKinematicsConstraint Joints toward an IK solution of a target pose equality SQUARED, ABSOLUTE
DiscreteCollisionConstraintD Collision at a waypoint, one row per contact (the TrajOpt-Ifopt planner's term) one-sided (upper) HINGE
ContinuousCollisionConstraintD Collision along a segment, one row per contact one-sided (upper) HINGE
ContinuousCollisionNumericalConstraint ContinuousCollisionConstraint with a numerical Jacobian one-sided (upper) HINGE

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: addCostSet raises, and a constraint raises Unsupported bounds type! at the first convexify(). Measured on trajopt 0.35.0: each form in the table was 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.

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(), which TrajOptQPProblem.setup() calls for every set.

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.getJacobian() raises CartLineIndicesUnsupportedError (a ValueError) unless CartLineInfo.indices holds all 6 twist rows. Upstream sizes the Jacobian by the number of indices but fills 6 rows, numeric and analytic alike, so fewer corrupt memory (#237). Construction, getValues() and calcValues() work with 1 to 6 indices. The check covers direct Python calls only: TrajOptQPProblem calls the C++ Jacobian, so a solve with fewer than 6 indices can still crash. Use all 6 indices for a constraint you solve. The check goes once the pinned trajopt includes the upstream fix (trajopt#618).

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 and DiscreteCollisionNumericalConstraint evaluates the Jacobian at given joint values, sized like getJacobian(), so it also needs linked variables; a wrong joint_vals length raises JointValuesSizeError. InverseKinematicsConstraint declares the method, but trajopt 0.35.0 never defines it, so it is not bound. Nor are the list[Bounds] constructors of JointVelConstraint, JointAccelConstraint and JointJerkConstraint, for the same reason.

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.tesseract_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.

Python
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) — re-exported here and from tesseract_motion_planners_trajopt.
  • CollisionCoeffData — per-pair collision coefficient data. getCollisionCoeffPairData() returns {(link_a, link_b): coeff}, getPairsWithZeroCoeff() the switched-off pairs.
  • CollisionCoeffData and TrajOptCollisionConfig compare by value and are unhashable.
  • Bounds(lower, upper) — single-variable bounds. b += s / b -= s shift both limits and reclassify getType().
  • Enums: BoundsType, RangeBoundHandling.

Utilities

  • interpolate(start, end, steps) — linear joint interpolation.
  • toBounds(joint_limits) — convert a limits matrix into a Bounds list.
  • getClosestValidPoint(x, bounds), calcBoundsErrors(x, bounds), calcBoundsViolations(x, bounds) — clamp into bounds, the signed error and the violation per entry; one bound per entry or BoundsSizeError.
  • toTrajArray(vars) / toJointTrajectory(vars) — the vars' values as a waypoints × joints matrix, or a JointTrajectory with one state per var.
  • calcNumericalConstraintGradient(variables, constraint_set, epsilon=1e-8) and calcForwardNumJac(f, x, epsilon) — forward-difference Jacobians, to check an analytic getJacobian() (a Python ConstraintSet subclass's included).
Python
numerical = calcNumericalConstraintGradient(nodes_variables, my_constraint)
assert np.allclose(numerical.toarray(), my_constraint.getJacobian().toarray(), atol=1e-6)

Module API

tesseract_robotics.trajopt_ifopt

ConstraintNotLinkedError

Bases: RuntimeError

getJacobian() on a constraint set that is not linked to variables yet.

CartLineIndicesUnsupportedError

Bases: ValueError

CartLineConstraint.getJacobian() with fewer than 6 indices (#237).

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

UNBOUNDED class-attribute instance-attribute
Python
UNBOUNDED = 0
EQUALITY class-attribute instance-attribute
Python
EQUALITY = 1
LOWER_BOUND class-attribute instance-attribute
Python
LOWER_BOUND = 2
UPPER_BOUND class-attribute instance-attribute
Python
UPPER_BOUND = 3
RANGE_BOUND class-attribute instance-attribute
Python
RANGE_BOUND = 4

RangeBoundHandling

Bases: Enum

KEEP_AS_IS class-attribute instance-attribute
Python
KEEP_AS_IS = 0
SPLIT_TO_TWO_INEQUALITIES class-attribute instance-attribute
Python
SPLIT_TO_TWO_INEQUALITIES = 1

Bounds

__init__
Python
__init__(lower: float = 0.0, upper: float = 0.0) -> None

Create bounds with lower and upper limits

set
Python
set(lower: float, upper: float) -> None

Set both lower and upper bounds

setLower
Python
setLower(lower: float) -> None

Set the lower bound

getLower
Python
getLower() -> float

Get the lower bound value

setUpper
Python
setUpper(upper: float) -> None

Set the upper bound

getUpper
Python
getUpper() -> float

Get the upper bound value

getType
Python
getType() -> BoundsType

Get the cached bound classification

__iadd__
Python
__iadd__(scalar: float) -> Bounds

Shift both limits up by scalar and reclassify the bounds type

__isub__
Python
__isub__(scalar: float) -> Bounds

Shift both limits down by scalar and reclassify the bounds type

Component

getName
Python
getName() -> str

Get the component name

getRows
Python
getRows() -> int

Get the number of rows

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

Get the current values (variables, or constraint / cost values)

getBounds
Python
getBounds() -> list[Bounds]

Get the bounds, one per row

Variables

Bases: Component

setVariables
Python
setVariables(x: Annotated[NDArray[float64], dict(shape=(None,), order=C)] | Sequence[float]) -> None

Set the variable values from a flat decision vector

getHash
Python
getHash() -> int

Get the variable hash for caching

Differentiable

Bases: Component

Mode

Bases: Enum

STACK_ROWS class-attribute instance-attribute
Python
STACK_ROWS = 0

One row per value

SUM_TO_SCALAR class-attribute instance-attribute
Python
SUM_TO_SCALAR = 1

All values summed into one row

getJacobian
Python
getJacobian() -> scipy.sparse.csr_matrix[float]

Get the Jacobian (sparse matrix); a constraint set must be linked to variables first

isDynamic
Python
isDynamic() -> bool

Whether this component can change its number of rows

isScalar
Python
isScalar() -> bool

Whether this component is scalar or stacked

getNonZeros
Python
getNonZeros() -> int

Estimated number of non-zero entries in the Jacobian

update
Python
update() -> int

Recompute sizing/state for dynamic-sized components

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

Get the coefficient vector

ConstraintSet

Bases: Differentiable

__init__
Python
__init__(name: str, n_constraints: int) -> None

Create a constraint set with name and number of constraints

linkWithVariables
Python
linkWithVariables(x: Variables) -> None

Connect the constraint with the optimization variables

getVariables
Python
getVariables() -> Variables

Get the linked optimization variables (protected in C++, exposed via trampoline)

getJacobian
Python
getJacobian() -> scipy.sparse.csr_matrix[float]

Get the constraint Jacobian (sparse matrix)

update
Python
update() -> int

Recompute sizing/state for dynamic-sized components

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

Get the coefficient vector

Var

getIdentifier
Python
getIdentifier() -> str

Get the identifier for the variable segment

getParent
Python
getParent() -> Node | None

The Node holding this variable, or None for a standalone Var

getIndex
Python
getIndex() -> int

Get the starting index of this variable segment

size
Python
size() -> int

Get the number of elements in this variable segment

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

Get the current variable values

name
Python
name() -> list[str]

Get the variable names

bounds
Python
bounds() -> list[Bounds]

Get the variable bounds

setVariables
Python
setVariables(x: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> None

Set the variable values from the full decision vector

Node

__init__
Python
__init__(node_name: str = 'Node') -> None

Create a node with an optional name

getName
Python
getName() -> str

Get the name of this node

getParent
Python
getParent() -> NodesVariables | None

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

hasVar
Python
hasVar(name: str) -> bool

Check whether this node has a variable by name

getVar
Python
getVar(name: str) -> Var

Get a variable by name

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

Get all variable values as a single vector

getBounds
Python
getBounds() -> list[Bounds]

Get all variable bounds

size
Python
size() -> int

Get total number of scalar decision variables

setVariables
Python
setVariables(x: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> None

Update all variable values from the full decision vector

NodesVariables

Bases: Variables

__init__
Python
__init__(variable_name: str, nodes: Sequence[Node]) -> None

Create the variable set from its nodes, in order; takes ownership of the nodes, which become unusable through the caller's references (read them back with getNode / getNodes). Raises NodeOwnershipError for a Node given twice or already in a set

getNode
Python
getNode(opt_idx: int) -> Node

Get node based on index

getNodes
Python
getNodes() -> list[Node]

Get all nodes

getDim
Python
getDim() -> int

Get the dimensions of every node

CartPosConstraint

Bases: ConstraintSet

use_numeric_differentiation property writable
Python
use_numeric_differentiation: bool

If true, use numeric differentiation (default: true)

calcValues
Python
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
Python
calcJacobianBlock(joint_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> scipy.sparse.csr_matrix[float]

The constraint Jacobian at joint_vals (sparse matrix, sized like getJacobian()); needs linked variables

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

Get current constraint values

setTargetPose
Python
setTargetPose(target_frame_offset: Isometry3d) -> None

Set the target pose - critical for online planning!

getTargetPose
Python
getTargetPose() -> tesseract_robotics.tesseract_common._tesseract_common.Isometry3d

Get the target pose for the constraint

getCurrentPose
Python
getCurrentPose() -> tesseract_robotics.tesseract_common._tesseract_common.Isometry3d

Get current TCP pose in world frame

JointPosConstraint

Bases: ConstraintSet

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

Get current constraint values

JointVelConstraint

Bases: ConstraintSet

__init__
Python
__init__(targets: Annotated[NDArray[float64], dict(shape=(None,), order=C)] | Sequence[float], position_vars: Sequence[Var], coeffs: Annotated[NDArray[float64], dict(shape=(None,), order=C)] | Sequence[float], name: str = 'JointVel') -> None

Create joint velocity constraint with target values

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

Get current constraint values

JointAccelConstraint

Bases: ConstraintSet

__init__
Python
__init__(targets: Annotated[NDArray[float64], dict(shape=(None,), order=C)] | Sequence[float], position_vars: Sequence[Var], coeffs: Annotated[NDArray[float64], dict(shape=(None,), order=C)] | Sequence[float], name: str = 'JointAccel') -> None

Create joint acceleration constraint with target values

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

Get current constraint values

CollisionCoeffData

__hash__ class-attribute instance-attribute
Python
__hash__: None = None
__eq__
Python
__eq__(arg: CollisionCoeffData) -> bool
__ne__
Python
__ne__(arg: CollisionCoeffData) -> bool
setDefaultCollisionCoeff
Python
setDefaultCollisionCoeff(arg: float) -> None
getDefaultCollisionCoeff
Python
getDefaultCollisionCoeff() -> float
setCollisionCoeff
Python
setCollisionCoeff(obj1: str, obj2: str, collision_coeff: float) -> None
getCollisionCoeff
Python
getCollisionCoeff(obj1: str, obj2: str) -> float
getCollisionCoeffPairData
Python
getCollisionCoeffPairData() -> dict[tuple[str, str], float]

Every pair with a coefficient: {(link_a, link_b): coeff}

getPairsWithZeroCoeff
Python
getPairsWithZeroCoeff() -> set[tuple[str, str]]

The pairs whose coefficient is 0 (collision checking switched off)

TrajOptCollisionConfig

__hash__ class-attribute instance-attribute
Python
__hash__: None = None
enabled property writable
Python
enabled: bool
contact_manager_config property writable
Python
contact_manager_config: ContactManagerConfig
collision_check_config property writable
Python
collision_check_config: CollisionCheckConfig
collision_coeff_data property writable
Python
collision_coeff_data: CollisionCoeffData
collision_margin_buffer property writable
Python
collision_margin_buffer: float
max_num_cnt property writable
Python
max_num_cnt: int
__eq__
Python
__eq__(arg: TrajOptCollisionConfig) -> bool
__ne__
Python
__ne__(arg: TrajOptCollisionConfig) -> bool

LinkGradientResults

has_gradient property writable
Python
has_gradient: bool
gradient property writable
Python
gradient: Annotated[NDArray[float64], dict(shape=(None,), order=C)]
translation_vector property writable
Python
translation_vector: Annotated[NDArray[float64], dict(shape=(None,), order=C)]
jacobian property writable
Python
jacobian: Annotated[NDArray[float64], dict(shape=(None, None), order=F)]
scale property writable
Python
scale: float
cc_type property writable
Python
cc_type: ContinuousCollisionType
__init__
Python
__init__() -> None
clear
Python
clear() -> None

GradientResults

gradients property writable
Python
gradients: list[LinkGradientResults]
cc_gradients property writable
Python
cc_gradients: list[LinkGradientResults]
error property writable
Python
error: float
error_with_buffer property writable
Python
error_with_buffer: float
__init__
Python
__init__() -> None
clear
Python
clear() -> None

LinkMaxError

has_error property writable
Python
has_error: list[bool]
error property writable
Python
error: list[float]
error_with_buffer property writable
Python
error_with_buffer: list[float]
__init__
Python
__init__() -> None
getMaxError
Python
getMaxError() -> float
getMaxErrorWithBuffer
Python
getMaxErrorWithBuffer() -> float

GradientResultsSet

key property writable
Python
key: tuple[str, str]
shape_key property writable
Python
shape_key: tuple[int, int]
coeff property writable
Python
coeff: float
is_continuous property writable
Python
is_continuous: bool
max_error property writable
Python
max_error: list[LinkMaxError]
results property writable
Python
results: list[GradientResults]
add
Python
add(gradient_result: GradientResults) -> None
getMaxError
Python
getMaxError() -> float
getMaxErrorT0
Python
getMaxErrorT0() -> float
getMaxErrorT1
Python
getMaxErrorT1() -> float
getMaxErrorWithBuffer
Python
getMaxErrorWithBuffer() -> float
getMaxErrorWithBufferT0
Python
getMaxErrorWithBufferT0() -> float
getMaxErrorWithBufferT1
Python
getMaxErrorWithBufferT1() -> float

CollisionCacheData

contact_results_map property writable
Python
contact_results_map: ContactResultMap
gradient_results_sets property writable
Python
gradient_results_sets: list[GradientResultsSet]
__init__
Python
__init__() -> None

DiscreteCollisionEvaluator

calcCollisions
Python
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

getGradient
Python
getGradient(dofvals: Annotated[NDArray[float64], dict(shape=(None,), order=C)] | Sequence[float], contact_result: ContactResult) -> GradientResults

Gradient of one contact at dofvals

getJointGroup
Python
getJointGroup() -> tesseract_robotics.tesseract_kinematics._tesseract_kinematics.JointGroup
getCollisionMarginBuffer
Python
getCollisionMarginBuffer() -> float
getCollisionMarginData
Python
getCollisionMarginData() -> tesseract_robotics.tesseract_common._tesseract_common.CollisionMarginData
getCollisionCoeffData
Python
getCollisionCoeffData() -> CollisionCoeffData

SingleTimestepCollisionEvaluator

Bases: DiscreteCollisionEvaluator

__init__
Python
__init__(manip: JointGroup, env: Environment, collision_config: TrajOptCollisionConfig, dynamic_environment: bool = False) -> None

DiscreteCollisionConstraint

Bases: ConstraintSet

__init__
Python
__init__(collision_evaluator: DiscreteCollisionEvaluator, position_var: Var, max_num_cnt: int = 1, fixed_sparsity: bool = False, name: str = 'DiscreteCollision') -> None
getValues
Python
getValues() -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]
setBounds
Python
setBounds(bounds: Sequence[Bounds]) -> None

Set the bounds, one per row; raises BoundsSizeError otherwise

getCollisionEvaluator
Python
getCollisionEvaluator() -> DiscreteCollisionEvaluator

ContinuousCollisionEvaluator

calcCollisionData
Python
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

calcGradientData
Python
calcGradientData(dof_vals0: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float], dof_vals1: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float], contact_results: ContactResult) -> GradientResults

Gradient of one contact along the segment dof_vals0 -> dof_vals1

getJointGroup
Python
getJointGroup() -> tesseract_robotics.tesseract_kinematics._tesseract_kinematics.JointGroup
getCollisionMarginBuffer
Python
getCollisionMarginBuffer() -> float
getCollisionMarginData
Python
getCollisionMarginData() -> tesseract_robotics.tesseract_common._tesseract_common.CollisionMarginData
getCollisionCoeffData
Python
getCollisionCoeffData() -> CollisionCoeffData

LVSDiscreteCollisionEvaluator

Bases: ContinuousCollisionEvaluator

__init__
Python
__init__(manip: JointGroup, env: Environment, collision_config: TrajOptCollisionConfig, dynamic_environment: bool = False) -> None

LVSContinuousCollisionEvaluator

Bases: ContinuousCollisionEvaluator

__init__
Python
__init__(manip: JointGroup, env: Environment, collision_config: TrajOptCollisionConfig, dynamic_environment: bool = False) -> None

ContinuousCollisionConstraint

Bases: ConstraintSet

__init__
Python
__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
getValues
Python
getValues() -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]
setBounds
Python
setBounds(bounds: Sequence[Bounds]) -> None

Set the bounds, one per row; raises BoundsSizeError otherwise

getCollisionEvaluator
Python
getCollisionEvaluator() -> ContinuousCollisionEvaluator

DiscreteCollisionConstraintD

Bases: ConstraintSet

__init__
Python
__init__(collision_evaluator: DiscreteCollisionEvaluator, position_var: Var, name: str = 'DiscreteCollision') -> None

Create a discrete collision constraint with one row per contact (rows change on update())

getValues
Python
getValues() -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]
setBounds
Python
setBounds(bounds: Sequence[Bounds]) -> None

Set the bounds, one per row; raises BoundsSizeError otherwise

getCollisionEvaluator
Python
getCollisionEvaluator() -> DiscreteCollisionEvaluator

ContinuousCollisionConstraintD

Bases: ConstraintSet

__init__
Python
__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())

getValues
Python
getValues() -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]
setBounds
Python
setBounds(bounds: Sequence[Bounds]) -> None

Set the bounds, one per row; raises BoundsSizeError otherwise

getCollisionEvaluator
Python
getCollisionEvaluator() -> ContinuousCollisionEvaluator

ContinuousCollisionNumericalConstraint

Bases: ConstraintSet

__init__
Python
__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

getValues
Python
getValues() -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]
setBounds
Python
setBounds(bounds: Sequence[Bounds]) -> None

Set the bounds, one per row; raises BoundsSizeError otherwise

getCollisionEvaluator
Python
getCollisionEvaluator() -> ContinuousCollisionEvaluator

JointJerkConstraint

Bases: ConstraintSet

__init__
Python
__init__(targets: Annotated[NDArray[float64], dict(shape=(None,), order=C)] | Sequence[float], position_vars: Sequence[Var], coeffs: Annotated[NDArray[float64], dict(shape=(None,), order=C)] | Sequence[float], name: str = 'JointJerk') -> None

Create joint jerk constraint (requires 4+ consecutive waypoints)

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

Get current constraint values

CartLineInfo

manip property writable
Python
manip: JointGroup

The joint group

source_frame property writable
Python
source_frame: str

TCP frame

target_frame property writable
Python
target_frame: str

Reference frame

source_frame_offset property writable
Python
source_frame_offset: Isometry3d

TCP offset

target_frame_offset1 property writable
Python
target_frame_offset1: Isometry3d

Line start pose

target_frame_offset2 property writable
Python
target_frame_offset2: Isometry3d

Line end pose

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

DOF indices to constrain: default {0,1,2,3,4,5}

CartLineConstraint

Bases: ConstraintSet

use_numeric_differentiation property writable
Python
use_numeric_differentiation: bool

If true, use numeric differentiation (default: true)

__init__
Python
__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
Python
getValues() -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]

Get current constraint values

setBounds
Python
setBounds(bounds: Sequence[Bounds]) -> None

Set the bounds, one per row; raises BoundsSizeError otherwise

getLine
Python
getLine() -> tuple[tesseract_robotics.tesseract_common._tesseract_common.Isometry3d, tesseract_robotics.tesseract_common._tesseract_common.Isometry3d]

The line's two end poses (target_frame_offset1, target_frame_offset2)

getInfo
Python
getInfo() -> CartLineInfo

The constraint's CartLineInfo

getJacobian
Python
getJacobian() -> scipy.sparse.csr_matrix[float]

Get the constraint Jacobian (sparse matrix); raises CartLineIndicesUnsupportedError unless all 6 indices are set (#237)

calcJacobianBlock
Python
calcJacobianBlock(joint_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> scipy.sparse.csr_matrix[float]

The constraint Jacobian at joint_vals (sparse matrix, sized like getJacobian()); needs linked variables and all 6 indices (#237)

calcValues
Python
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
Python
getLinePoint(source_tf: Isometry3d, target_tf1: Isometry3d, target_tf2: Isometry3d) -> tesseract_robotics.tesseract_common._tesseract_common.Isometry3d

Find nearest point on line (uses SLERP for orientation)

DiscreteCollisionNumericalConstraint

Bases: ConstraintSet

__init__
Python
__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

getValues
Python
getValues() -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]
setBounds
Python
setBounds(bounds: Sequence[Bounds]) -> None

Set the bounds, one per row; raises BoundsSizeError otherwise

calcValues
Python
calcValues(joint_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]
calcJacobianBlock
Python
calcJacobianBlock(joint_vals: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> scipy.sparse.csr_matrix[float]

The constraint Jacobian at joint_vals (sparse matrix, sized like getJacobian()); needs linked variables and an update() since the last variable change

getCollisionEvaluator
Python
getCollisionEvaluator() -> DiscreteCollisionEvaluator

InverseKinematicsInfo

manip property writable
Python
manip: KinematicGroup

The kinematic group (with IK solver)

working_frame property writable
Python
working_frame: str

Working frame (not currently used)

tcp_frame property writable
Python
tcp_frame: str

TCP frame (not currently used)

tcp_offset property writable
Python
tcp_offset: Isometry3d

TCP offset (not currently used)

InverseKinematicsConstraint

Bases: ConstraintSet

__init__
Python
__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
Python
getValues() -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]

Get current constraint values

setBounds
Python
setBounds(bounds: Sequence[Bounds]) -> None

Set the bounds, one per row; raises BoundsSizeError otherwise

setTargetPose
Python
setTargetPose(target_pose: Isometry3d) -> None

Set the target pose the IK solution is computed for

getKinematicInfo
Python
getKinematicInfo() -> InverseKinematicsInfo

The constraint's InverseKinematicsInfo

createNodesVariables

Python
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

Python
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

Python
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

Python
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

Python
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

Python
calcNumericalConstraintGradient(variables: Variables, constraint_set: ConstraintSet, epsilon: float = 1e-08) -> scipy.sparse.csr_matrix[float]

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

Python
calcForwardNumJac(f: Callable[[Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]], 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[float]

Forward-difference Jacobian of f at x (sparse matrix, len(f(x)) x len(x)); raises NumericalJacobianSizeError if f's output length changes

toTrajArray

Python
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

Python
toJointTrajectory(joint_positions: Sequence[Var]) -> tesseract_robotics.tesseract_common._tesseract_common.JointTrajectory

A JointTrajectory with one JointState (the Var's names and values) per Var

See also