Skip to content

tesseract_robotics.tesseract_kinematics

Forward and inverse kinematics solvers.

Kinematic Groups

KinematicGroup

Full kinematics with FK and IK.

Python
from tesseract_robotics.tesseract_kinematics import KinematicGroup

# Get from environment (defined in SRDF)
manip = env.getKinematicGroup("manipulator")

# Joint information
joint_names = manip.getJointNames()
n_joints = len(joint_names)

# Limits
limits = manip.getLimits()
pos_min = limits.joint_limits[:, 0]
pos_max = limits.joint_limits[:, 1]
vel_limits = limits.velocity_limits

A KinematicGroup can also be built without an Environment, from an inverse kinematics solver the plugin factory created. The group takes a copy (clone()) of inv_kin, so the solver passed in stays usable; the group keeps it, and so its factory, alive.

Python
inv_kin = factory.createInvKin("manipulator", "KDLInvKinChainLMA", scene_graph, scene_state)
manip = KinematicGroup("manipulator", joint_names, inv_kin, scene_graph, scene_state)

Upstream's checks raise RuntimeError: joint_names of the wrong size or with other names than the solver's, or a working frame or tip link that is not a link in scene_state.

JointGroup

Forward kinematics only (no IK solver).

Python
from tesseract_robotics.tesseract_kinematics import JointGroup

group = env.getJointGroup("manipulator")

# Same interface as KinematicGroup for FK

Jacobians

