Skip to content

tesseract_robotics.trajopt_ifopt

Robotics-specific constraints and variables for trajectory optimization.

Overview

Python
from tesseract_robotics.trajopt_ifopt import (
    # Variables
    JointPosition,
    # Joint constraints
    JointPosConstraint, JointVelConstraint, JointAccelConstraint, JointJerkConstraint,
    # Cartesian constraints
    CartPosInfo, CartPosConstraint, CartLineInfo, CartLineConstraint,
    # Collision constraints
    TrajOptCollisionConfig, DiscreteCollisionConstraint, ContinuousCollisionConstraint,
    # IK constraint
    InverseKinematicsInfo, InverseKinematicsConstraint,
    # Utilities
    interpolate, toBounds,
)

Variables

JointPosition

Optimization variable representing joint configuration at a timestep.

Python
from tesseract_robotics.trajopt_ifopt import JointPosition
import numpy as np

joint_names = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"]
initial_values = np.array([0.0, -0.5, 0.5, 0.0, 0.5, 0.0])

# Create variable
var = JointPosition(initial_values, joint_names, "position_0")

# Access values
print(f"Values: {var.GetValues()}")
print(f"Names: {var.GetJointNames()}")

# Set bounds
from tesseract_robotics.ifopt import Bounds
bounds = [Bounds(-3.14, 3.14) for _ in range(6)]
var.SetBounds(bounds)
Method Returns Description
GetValues() np.ndarray Current joint values
SetVariables(x) None Set joint values
GetJointNames() list[str] Joint names
GetBounds() list[Bounds] Joint limits
SetBounds(bounds) None Set joint limits

Joint Constraints

JointPosConstraint

Constrain joint positions to target values.

Python
from tesseract_robotics.trajopt_ifopt import JointPosConstraint
import numpy as np

targets = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])  # home position
coeffs = np.ones(6)  # weight per joint

constraint = JointPosConstraint(targets, position_var, coeffs, "joint_pos")

JointVelConstraint

Constrain joint velocities between consecutive timesteps.

Python
from tesseract_robotics.trajopt_ifopt import JointVelConstraint

targets = np.zeros(6)  # zero velocity
coeffs = np.ones(6)

# Requires two consecutive position variables
constraint = JointVelConstraint(targets, [pos_var_0, pos_var_1], coeffs, "joint_vel")

JointAccelConstraint

Constrain joint accelerations (requires 3 consecutive positions).

Python
from tesseract_robotics.trajopt_ifopt import JointAccelConstraint

targets = np.zeros(6)
coeffs = np.ones(6)

constraint = JointAccelConstraint(
    targets, [pos_var_0, pos_var_1, pos_var_2], coeffs, "joint_accel"
)

JointJerkConstraint

Constrain joint jerk (requires 4 consecutive positions).

Python
from tesseract_robotics.trajopt_ifopt import JointJerkConstraint

targets = np.zeros(6)
coeffs = np.ones(6)

constraint = JointJerkConstraint(
    targets, [pos_var_0, pos_var_1, pos_var_2, pos_var_3], coeffs, "joint_jerk"
)

Cartesian Constraints

CartPosInfo / CartPosConstraint

Constrain TCP to a Cartesian pose.

Python
from tesseract_robotics.trajopt_ifopt import CartPosInfo, CartPosInfoType, CartPosConstraint
from tesseract_robotics.tesseract_common import Isometry3d

# Create info struct
info = CartPosInfo()
info.manip = kin_group  # KinematicGroup from env.getKinematicGroup()
info.source_frame = "tool0"
info.target_frame = "base_link"
info.target_frame_offset = target_pose  # Isometry3d

# Constraint indices (which DoFs to constrain)
# CartPosInfoType: FULL (all 6), POSITION_ONLY (xyz), ORIENTATION_ONLY (rpy)
info.indices = [0, 1, 2, 3, 4, 5]  # all 6 DoF

# Create constraint
coeffs = np.array([10.0, 10.0, 10.0, 5.0, 5.0, 5.0])  # xyz, rpy weights
constraint = CartPosConstraint(info, position_var, coeffs, "cart_pos")

# Get current error
values = constraint.GetValues()

CartLineInfo / CartLineConstraint

Constrain TCP to lie on a line between two points.

