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

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.

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

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

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, 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.
  • 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
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

Python
from tesseract_robotics.trajopt_ifopt.python_constraints import PointsDown

for var in vars_list:
    problem.addConstraintSet(PointsDown(var, manip, "tool0", max_tilt=np.radians(20)))

DEFAULT_EPSILON module-attribute

Python
DEFAULT_EPSILON = 1e-06

CLEARANCE_MARGIN_PER_D_MAX module-attribute

Python
CLEARANCE_MARGIN_PER_D_MAX = 2.0

WORLD_HOLD_TOL_M module-attribute

Python
WORLD_HOLD_TOL_M = 0.001

BoundsShapeError

Bases: ValueError

lower and upper have different numbers of rows.

Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Python
class BoundsShapeError(ValueError):
    """`lower` and `upper` have different numbers of rows."""

InvertedBoundsError

Bases: ValueError

A row's lower bound is above its upper bound.

Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Python
class InvertedBoundsError(ValueError):
    """A row's lower bound is above its upper bound."""

ClearanceMarginError

Bases: ValueError

Clearance's d_max is not below its contact margin, so the upper bound is never seen.

Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Python
class ClearanceMarginError(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
class ConstraintOptions(TypedDict, total=False):
    """The `PyConstraint` options every rule passes through.

    Attributes:
        epsilon: Forward-difference step of the default Jacobian [joint units].
        coeff: Merit coefficient per row (scalar or one per row).
    """

    epsilon: float
    coeff: float | Sequence[float]

epsilon instance-attribute

Python
epsilon: float

coeff instance-attribute

Python
coeff: float | Sequence[float]

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 Var (node.getVar("joints"))

required
lower ArrayLike

lower bound per row; -np.inf for none

required
upper ArrayLike

upper bound per row; np.inf for none

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
class PyConstraint(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.

    Args:
        name: name of the constraint set (shown in the solver's output)
        var: the waypoint's `Var` (`node.getVar("joints")`)
        lower: lower bound per row; `-np.inf` for none
        upper: upper bound per row; `np.inf` for none
        epsilon: forward-difference step of the default Jacobian
        coeff: 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)
    """

    def __init__(
        self,
        name: str,
        var: Var,
        lower: npt.ArrayLike,
        upper: npt.ArrayLike,
        epsilon: float = DEFAULT_EPSILON,
        coeff: float | Sequence[float] = 1.0,
    ) -> None:
        lows = np.asarray(lower, dtype=float).ravel()
        highs = np.asarray(upper, dtype=float).ravel()
        if lows.shape != highs.shape:
            raise BoundsShapeError(f"lower has {lows.size} rows, upper {highs.size}")
        inverted = np.flatnonzero(lows > highs)
        if inverted.size:
            row = int(inverted[0])
            raise InvertedBoundsError(
                f"row {row}: lower bound {lows[row]} is above upper bound {highs[row]}"
            )
        # one-sided rows: a range [lo, hi] becomes [lo, inf] and [-inf, hi] on the same value
        row_map: list[int] = []
        bounds: list[Bounds] = []
        for i, (lo, hi) in enumerate(zip(lows, highs)):
            if np.isfinite(lo) and np.isfinite(hi) and lo < hi:
                row_map += [i, i]
                bounds += [Bounds(lo, np.inf), Bounds(-np.inf, hi)]
            else:
                row_map.append(i)
                bounds.append(Bounds(lo, hi))
        super().__init__(name, len(row_map))
        self.var = var
        self.epsilon = epsilon
        self._row_map: np.ndarray = np.asarray(row_map, dtype=np.intp)
        self._coeff: np.ndarray = np.broadcast_to(np.asarray(coeff, dtype=float), lows.shape)[
            self._row_map
        ]
        self._bounds = bounds
        self._rows = len(row_map)

    # -- what a subclass writes ------------------------------------------------------------

    def f(self, q: np.ndarray) -> npt.ArrayLike:
        """The rule's values at joint values `q` (one waypoint)."""
        raise NotImplementedError

    def jacobian(self, q: np.ndarray) -> np.ndarray:
        """d f / d q, shape (rows, n_dof). Default: forward difference over `q`."""
        return calcForwardNumJac(
            lambda x: np.asarray(self.f(x), dtype=float), q, self.epsilon
        ).toarray()

    # -- the ConstraintSet interface -------------------------------------------------------

    @override
    def getValues(self) -> np.ndarray:
        return np.asarray(self.f(np.asarray(self.var.value())), dtype=float).ravel()[self._row_map]

    @override
    def getBounds(self) -> list[Bounds]:
        return self._bounds

    @override
    def getJacobian(self) -> scipy.sparse.csr_matrix:
        n_vars = self.getVariables().getRows()
        block = np.asarray(self.jacobian(np.asarray(self.var.value())), dtype=float)
        block = block.reshape(-1, block.shape[-1])[self._row_map]
        # a dense block in this waypoint's columns, built straight in CSR form
        n_dof = block.shape[1]
        indices = np.tile(np.arange(self.var.getIndex(), self.var.getIndex() + n_dof), self._rows)
        indptr = np.arange(0, self._rows * n_dof + 1, n_dof)
        return scipy.sparse.csr_matrix((block.ravel(), indices, indptr), shape=(self._rows, n_vars))

    @override
    def update(self) -> int:
        return self._rows

    @override
    def getCoefficients(self) -> np.ndarray:
        return np.array(self._coeff)

var instance-attribute

Python
var = var

epsilon instance-attribute

Python
epsilon = epsilon

__init__

Python
__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
Python
def __init__(
    self,
    name: str,
    var: Var,
    lower: npt.ArrayLike,
    upper: npt.ArrayLike,
    epsilon: float = DEFAULT_EPSILON,
    coeff: float | Sequence[float] = 1.0,
) -> None:
    lows = np.asarray(lower, dtype=float).ravel()
    highs = np.asarray(upper, dtype=float).ravel()
    if lows.shape != highs.shape:
        raise BoundsShapeError(f"lower has {lows.size} rows, upper {highs.size}")
    inverted = np.flatnonzero(lows > highs)
    if inverted.size:
        row = int(inverted[0])
        raise InvertedBoundsError(
            f"row {row}: lower bound {lows[row]} is above upper bound {highs[row]}"
        )
    # one-sided rows: a range [lo, hi] becomes [lo, inf] and [-inf, hi] on the same value
    row_map: list[int] = []
    bounds: list[Bounds] = []
    for i, (lo, hi) in enumerate(zip(lows, highs)):
        if np.isfinite(lo) and np.isfinite(hi) and lo < hi:
            row_map += [i, i]
            bounds += [Bounds(lo, np.inf), Bounds(-np.inf, hi)]
        else:
            row_map.append(i)
            bounds.append(Bounds(lo, hi))
    super().__init__(name, len(row_map))
    self.var = var
    self.epsilon = epsilon
    self._row_map: np.ndarray = np.asarray(row_map, dtype=np.intp)
    self._coeff: np.ndarray = np.broadcast_to(np.asarray(coeff, dtype=float), lows.shape)[
        self._row_map
    ]
    self._bounds = bounds
    self._rows = len(row_map)

f

Python
f(q: ndarray) -> npt.ArrayLike

The rule's values at joint values q (one waypoint).

Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Python
def f(self, q: np.ndarray) -> npt.ArrayLike:
    """The rule's values at joint values `q` (one waypoint)."""
    raise NotImplementedError

jacobian

Python
jacobian(q: ndarray) -> np.ndarray

d f / d q, shape (rows, n_dof). Default: forward difference over q.

Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Python
def jacobian(self, q: np.ndarray) -> np.ndarray:
    """d f / d q, shape (rows, n_dof). Default: forward difference over `q`."""
    return calcForwardNumJac(
        lambda x: np.asarray(self.f(x), dtype=float), q, self.epsilon
    ).toarray()

getValues

Python
getValues() -> np.ndarray
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Python
@override
def getValues(self) -> np.ndarray:
    return np.asarray(self.f(np.asarray(self.var.value())), dtype=float).ravel()[self._row_map]

getBounds

Python
getBounds() -> list[Bounds]
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Python
@override
def getBounds(self) -> list[Bounds]:
    return self._bounds

getJacobian

Python
getJacobian() -> scipy.sparse.csr_matrix
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Python
@override
def getJacobian(self) -> scipy.sparse.csr_matrix:
    n_vars = self.getVariables().getRows()
    block = np.asarray(self.jacobian(np.asarray(self.var.value())), dtype=float)
    block = block.reshape(-1, block.shape[-1])[self._row_map]
    # a dense block in this waypoint's columns, built straight in CSR form
    n_dof = block.shape[1]
    indices = np.tile(np.arange(self.var.getIndex(), self.var.getIndex() + n_dof), self._rows)
    indptr = np.arange(0, self._rows * n_dof + 1, n_dof)
    return scipy.sparse.csr_matrix((block.ravel(), indices, indptr), shape=(self._rows, n_vars))

update

Python
update() -> int
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Python
@override
def update(self) -> int:
    return self._rows

getCoefficients

Python
getCoefficients() -> np.ndarray
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Python
@override
def getCoefficients(self) -> np.ndarray:
    return np.array(self._coeff)

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
Python
class PointsDown(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.
    """

    def __init__(
        self,
        var: Var,
        manip: JointGroup,
        tcp: str,
        max_tilt: float,
        down: npt.ArrayLike = (0.0, 0.0, -1.0),
        axis: int = 2,
        analytic: bool = True,
        name: str = "points_down",
        **options: Unpack[ConstraintOptions],
    ) -> None:
        super().__init__(name, var, [np.cos(max_tilt)], [np.inf], **options)
        self.manip, self.tcp, self.axis, self.analytic = manip, tcp, axis, analytic
        self.down = np.asarray(down, dtype=float) / np.linalg.norm(down)

    def f(self, q: np.ndarray) -> np.ndarray | Sequence[float]:
        return [self.manip.calcFwdKin(q)[self.tcp].rotation[:, self.axis] @ self.down]

    def jacobian(self, q: np.ndarray) -> np.ndarray:
        if not self.analytic:
            return super().jacobian(q)
        a = self.manip.calcFwdKin(q)[self.tcp].rotation[:, self.axis]
        omega = self.manip.calcJacobian(q, self.tcp)[3:6]
        return (self.down @ _skew_cross_rows(a, omega)).reshape(1, -1)

down instance-attribute

Python
down = np.asarray(down, dtype=float) / np.linalg.norm(down)

__init__

Python
__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
Python
def __init__(
    self,
    var: Var,
    manip: JointGroup,
    tcp: str,
    max_tilt: float,
    down: npt.ArrayLike = (0.0, 0.0, -1.0),
    axis: int = 2,
    analytic: bool = True,
    name: str = "points_down",
    **options: Unpack[ConstraintOptions],
) -> None:
    super().__init__(name, var, [np.cos(max_tilt)], [np.inf], **options)
    self.manip, self.tcp, self.axis, self.analytic = manip, tcp, axis, analytic
    self.down = np.asarray(down, dtype=float) / np.linalg.norm(down)

f

Python
f(q: ndarray) -> np.ndarray | Sequence[float]
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Python
def f(self, q: np.ndarray) -> np.ndarray | Sequence[float]:
    return [self.manip.calcFwdKin(q)[self.tcp].rotation[:, self.axis] @ self.down]

jacobian

Python
jacobian(q: ndarray) -> np.ndarray
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Python
def jacobian(self, q: np.ndarray) -> np.ndarray:
    if not self.analytic:
        return super().jacobian(q)
    a = self.manip.calcFwdKin(q)[self.tcp].rotation[:, self.axis]
    omega = self.manip.calcJacobian(q, self.tcp)[3:6]
    return (self.down @ _skew_cross_rows(a, omega)).reshape(1, -1)

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
Python
class HoldAxis(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).
    """

    def __init__(
        self,
        var: Var,
        manip: JointGroup,
        tcp: str,
        direction: npt.ArrayLike,
        tol: float,
        axis: int = 2,
        analytic: bool = True,
        name: str = "hold_axis",
        **options: Unpack[ConstraintOptions],
    ) -> None:
        side = np.sin(tol)
        super().__init__(name, var, [-side, -side, 0.0], [side, side, np.inf], **options)
        self.manip, self.tcp, self.axis, self.analytic = manip, tcp, axis, analytic
        direction = np.asarray(direction, dtype=float) / np.linalg.norm(direction)
        self._basis = np.vstack([_perpendicular_pair(direction), direction])

    def f(self, q: np.ndarray) -> np.ndarray | Sequence[float]:
        return self._basis @ self.manip.calcFwdKin(q)[self.tcp].rotation[:, self.axis]

    def jacobian(self, q: np.ndarray) -> np.ndarray:
        if not self.analytic:
            return super().jacobian(q)
        a = self.manip.calcFwdKin(q)[self.tcp].rotation[:, self.axis]
        omega = self.manip.calcJacobian(q, self.tcp)[3:6]
        return self._basis @ _skew_cross_rows(a, omega)

__init__

Python
__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
Python
def __init__(
    self,
    var: Var,
    manip: JointGroup,
    tcp: str,
    direction: npt.ArrayLike,
    tol: float,
    axis: int = 2,
    analytic: bool = True,
    name: str = "hold_axis",
    **options: Unpack[ConstraintOptions],
) -> None:
    side = np.sin(tol)
    super().__init__(name, var, [-side, -side, 0.0], [side, side, np.inf], **options)
    self.manip, self.tcp, self.axis, self.analytic = manip, tcp, axis, analytic
    direction = np.asarray(direction, dtype=float) / np.linalg.norm(direction)
    self._basis = np.vstack([_perpendicular_pair(direction), direction])

f

Python
f(q: ndarray) -> np.ndarray | Sequence[float]
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Python
def f(self, q: np.ndarray) -> np.ndarray | Sequence[float]:
    return self._basis @ self.manip.calcFwdKin(q)[self.tcp].rotation[:, self.axis]

jacobian

Python
jacobian(q: ndarray) -> np.ndarray
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Python
def jacobian(self, q: np.ndarray) -> np.ndarray:
    if not self.analytic:
        return super().jacobian(q)
    a = self.manip.calcFwdKin(q)[self.tcp].rotation[:, self.axis]
    omega = self.manip.calcJacobian(q, self.tcp)[3:6]
    return self._basis @ _skew_cross_rows(a, omega)

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
Python
class StayDexterous(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`).
    """

    def __init__(
        self,
        var: Var,
        manip: JointGroup,
        tcp: str,
        w_min: float,
        name: str = "stay_dexterous",
        **options: Unpack[ConstraintOptions],
    ) -> None:
        super().__init__(name, var, [w_min], [np.inf], **options)
        self.manip, self.tcp = manip, tcp

    def f(self, q: np.ndarray) -> np.ndarray | Sequence[float]:
        return [calcManipulability(self.manip.calcJacobian(q, self.tcp)).m.volume]

__init__

Python
__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
Python
def __init__(
    self,
    var: Var,
    manip: JointGroup,
    tcp: str,
    w_min: float,
    name: str = "stay_dexterous",
    **options: Unpack[ConstraintOptions],
) -> None:
    super().__init__(name, var, [w_min], [np.inf], **options)
    self.manip, self.tcp = manip, tcp

f

Python
f(q: ndarray) -> np.ndarray | Sequence[float]
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Python
def f(self, q: np.ndarray) -> np.ndarray | Sequence[float]:
    return [calcManipulability(self.manip.calcJacobian(q, self.tcp)).m.volume]

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
Python
class ElbowUp(PyConstraint):
    """`upper_link` stays at least `delta` above `lower_link` (one IK branch)."""

    def __init__(
        self,
        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:
        super().__init__(name, var, [delta], [np.inf], **options)
        self.manip, self.upper_link, self.lower_link, self.axis = (
            manip,
            upper_link,
            lower_link,
            axis,
        )

    def f(self, q: np.ndarray) -> np.ndarray | Sequence[float]:
        tf = self.manip.calcFwdKin(q)
        return [
            tf[self.upper_link].translation[self.axis] - tf[self.lower_link].translation[self.axis]
        ]

__init__

Python
__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
Python
def __init__(
    self,
    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:
    super().__init__(name, var, [delta], [np.inf], **options)
    self.manip, self.upper_link, self.lower_link, self.axis = (
        manip,
        upper_link,
        lower_link,
        axis,
    )

f

Python
f(q: ndarray) -> np.ndarray | Sequence[float]
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Python
def f(self, q: np.ndarray) -> np.ndarray | Sequence[float]:
    tf = self.manip.calcFwdKin(q)
    return [
        tf[self.upper_link].translation[self.axis] - tf[self.lower_link].translation[self.axis]
    ]

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
Python
class WatchTheTool(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`.
    """

    def __init__(
        self,
        var: Var,
        manip: JointGroup,
        camera: str,
        tcp: str,
        alpha: float,
        optical_axis: int = 2,
        name: str = "watch_the_tool",
        **options: Unpack[ConstraintOptions],
    ) -> None:
        super().__init__(name, var, [np.cos(alpha)], [np.inf], **options)
        self.manip, self.camera, self.tcp = manip, camera, tcp
        self.optical_axis = optical_axis

    def f(self, q: np.ndarray) -> np.ndarray | Sequence[float]:
        tf = self.manip.calcFwdKin(q)
        cam = tf[self.camera]
        v = tf[self.tcp].translation - cam.translation
        return [float(cam.rotation[:, self.optical_axis] @ v / np.linalg.norm(v))]

optical_axis instance-attribute

Python
optical_axis = optical_axis

__init__

Python
__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
Python
def __init__(
    self,
    var: Var,
    manip: JointGroup,
    camera: str,
    tcp: str,
    alpha: float,
    optical_axis: int = 2,
    name: str = "watch_the_tool",
    **options: Unpack[ConstraintOptions],
) -> None:
    super().__init__(name, var, [np.cos(alpha)], [np.inf], **options)
    self.manip, self.camera, self.tcp = manip, camera, tcp
    self.optical_axis = optical_axis

f

Python
f(q: ndarray) -> np.ndarray | Sequence[float]
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Python
def f(self, q: np.ndarray) -> np.ndarray | Sequence[float]:
    tf = self.manip.calcFwdKin(q)
    cam = tf[self.camera]
    v = tf[self.tcp].translation - cam.translation
    return [float(cam.rotation[:, self.optical_axis] @ v / np.linalg.norm(v))]

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
Python
class Clearance(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.
    """

    def __init__(
        self,
        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:
        from tesseract_robotics.collision import (
            ContactRequest,
            ContactResultMap,
            ContactResultVector,
            ContactTestType,
        )
        from tesseract_robotics.common import CollisionMarginData

        super().__init__(name, var, [d_min], [d_max], **options)
        self.env, self.joint_ids = env, list(joint_ids)
        self.margin = margin if margin is not None else CLEARANCE_MARGIN_PER_D_MAX * d_max
        if d_max >= self.margin:
            raise ClearanceMarginError(f"d_max {d_max} must be below the margin {self.margin}")
        self.mgr = env.getDiscreteContactManager().clone()
        self.mgr.setActiveCollisionObjects(list(links))
        self.mgr.setCollisionMarginData(CollisionMarginData(self.margin))
        self._map, self._vec = ContactResultMap, ContactResultVector
        self._request = ContactRequest(ContactTestType.CLOSEST)

    def f(self, q: np.ndarray) -> np.ndarray | Sequence[float]:
        self.mgr.setCollisionObjectsTransform(self.env.getState(self.joint_ids, q).link_transforms)
        hits, flat = self._map(), self._vec()
        self.mgr.contactTest(hits, self._request)
        hits.flattenMoveResults(flat)
        return [min((c.distance for c in flat), default=self.margin)]

margin instance-attribute

Python
margin = margin if margin is not None else CLEARANCE_MARGIN_PER_D_MAX * d_max

mgr instance-attribute

Python
mgr = env.getDiscreteContactManager().clone()

__init__

Python
__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
Python
def __init__(
    self,
    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:
    from tesseract_robotics.collision import (
        ContactRequest,
        ContactResultMap,
        ContactResultVector,
        ContactTestType,
    )
    from tesseract_robotics.common import CollisionMarginData

    super().__init__(name, var, [d_min], [d_max], **options)
    self.env, self.joint_ids = env, list(joint_ids)
    self.margin = margin if margin is not None else CLEARANCE_MARGIN_PER_D_MAX * d_max
    if d_max >= self.margin:
        raise ClearanceMarginError(f"d_max {d_max} must be below the margin {self.margin}")
    self.mgr = env.getDiscreteContactManager().clone()
    self.mgr.setActiveCollisionObjects(list(links))
    self.mgr.setCollisionMarginData(CollisionMarginData(self.margin))
    self._map, self._vec = ContactResultMap, ContactResultVector
    self._request = ContactRequest(ContactTestType.CLOSEST)

f

Python
f(q: ndarray) -> np.ndarray | Sequence[float]
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Python
def f(self, q: np.ndarray) -> np.ndarray | Sequence[float]:
    self.mgr.setCollisionObjectsTransform(self.env.getState(self.joint_ids, q).link_transforms)
    hits, flat = self._map(), self._vec()
    self.mgr.contactTest(hits, self._request)
    hits.flattenMoveResults(flat)
    return [min((c.distance for c in flat), default=self.margin)]

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
Python
class WorldHold(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.
    """

    def __init__(
        self,
        var: Var,
        manip: JointGroup,
        tcp: str,
        target: npt.ArrayLike,
        base_pose: Callable[[], Isometry3d] | Isometry3d,
        tol: npt.ArrayLike = WORLD_HOLD_TOL_M,
        analytic: bool = True,
        name: str = "world_hold",
        **options: Unpack[ConstraintOptions],
    ) -> None:
        band = np.broadcast_to(np.asarray(tol, dtype=float), (3,))
        super().__init__(name, var, -band, band, **options)
        self.manip, self.tcp, self.analytic = manip, tcp, analytic
        self.target = np.asarray(target, dtype=float)
        self._base: Callable[[], Isometry3d] = (
            (lambda: base_pose) if isinstance(base_pose, Isometry3d) else base_pose
        )

    def f(self, q: np.ndarray) -> np.ndarray | Sequence[float]:
        base = self._base()
        p = self.manip.calcFwdKin(q)[self.tcp].translation
        return base.rotation @ p + base.translation - self.target

    def jacobian(self, q: np.ndarray) -> np.ndarray:
        if not self.analytic:
            return super().jacobian(q)
        return self._base().rotation @ self.manip.calcJacobian(q, self.tcp)[0:3]

target instance-attribute

Python
target = np.asarray(target, dtype=float)

__init__

Python
__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
Python
def __init__(
    self,
    var: Var,
    manip: JointGroup,
    tcp: str,
    target: npt.ArrayLike,
    base_pose: Callable[[], Isometry3d] | Isometry3d,
    tol: npt.ArrayLike = WORLD_HOLD_TOL_M,
    analytic: bool = True,
    name: str = "world_hold",
    **options: Unpack[ConstraintOptions],
) -> None:
    band = np.broadcast_to(np.asarray(tol, dtype=float), (3,))
    super().__init__(name, var, -band, band, **options)
    self.manip, self.tcp, self.analytic = manip, tcp, analytic
    self.target = np.asarray(target, dtype=float)
    self._base: Callable[[], Isometry3d] = (
        (lambda: base_pose) if isinstance(base_pose, Isometry3d) else base_pose
    )

f

Python
f(q: ndarray) -> np.ndarray | Sequence[float]
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Python
def f(self, q: np.ndarray) -> np.ndarray | Sequence[float]:
    base = self._base()
    p = self.manip.calcFwdKin(q)[self.tcp].translation
    return base.rotation @ p + base.translation - self.target

jacobian

Python
jacobian(q: ndarray) -> np.ndarray
Source code in src/tesseract_robotics/trajopt_ifopt/python_constraints.py
Python
def jacobian(self, q: np.ndarray) -> np.ndarray:
    if not self.analytic:
        return super().jacobian(q)
    return self._base().rotation @ self.manip.calcJacobian(q, self.tcp)[0:3]

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

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

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

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

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.common.Isometry3d

Get the target pose for the constraint

getCurrentPose

Python
getCurrentPose() -> tesseract_robotics.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

getValues

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

Get current constraint values

JointAccelConstraint

Bases: ConstraintSet

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__(other: object) -> bool

__ne__

Python
__ne__(other: object) -> bool

setDefaultCollisionCoeff

Python
setDefaultCollisionCoeff(arg: float) -> None

getDefaultCollisionCoeff

Python
getDefaultCollisionCoeff() -> float

getCollisionCoeff

Python
getCollisionCoeff(pair: LinkIdPair | tuple[str, str]) -> float

getCollisionCoeffPairData

Python
getCollisionCoeffPairData() -> dict[tesseract_robotics.common.LinkIdPair, float]

Every pair with a coefficient: {LinkIdPair: coeff}

hasZeroCoeff

Python
hasZeroCoeff(pair: LinkIdPair | tuple[str, str]) -> bool

Whether the pair's coefficient is 0 (collision checking switched off for it)

getPairsWithZeroCoeff

Python
getPairsWithZeroCoeff() -> set[tesseract_robotics.common.LinkIdPair]

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__(other: object) -> bool

__ne__

Python
__ne__(other: object) -> 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: LinkIdPair

shape_key property writable

Python
shape_key: tuple[tuple[int, int], 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

getJointGroup

Python
getJointGroup() -> tesseract_robotics.kinematics.JointGroup

getCollisionMarginBuffer

Python
getCollisionMarginBuffer() -> float

getCollisionMarginData

Python
getCollisionMarginData() -> tesseract_robotics.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

getJointGroup

Python
getJointGroup() -> tesseract_robotics.kinematics.JointGroup

getCollisionMarginBuffer

Python
getCollisionMarginBuffer() -> float

getCollisionMarginData

Python
getCollisionMarginData() -> tesseract_robotics.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

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

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

JointJerkConstraint

Bases: ConstraintSet

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: LinkId

TCP frame

target_frame property writable

Python
target_frame: LinkId

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.common.Isometry3d, tesseract_robotics.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

Get the constraint Jacobian (sparse matrix), one row per index

calcJacobianBlock

Python
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

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 staticmethod

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

SIN_ANGLE_FLOOR class-attribute instance-attribute

Python
SIN_ANGLE_FLOOR: float = 1e-12

__init__

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

Python
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

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

numJoints

Python
numJoints() -> int

getSourceAxis

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

The unit source axis, in source link coordinates

mostReachableDirection staticmethod

Python
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

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

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

Python
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

Python
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

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

Set the cone axis, in target link coordinates

getTargetAxis

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

The cone axis, normalized

getHalfAngle

Python
getHalfAngle() -> float

The cone half angle, in radians

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__

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

Python
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

Python
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

Python
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

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

The axis to align with, normalized

InverseKinematicsInfo

manip property writable

Python
manip: KinematicGroup

The kinematic group (with IK solver)

working_frame property writable

Python
working_frame: LinkId

Working frame (not currently used)

tcp_frame property writable

Python
tcp_frame: LinkId

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

calcJacobianBlock

Python
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

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

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)]], 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

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.common.JointTrajectory

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

See also