calcJacobian has the four C++ overloads. base_link_name is the frame the Jacobian is expressed in; link_point is a point in the link frame (the Jacobian's reference point moves there), a NumPy array of 3 floats.

Python
p = np.array([0.0, 0.0, 0.1])
J = group.calcJacobian(q, "tool0")                  # in the group base frame
J = group.calcJacobian(q, "tool0", p)               # at a point on tool0
J = group.calcJacobian(q, "link_2", "tool0")        # expressed in link_2
J = group.calcJacobian(q, "link_2", "tool0", p)

An unknown link name raises KeyError. calcJacobianWithPoint(q, link, p) is an alias of calcJacobian(q, link, p).

Forward Kinematics

Compute link transforms from joint values.

Python
import numpy as np

manip = env.getKinematicGroup("manipulator")
joint_values = np.array([0.0, -0.5, 0.5, 0.0, 0.5, 0.0])

# Compute all link transforms
transforms = manip.calcFwdKin(joint_values)

# Get specific link
tcp_pose = transforms["tool0"]
print(f"TCP position: {tcp_pose.translation()}")
print(f"TCP rotation:\n{tcp_pose.rotation()}")

Inverse Kinematics

Compute joint values for a target pose. calcInvKin takes a KinGroupIKInput (not a raw Isometry3d) — it bundles the target pose with the tip link and working frame.

Python
from tesseract_robotics.tesseract_common import Isometry3d
from tesseract_robotics.tesseract_kinematics import KinGroupIKInput
import numpy as np

manip = env.getKinematicGroup("manipulator")

# Build target
mat = np.eye(4)
mat[:3, 3] = [0.5, 0.2, 0.3]
target_pose = Isometry3d(mat)

ik_input = KinGroupIKInput(target_pose, "base_link", "tool0")
seed = np.zeros(6)

solutions = manip.calcInvKin(ik_input, seed)

if solutions:
    print(f"Found {len(solutions)} solutions")
    best = solutions[0]
else:
    print("No IK solution found")

Batch IK

calcInvKin also takes a list of KinGroupIKInput (or a KinGroupIKInputs), one per tip link; calcInvKinMultiple is an alias of the list form.

Python
solutions = manip.calcInvKin([ik_input, another_input], seed)

Redundant Solutions

Get all IK solutions including joint wraparound.

Python
from tesseract_robotics.tesseract_kinematics import getRedundantSolutions

# Initial solution
solution = solutions[0]

# Get redundant solutions by adding ±2π to redundancy-capable joints
# Signature: getRedundantSolutions(sol, limits[:, 0:2], redundancy_capable_joint_indices)
redundant = getRedundantSolutions(
    solution,
    limits.joint_limits,       # N×2 matrix (lower, upper per joint)
    [5],                       # redundant joint indices (e.g., last wrist joint)
)

Kinematics Utilities

From tesseract/kinematics/utils.h and validate.h. Each one checks its inputs first and raises a Python exception where upstream would index or read out of bounds.

Python
import numpy as np
from tesseract_robotics.tesseract_common import Isometry3d
from tesseract_robotics.tesseract_kinematics import (
    calcManipulability, checkKinematics, harmonizeTowardMedian, harmonizeTowardZero,
    isNearSingularity, isValid, numericalJacobian,
)

# Finite-difference Jacobian (forward difference, step 1e-8): an oracle for calcJacobian
J_num = numericalJacobian(Isometry3d.Identity(), group, q, "tool0", np.zeros(3))
J_rel = numericalJacobian(group, q, "link_2", Isometry3d.Identity(), "tool0", Isometry3d.Identity())

J = group.calcJacobian(q, "tool0")
manip = calcManipulability(J)        # 6-row jacobian; manip.m, .m_linear, .f, ...
manip.m.eigen_values, manip.m.measure, manip.m.condition, manip.m.volume
isNearSingularity(J)                 # smallest singular value < 0.01 (threshold=...)

qs = np.array([3 * np.pi / 2, -3 * np.pi / 2, 5.0])
harmonizeTowardZero(qs, [0, 1])      # in place: [-pi/2, pi/2, 5.0]
harmonizeTowardMedian(qs, [0, 1], limits)  # in place, around each joint's limit midpoint

isValid([0.0] * 6)                   # all six finite
checkKinematics(kin_group)           # FK -> IK round trip, tol=1e-3 (m and rad)
function raises
numericalJacobian KeyError for a link the solver or group does not have; ValueError when joint_values is not numJoints() long
calcManipulability TypeError for a jacobian without 6 rows
isNearSingularity ValueError for an empty matrix
harmonizeTowardZero, harmonizeTowardMedian IndexError for an index outside qs (or position_limits), before any write; TypeError for a list, a tuple or an array that would need converting (float32, read-only)
isValid TypeError for anything but 6 values

Upstream semantics

calcManipulability sets measure and condition to the largest double (sys.float_info.max) when an ellipsoid's smallest eigenvalue is about zero. checkKinematics returns True when IK finds no solution at all: only a solution farther than tol from the FK pose makes it False. checkKinematics releases the GIL while it runs.

solvePInv and dampedPInv are not bound (use np.linalg.pinv / lstsq), nor is getRedundantSolutionsHelper (the recursion behind getRedundantSolutions).

UR Robot Parameters

Analytical IK parameters for Universal Robots.

Python
from tesseract_robotics.tesseract_kinematics import (
    URParameters, UR10Parameters, UR10eParameters,
    UR5Parameters, UR5eParameters, UR3Parameters, UR3eParameters
)

# Get default parameters
params = UR10Parameters()
print(f"d1: {params.d1}")
print(f"a2: {params.a2}")
print(f"a3: {params.a3}")
print(f"d4: {params.d4}")
print(f"d5: {params.d5}")
print(f"d6: {params.d6}")

Kinematics Plugin Factory

Load kinematics solvers from plugins. Environment usually does this from the SRDF's kinematics plugin config; a factory can also be built from a config file (pathlib.Path) or YAML content (str).

Python
from pathlib import Path

from tesseract_robotics.tesseract_kinematics import KinematicsPluginFactory

factory = KinematicsPluginFactory(Path("robot_plugins.yaml"), locator)

# Solvers per group: {group_name: PluginInfoContainer}
fwd = factory.getFwdKinPlugins()
info = fwd["manipulator"].plugins["KDLFwdKinChain"]

# Register a second solver for the group and make it the default
factory.addFwdKinPlugin("manipulator", "MyChain", info)
factory.setDefaultFwdKinPlugin("manipulator", "MyChain")

# Create by group and solver name, or from an explicit PluginInfo
fwd_kin = factory.createFwdKin("manipulator", "MyChain", scene_graph, scene_state)
fwd_kin = factory.createFwdKin("my_chain", info, scene_graph, scene_state)

factory.removeFwdKinPlugin("manipulator", "MyChain")
factory.saveConfig(Path("saved_plugins.yaml"))  # getConfig() returns the same YAML as str

The same methods exist for inverse kinematics (addInvKinPlugin, getInvKinPlugins, …, createInvKin). A solver keeps its factory alive: its code lives in a plugin library the factory loaded.

setDefault…KinPlugin and remove…KinPlugin raise KeyError for an unknown group or solver. saveConfig raises OSError when the file cannot be written.

A group's last solver cannot be removed

remove…KinPlugin raises KinematicsPluginRemovalError (a RuntimeError) when the solver is the last one of its group, and leaves the factory unchanged. tesseract 0.35.0 erases the group in that case and then reads, and for the default solver writes, through the erased map iterator (kinematics_plugin_factory.cpp:150–154; reported upstream as tesseract#1381). The guard goes once a fixed tesseract is the minimum version.

Usage Example

Python
from tesseract_robotics.planning import Robot
from tesseract_robotics.tesseract_common import Isometry3d
from tesseract_robotics.tesseract_kinematics import KinGroupIKInput
import numpy as np

# Load robot
robot = Robot.from_tesseract_support("abb_irb2400")
manip = robot.env.getKinematicGroup("manipulator")

# Current pose
joints = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
fk = manip.calcFwdKin(joints)
current_pose = fk["tool0"]

# Move 10cm in X — build a new 4x4 matrix
mat = current_pose.matrix().copy()
mat[0, 3] += 0.1
target_pose = Isometry3d(mat)

# Solve IK
solutions = manip.calcInvKin(KinGroupIKInput(target_pose, "base_link", "tool0"), joints)

if solutions:
    # Verify solution
    fk_check = manip.calcFwdKin(solutions[0])
    check_pose = fk_check["tool0"]

    error = np.linalg.norm(
        target_pose.translation() - check_pose.translation()
    )
    print(f"Position error: {error:.6f} m")

Tips

  1. IK Seeds: Better seeds = faster convergence
  2. Multiple Solutions: Most 6-DOF robots have up to 8 IK solutions
  3. Joint Limits: IK solvers respect joint limits
  4. Analytical vs Numerical: OPW (UR, ABB) is analytical and fast; KDL is numerical

Auto-generated API Reference

tesseract_kinematics Python bindings

UR10Parameters module-attribute

Python
UR10Parameters: URParameters = ...

UR5Parameters module-attribute

Python
UR5Parameters: URParameters = ...

UR3Parameters module-attribute

Python
UR3Parameters: URParameters = ...

UR10eParameters module-attribute

Python
UR10eParameters: URParameters = ...

UR5eParameters module-attribute

Python
UR5eParameters: URParameters = ...

UR3eParameters module-attribute

Python
UR3eParameters: URParameters = ...

URParameters

d1 property writable

Python
d1: float

a2 property writable

Python
a2: float

a3 property writable

Python
a3: float

d4 property writable

Python
d4: float

d5 property writable

Python
d5: float

d6 property writable

Python
d6: float

KinGroupIKInput

pose property writable

Python
pose: Isometry3d

working_frame property writable

Python
working_frame: str
Python
tip_link_name: str

KinGroupIKInputs

__init__

Python
__init__() -> None

__len__

Python
__len__() -> int

__getitem__

Python
__getitem__(arg: int) -> KinGroupIKInput

append

Python
append(arg: KinGroupIKInput) -> None

clear

Python
clear() -> None

ForwardKinematics

calcFwdKin

Python
calcFwdKin(joint_angles: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> dict[str, tesseract_robotics.tesseract_common._tesseract_common.Isometry3d]

calcJacobian

Python
calcJacobian(joint_angles: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float], link_name: str) -> Annotated[NDArray[numpy.float64], dict(shape=(None, None), order=F)]

getBaseLinkName

Python
getBaseLinkName() -> str

getJointNames

Python
getJointNames() -> list[str]

getTipLinkNames

Python
getTipLinkNames() -> list[str]

numJoints

Python
numJoints() -> int

getSolverName

Python
getSolverName() -> str

clone

Python
clone() -> ForwardKinematics

InverseKinematics

calcInvKin

Python
calcInvKin(tip_link_poses: Mapping[str, Isometry3d], seed: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> list[Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]]

getJointNames

Python
getJointNames() -> list[str]

numJoints

Python
numJoints() -> int

getBaseLinkName

Python
getBaseLinkName() -> str

getWorkingFrame

Python
getWorkingFrame() -> str

getTipLinkNames

Python
getTipLinkNames() -> list[str]

getSolverName

Python
getSolverName() -> str

clone

Python
clone() -> InverseKinematics

JointGroup

__init__

Python
__init__(name: str, joint_names: Sequence[str], scene_graph: SceneGraph, scene_state: SceneState) -> None

calcFwdKin

Python
calcFwdKin(joint_angles: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> dict[str, tesseract_robotics.tesseract_common._tesseract_common.Isometry3d]

calcJacobianWithPoint

Python
calcJacobianWithPoint(joint_angles: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float], link_name: str, link_point: Annotated[NDArray[float64], dict(shape=3, order=C)] | Sequence[float]) -> Annotated[NDArray[numpy.float64], dict(shape=(None, None), order=F)]

Alias of calcJacobian(joint_angles, link_name, link_point), the native form.

getJointNames

Python
getJointNames() -> list[str]

getLinkNames

Python
getLinkNames() -> list[str]

getActiveLinkNames

Python
getActiveLinkNames() -> list[str]

getStaticLinkNames

Python
getStaticLinkNames() -> list[str]

isActiveLinkName

Python
isActiveLinkName(link_name: str) -> bool

hasLinkName

Python
hasLinkName(link_name: str) -> bool

getLimits

Python
getLimits() -> tesseract_robotics.tesseract_common._tesseract_common.KinematicLimits

setLimits

Python
setLimits(limits: KinematicLimits) -> None

getRedundancyCapableJointIndices

Python
getRedundancyCapableJointIndices() -> list[int]

numJoints

Python
numJoints() -> int

getBaseLinkName

Python
getBaseLinkName() -> str

getName

Python
getName() -> str

checkJoints

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

KinematicGroup

Bases: JointGroup

__init__

Python
__init__(name: str, joint_names: Sequence[str], inv_kin: InverseKinematics, scene_graph: SceneGraph, scene_state: SceneState) -> None

calcInvKinMultiple

Python
calcInvKinMultiple(tip_link_poses: Sequence[KinGroupIKInput], seed: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float]) -> list[Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]]