Python
from tesseract_robotics.trajopt_ifopt import CartLineInfo, CartLineConstraint

info = CartLineInfo()
info.manip = kin_group
info.source_frame = "tool0"
info.target_frame = "base_link"
info.target_frame_offset1 = line_start  # Isometry3d
info.target_frame_offset2 = line_end    # Isometry3d

constraint = CartLineConstraint(info, position_var, coeffs, "cart_line")

Collision Constraints

TrajOptCollisionConfig

Configuration for collision checking.

Python
from tesseract_robotics.trajopt_ifopt import TrajOptCollisionConfig

config = TrajOptCollisionConfig()
config.contact_manager_config.margin_data_override_type = 0
config.contact_manager_config.margin_data.setDefaultCollisionMargin(0.025)
config.collision_margin_buffer = 0.01
config.collision_coeff_data.default_collision_coeff = 20.0
Attribute Type Description
contact_manager_config object Contact manager settings
collision_margin_buffer float Safety margin buffer
collision_coeff_data object Collision cost coefficients

DiscreteCollisionConstraint

Collision avoidance at a single configuration.

Python
from tesseract_robotics.trajopt_ifopt import (
    DiscreteCollisionConstraint, SingleTimestepCollisionEvaluator, CollisionCache
)

# Create evaluator
cache = CollisionCache(100)
evaluator = SingleTimestepCollisionEvaluator(
    cache, kin_group, env, config, True  # True = dynamic environment
)

# Create constraint
constraint = DiscreteCollisionConstraint(
    evaluator,
    position_var,
    max_num_cnt=3,       # max collision pairs
    fixed_sparsity=False,
    name="collision"
)

ContinuousCollisionConstraint

Collision avoidance between consecutive configurations (swept volume).

Python
from tesseract_robotics.trajopt_ifopt import (
    ContinuousCollisionConstraint, LVSContinuousCollisionEvaluator
)

# LVS = Longest Valid Segment (interpolates and checks)
evaluator = LVSContinuousCollisionEvaluator(
    cache, kin_group, env, config, True
)

# Constraint between two consecutive position variables
constraint = ContinuousCollisionConstraint(
    evaluator,
    position_var_0,
    position_var_1,
    max_num_cnt=3,
    fixed_sparsity=False,
    name="continuous_collision"
)

Collision Evaluators

Class Description
SingleTimestepCollisionEvaluator Discrete check at single config
LVSDiscreteCollisionEvaluator Discrete checks at interpolated points
LVSContinuousCollisionEvaluator Swept volume collision check

IK Constraint

InverseKinematicsInfo / InverseKinematicsConstraint

Constrain variables to match IK solution.

Python
from tesseract_robotics.trajopt_ifopt import InverseKinematicsInfo, InverseKinematicsConstraint

info = InverseKinematicsInfo()
info.manip = kin_group
info.working_frame = "base_link"
info.tcp_frame = "tool0"
info.tcp_offset = Isometry3d.Identity()

constraint = InverseKinematicsConstraint(
    target_pose,      # Isometry3d
    info,
    constraint_var,   # variable to constrain
    seed_var,         # seed for IK
    "ik_constraint"
)

Utilities

interpolate

Linear interpolation between joint configurations.

Python
from tesseract_robotics.trajopt_ifopt import interpolate
import numpy as np

start = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
end = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
steps = 10

trajectory = interpolate(start, end, steps)  # (10, 6) array

toBounds

Convert numpy arrays to ifopt Bounds.

Python
from tesseract_robotics.trajopt_ifopt import toBounds
import numpy as np

lower = np.array([-3.14, -3.14, -3.14])
upper = np.array([3.14, 3.14, 3.14])

bounds = toBounds(lower, upper)  # list[Bounds]

Auto-generated API Reference

JointPosition

Bases: VariableSet

__init__

Python
__init__(init_value: Annotated[NDArray[float64], dict(shape=(None,), writable=False)], joint_names: Sequence[str], name: str = 'Joint_Position') -> None

Create joint position variable set with initial values and joint names

SetVariables

Python
SetVariables(x: Annotated[NDArray[float64], dict(shape=(None,), order=C)]) -> None

Set the joint position values

GetValues

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

Get the current joint position values

GetJointNames

Python
GetJointNames() -> list[str]

Get the joint names associated with this variable set

GetRows

Python
GetRows() -> int

Get number of variables

GetName

Python
GetName() -> str

Get the name of this variable set

SetBounds

Python
SetBounds(bounds: Annotated[NDArray[float64], dict(shape=(None, 2), writable=False)]) -> None

Set joint bounds from Nx2 matrix [lower, upper]

GetBounds

Python
GetBounds() -> list[tesseract_robotics.ifopt._ifopt.Bounds]

Get the variable bounds

CartPosInfoType

Bases: Enum

TARGET_ACTIVE class-attribute instance-attribute

Python
TARGET_ACTIVE = 0

SOURCE_ACTIVE class-attribute instance-attribute

Python
SOURCE_ACTIVE = 1

BOTH_ACTIVE class-attribute instance-attribute

Python
BOTH_ACTIVE = 2

CartPosInfo

manip property writable

Python
manip: 'tesseract_kinematics::JointGroup'

The joint group

source_frame property writable

Python
source_frame: str

Link which should reach desired pos

target_frame property writable

Python
target_frame: str

Target frame to be reached

source_frame_offset property writable

Python
source_frame_offset: 'Eigen::Transform<double, 3, 1, 0>'

Static transform applied to source_frame

target_frame_offset property writable

Python
target_frame_offset: 'Eigen::Transform<double, 3, 1, 0>'

Static transform applied to target_frame

type property writable

Python
type: CartPosInfoType

Indicates which link is active

indices property writable

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

Indices to return: default {0,1,2,3,4,5}. Position: {0,1,2}, Rotation: {3,4,5}

__init__

Python
__init__() -> None

CartPosConstraint

Bases: ConstraintSet

use_numeric_differentiation property writable

Python
use_numeric_differentiation: bool

If true, use numeric differentiation (default: true)

__init__

Python
__init__(info: CartPosInfo, position_var: JointPosition, name: str = 'CartPos') -> None

Create Cartesian position constraint

CalcValues

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

Calculate error values for given joint values

GetValues

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

Get current constraint values

SetTargetPose

Python
SetTargetPose(target_frame_offset: 'Eigen::Transform<double, 3, 1, 0>') -> None

Set the target pose - critical for online planning!

GetTargetPose

Python
GetTargetPose() -> 'Eigen::Transform<double, 3, 1, 0>'

Get the target pose for the constraint

GetCurrentPose

Python
GetCurrentPose() -> 'Eigen::Transform<double, 3, 1, 0>'

Get current TCP pose in world frame

JointPosConstraint

Bases: ConstraintSet

__init__

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

Create joint position constraint with target values

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)], position_vars: Sequence[JointPosition], coeffs: Annotated[NDArray[float64], dict(shape=(None,), order=C)], 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)], position_vars: Sequence[JointPosition], coeffs: Annotated[NDArray[float64], dict(shape=(None,), order=C)], 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

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

setPairCollisionCoeff

Python
setPairCollisionCoeff(obj1: str, obj2: str, collision_coeff: float) -> None

getPairCollisionCoeff

Python
getPairCollisionCoeff(obj1: str, obj2: str) -> float

TrajOptCollisionConfig

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

DiscreteCollisionEvaluator

GetCollisionMarginBuffer

Python
GetCollisionMarginBuffer() -> float

GetCollisionMarginData

Python
GetCollisionMarginData() -> 'tesseract_common::CollisionMarginData'

GetCollisionCoeffData

Python
GetCollisionCoeffData() -> CollisionCoeffData

SingleTimestepCollisionEvaluator

Bases: DiscreteCollisionEvaluator

__init__

Python
__init__(collision_cache: CollisionCache, manip: 'tesseract_kinematics::JointGroup', env: 'tesseract_environment::Environment', collision_config: TrajOptCollisionConfig, dynamic_environment: bool = False) -> None

CollisionCache

__init__

Python
__init__(size: int = 10) -> None

DiscreteCollisionConstraint

Bases: ConstraintSet

__init__

Python
__init__(collision_evaluator: DiscreteCollisionEvaluator, position_var: JointPosition, max_num_cnt: int = 1, fixed_sparsity: bool = False, name: str = 'DiscreteCollisionV3') -> None