Alias of calcInvKin(tip_link_poses, seed) with a list, the native form.

getAllValidWorkingFrames

Python
getAllValidWorkingFrames() -> list[str]

getAllPossibleTipLinkNames

Python
getAllPossibleTipLinkNames() -> list[str]

getInverseKinematics

Python
getInverseKinematics() -> InverseKinematics

KinematicsPluginFactory

addSearchPath

Python
addSearchPath(path: str) -> None

getSearchPaths

Python
getSearchPaths() -> list[str]

addSearchLibrary

Python
addSearchLibrary(library_name: str) -> None

getSearchLibraries

Python
getSearchLibraries() -> list[str]

addFwdKinPlugin

Python
addFwdKinPlugin(group_name: str, solver_name: str, plugin_info: PluginInfo) -> None

getFwdKinPlugins

Python
getFwdKinPlugins() -> dict[str, tesseract_robotics.tesseract_common._tesseract_common.PluginInfoContainer]

removeFwdKinPlugin

Python
removeFwdKinPlugin(group_name: str, solver_name: str) -> None

Remove a forward kinematics solver from a group.

Raises:

Type Description
KeyError

the group or the solver is unknown.
KinematicsPluginRemovalError

it is the group's last solver.

setDefaultFwdKinPlugin

Python
setDefaultFwdKinPlugin(group_name: str, solver_name: str) -> None

Set a group's default forward kinematics solver.

Raises:

Type Description
KeyError

the group or the solver is unknown.

getDefaultFwdKinPlugin

Python
getDefaultFwdKinPlugin(group_name: str) -> str

addInvKinPlugin

Python
addInvKinPlugin(group_name: str, solver_name: str, plugin_info: PluginInfo) -> None

getInvKinPlugins

Python
getInvKinPlugins() -> dict[str, tesseract_robotics.tesseract_common._tesseract_common.PluginInfoContainer]

removeInvKinPlugin

Python
removeInvKinPlugin(group_name: str, solver_name: str) -> None

Remove an inverse kinematics solver from a group.

Raises:

Type Description
KeyError

the group or the solver is unknown.
KinematicsPluginRemovalError

it is the group's last solver.

setDefaultInvKinPlugin

Python
setDefaultInvKinPlugin(group_name: str, solver_name: str) -> None

Set a group's default inverse kinematics solver.

Raises:

Type Description
KeyError

the group or the solver is unknown.

getDefaultInvKinPlugin

Python
getDefaultInvKinPlugin(group_name: str) -> str

saveConfig

Python
saveConfig(file_path: str | PathLike) -> None

getConfig

Python
getConfig() -> str

The factory configuration as a YAML document string.

KinematicsPluginRemovalError