GetValues

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

CalcValues

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

GetCollisionEvaluator

Python
GetCollisionEvaluator() -> DiscreteCollisionEvaluator

ContinuousCollisionEvaluator

GetCollisionMarginBuffer

Python
GetCollisionMarginBuffer() -> float

GetCollisionMarginData

Python
GetCollisionMarginData() -> 'tesseract_common::CollisionMarginData'

GetCollisionCoeffData

Python
GetCollisionCoeffData() -> CollisionCoeffData

LVSDiscreteCollisionEvaluator

Bases: ContinuousCollisionEvaluator

__init__

Python
__init__(collision_cache: CollisionCache, manip: 'tesseract_kinematics::JointGroup', env: 'tesseract_environment::Environment', collision_config: TrajOptCollisionConfig, dynamic_environment: bool = False) -> None

LVSContinuousCollisionEvaluator

Bases: ContinuousCollisionEvaluator

__init__

Python
__init__(collision_cache: CollisionCache, manip: 'tesseract_kinematics::JointGroup', env: 'tesseract_environment::Environment', collision_config: TrajOptCollisionConfig, dynamic_environment: bool = False) -> None

ContinuousCollisionConstraint

Bases: ConstraintSet

__init__

Python
__init__(collision_evaluator: ContinuousCollisionEvaluator, position_var0: JointPosition, position_var1: JointPosition, 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)]

GetCollisionEvaluator

Python
GetCollisionEvaluator() -> ContinuousCollisionEvaluator

JointJerkConstraint

Bases: ConstraintSet

__init__

Python
__init__(targets: Annotated[NDArray[float64], dict(shape=(None,), order=C)], position_vars: Sequence[JointPosition], coeffs: Annotated[NDArray[float64], dict(shape=(None,), order=C)], 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: 'tesseract_kinematics::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: 'Eigen::Transform<double, 3, 1, 0>'

TCP offset

target_frame_offset1 property writable

Python
target_frame_offset1: 'Eigen::Transform<double, 3, 1, 0>'

Line start pose

target_frame_offset2 property writable

Python
target_frame_offset2: 'Eigen::Transform<double, 3, 1, 0>'

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}

__init__

Python
__init__() -> None

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: JointPosition, coeffs: Annotated[NDArray[float64], dict(shape=(None,), order=C)], name: str = 'CartLine') -> None

Create Cartesian line constraint

GetValues

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

Get current constraint values

CalcValues

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

Calculate error values for given joint values

GetLinePoint

Python
GetLinePoint(source_tf: 'Eigen::Transform<double, 3, 1, 0>', target_tf1: 'Eigen::Transform<double, 3, 1, 0>', target_tf2: 'Eigen::Transform<double, 3, 1, 0>') -> 'Eigen::Transform<double, 3, 1, 0>'

Find nearest point on line (uses SLERP for orientation)

DiscreteCollisionNumericalConstraint

Bases: ConstraintSet

__init__

Python
__init__(collision_evaluator: DiscreteCollisionEvaluator, position_var: JointPosition, 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)]

CalcValues

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

GetCollisionEvaluator

Python
GetCollisionEvaluator() -> DiscreteCollisionEvaluator

InverseKinematicsInfo

manip property writable

Python
manip: 'tesseract_kinematics::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: 'Eigen::Transform<double, 3, 1, 0>'

TCP offset (not currently used)

__init__

Python
__init__() -> None

InverseKinematicsConstraint

Bases: ConstraintSet

__init__

Python
__init__(target_pose: 'Eigen::Transform<double, 3, 1, 0>', kinematic_info: InverseKinematicsInfo, constraint_var: JointPosition, seed_var: JointPosition, 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

interpolate

Python
interpolate(start: Annotated[NDArray[float64], dict(shape=(None,), writable=False)], end: Annotated[NDArray[float64], dict(shape=(None,), writable=False)], steps: int) -> list[Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]]

Interpolate between two joint vectors, returning 'steps' waypoints

toBounds

Python
toBounds(limits: Annotated[NDArray[float64], dict(shape=(None, 2), writable=False)]) -> list[tesseract_robotics.ifopt._ifopt.Bounds]

Convert Nx2 matrix [lower, upper] to vector of ifopt::Bounds