Bases: RuntimeError

Refused to remove a group's last kinematics solver: tesseract 0.35.0 then reads and writes through an erased map iterator (kinematics_plugin_factory.cpp:150-154, tesseract#1381).

ManipulabilityEllipsoid

eigen_values property

Python
eigen_values: Annotated[NDArray[float64], dict(shape=(None,), order=C)]

measure property

Python
measure: float

condition property

Python
condition: float

volume property

Python
volume: float

__init__

Python
__init__() -> None

__repr__

Python
__repr__() -> str

Manipulability

m property

Python
m: ManipulabilityEllipsoid

m_linear property

Python
m_linear: ManipulabilityEllipsoid

m_angular property

Python
m_angular: ManipulabilityEllipsoid

f property

Python
f: ManipulabilityEllipsoid

f_linear property

Python
f_linear: ManipulabilityEllipsoid

f_angular property

Python
f_angular: ManipulabilityEllipsoid

__init__

Python
__init__() -> None

__repr__

Python
__repr__() -> str

getRedundantSolutions

Python
getRedundantSolutions(sol: Annotated[NDArray[float64], dict(shape=(None,), writable=False)] | Sequence[float], limits: Annotated[NDArray[float64], dict(shape=(None, 2), writable=False)] | Sequence[Sequence[float]], redundancy_capable_joints: Sequence[int]) -> list[Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]]

Get redundant solutions for a joint configuration by adding ± 2*pi to redundancy capable joints

isNearSingularity

Python
isNearSingularity(jacobian: Annotated[NDArray[float64], dict(shape=(None, None), writable=False)] | Sequence[Sequence[float]], threshold: float = 0.01) -> bool

True when the smallest singular value of jacobian is below threshold.

Raises:

Type Description
ValueError

jacobian is empty.

calcManipulability

Python
calcManipulability(jacobian: Annotated[NDArray[float64], dict(shape=(6, None), writable=False)] | Sequence[Sequence[float]]) -> Manipulability

Manipulability and force ellipsoids of a 6-row jacobian.

When an ellipsoid's smallest eigenvalue is ~0 (a singular jacobian), its measure and condition are the largest double (sys.float_info.max), as upstream returns them (utils.cpp:240-244).

harmonizeTowardZero

Python
harmonizeTowardZero(qs: Annotated[NDArray[float64], dict(shape=(None,), order=C)], redundancy_capable_joints: Sequence[int]) -> None

Wrap qs[i] into [-pi, pi) in place for each i in redundancy_capable_joints.

Raises:

Type Description
IndexError

an index is outside qs; qs is left unchanged.

harmonizeTowardMedian

Python
harmonizeTowardMedian(qs: Annotated[NDArray[float64], dict(shape=(None,), order=C)], redundancy_capable_joints: Sequence[int], position_limits: Annotated[NDArray[float64], dict(shape=(None, 2), writable=False)] | Sequence[Sequence[float]]) -> None

Wrap qs[i] into [median - pi, median + pi) in place, median the midpoint of row i of position_limits.

Raises:

Type Description
IndexError

an index is outside qs or position_limits; qs is left unchanged.

isValid

Python
isValid(qs: Sequence[float]) -> bool

True when all six values are finite.

checkKinematics

Python
checkKinematics(manip: KinematicGroup, tol: float = 0.001) -> bool

Round-trip self-check: FK then IK for every working frame and tip link of manip.

False when an IK solution's translation or angle distance from the FK pose exceeds tol (validate.cpp:38-175). True also when IK found no solution at all: only failed solutions make it False.