Skip to content

tesseract_robotics.kinematics

Forward and inverse kinematics solvers.

Kinematic Groups

KinematicGroup

Full kinematics with FK and IK.

Python
from tesseract_robotics.kinematics import KinematicGroup

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

# Joint information
joint_names = manip.getJointIds()
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", "OPWInvKin", 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.kinematics import JointGroup

group = env.getJointGroup("manipulator")

# Same interface as KinematicGroup for FK

Jacobians

calcJacobian has the four C++ overloads. base_link_id 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 is removed: pass the point to 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.common import Isometry3d
from tesseract_robotics.kinematics import KinGroupIKInput
import numpy as np

manip = env.getKinematicGroup("manipulator")

# Build a reachable target: the tool0 pose at known joint values
target_pose = Isometry3d(manip.calcFwdKin(np.array([0.0, -0.5, 0.5, 0.0, 0.5, 0.0]))["tool0"])

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

Python
another_input = KinGroupIKInput(target_pose, "base_link", "tool0")
solutions = manip.calcInvKin([ik_input, another_input], seed)

Redundant Solutions

Get all IK solutions including joint wraparound.

Python
from tesseract_robotics.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.common import Isometry3d
from tesseract_robotics.kinematics import (
    calcManipulability, calcManipulabilityVolume, calcManipulabilityVolume_each,
    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
w = calcManipulabilityVolume(J)      # bitwise manip.m.volume, from one SVD instead of three
ws = calcManipulabilityVolume_each(np.stack([J, J]))  # (M, 6, n) -> (M,), GIL released
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.joint_limits)  # in place, around each joint's limit midpoint

isValid([0.0] * 6)                   # all six finite
checkKinematics(env.getKinematicGroup("manipulator"))  # 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, calcManipulabilityVolume TypeError for a jacobian without 6 rows; EmptyJacobianError for one without columns; NonFiniteJacobianError for a NaN or infinite entry
calcManipulabilityVolume_each TypeError for anything but an (M, 6, n) stack; EmptyJacobianError for n = 0; NonFiniteJacobianError naming the index of the jacobian with a NaN or infinite entry
isNearSingularity EmptyJacobianError for a matrix without rows or columns; NonFiniteJacobianError for a NaN or infinite entry
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 and calcManipulabilityVolume_each release the GIL while they run.

Manipulability

Yoshikawa's manipulability of a 6 × n jacobian \(J\) is the volume of its velocity ellipsoid, the product of its singular values \(\sigma_i\):

\[ w(J) = \sqrt{\det\left(J J^{\mathsf T}\right)} = \prod_{i=1}^{6} \sigma_i \]

It is 0 at a singularity, and for every jacobian with fewer than 6 columns. The rows of \(J\) mix m/rad (linear, rows 0–2) and rad/rad (angular, rows 3–5), so \(w\) depends on the length unit: compare it between configurations of one robot, not between robots.

call returns cost
calcManipulability(J) Manipulability: the velocity ellipsoids m, m_linear, m_angular and the force ellipsoids f, f_linear, f_angular, each with eigen_values, measure, condition and volume three SVDs: \(J\), its linear rows and its angular rows
calcManipulabilityVolume(J) float, the same bits as calcManipulability(J).m.volume one SVD
calcManipulabilityVolume_each(Js) (M,) array for an (M, 6, n) stack; row i is the same bits as calcManipulabilityVolume(Js[i]) one SVD per jacobian, with the GIL released

All three follow tesseract's rule (tesseract#1396). \(w\) is the product of the singular values, multiplied in increasing order, when \(J\) has full numerical row rank, and exactly 0.0 below it. A singular value counts as zero when \(\sigma_i \le \min(6, n)\,\varepsilon\,\sigma_{\max}\) (Eigen's JacobiSVD rank threshold, \(\varepsilon\) = 2.2e-16). The values come from the SVD of \(J\), not from \(J J^{\mathsf T}\), which would square the condition number and lose every singular value below \(\sqrt{\varepsilon}\,\sigma_{\max}\).

Python
from tesseract_robotics.kinematics import NonFiniteJacobianError

Js = np.stack([J, 2.0 * J])
ws = calcManipulabilityVolume_each(Js)               # C-contiguous float64 is read in place
assert ws[0] == calcManipulabilityVolume(J) == calcManipulability(J).m.volume

bad = J.copy()
bad[2, 3] = np.nan
try:
    calcManipulabilityVolume_each(np.stack([J, bad]))
except NonFiniteJacobianError as error:              # a ValueError
    assert "jacobian 1 is not finite (entry [2, 3] = nan)" in str(error)

EmptyJacobianError and NonFiniteJacobianError are ValueErrors. A jacobian with a NaN or infinite entry is refused because its SVD reads rank 0, which is the answer for a singularity: volume 0.0, and isNearSingularity true. A blown-up forward-kinematics chain would otherwise read as a wrist singularity.

calcManipulabilityVolume is tesseract's function (tesseract#1408, carried on the build's fork until it merges). calcManipulabilityVolume_each is the bindings' batch form of it and keeps its spelling (binding conventions, rule 4). Every decomposition runs inside tesseract, in the routine calcManipulability uses, which is why the values are the same bits on every platform and whatever was imported first: a second SVD compiled into the binding module differed in the last bits on linux-arm64.

claim checked by
same bits as calcManipulability(J).m.volume, for n = 5, 6, 7 test_calc_manipulability_volume_is_the_full_calls_volume_bitwise, test_calc_manipulability_volume_each_row_is_the_per_jacobian_call_bitwise
same bits across the rank threshold, \(\sigma_{\min}/\sigma_{\max}\) from 1e-1 to 1e-20 test_calc_manipulability_volume_is_the_full_calls_volume_across_the_rank_threshold
a near-singular \(J\) (\(\sigma_{\min}\) = 1e-6) reads \(\prod \sigma_i\), not 0 test_calc_manipulability_volume_near_singular_is_singular_value_product
same bits with environment imported before kinematics test_calc_manipulability_volume_is_the_full_calls_volume_with_environment_imported_first
the GIL is released test_calc_manipulability_volume_each_releases_gil

The tests are in tests/tesseract_kinematics/test_kinematics_utils.py and tests/test_gil_entry_points.py. No timing is claimed: none has been measured on this build.

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.kinematics import (
    URParameters, UR10Parameters, UR10eParameters,
    UR5Parameters, UR5eParameters, UR3Parameters, UR3eParameters
)

# Get default parameters
params = UR10Parameters  # a ready-made URParameters, not a class
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 import get_tesseract_support_path
from tesseract_robotics.kinematics import KinematicsPluginFactory

plugins_path = get_tesseract_support_path() / "urdf" / "abb_irb2400_plugins.yaml"
factory = KinematicsPluginFactory(plugins_path, 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.common import Isometry3d
from tesseract_robotics.kinematics import KinGroupIKInput
import numpy as np

# Load robot
locator = GeneralResourceLocator(paths=[get_tesseract_resource_path()])  # the tesseract-robotics-nanobind-support wheel
robot = Robot.from_tesseract_support("abb_irb2400", locator)
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

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

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

calcJacobian

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

getBaseLinkId

Python
getBaseLinkId() -> tesseract_robotics.common.LinkId

getJointIds

Python
getJointIds() -> list[tesseract_robotics.common.JointId]

getTipLinkIds

Python
getTipLinkIds() -> list[tesseract_robotics.common.LinkId]

numJoints

Python
numJoints() -> int

getSolverName

Python
getSolverName() -> str

clone

Python
clone() -> ForwardKinematics

An independent copy; it keeps this object alive (gh-72).

InverseKinematics

calcInvKin

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

getJointIds

Python
getJointIds() -> list[tesseract_robotics.common.JointId]

numJoints

Python
numJoints() -> int

getBaseLinkId

Python
getBaseLinkId() -> tesseract_robotics.common.LinkId

getWorkingFrame

Python
getWorkingFrame() -> tesseract_robotics.common.LinkId

getTipLinkIds

Python
getTipLinkIds() -> list[tesseract_robotics.common.LinkId]

getSolverName

Python
getSolverName() -> str

clone

Python
clone() -> InverseKinematics

An independent copy; it keeps this object alive (gh-72).

JointGroup

__init__

Python
__init__(name: str, joint_ids: Sequence[JointId | 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.common.Isometry3d]

getJointIds

Python
getJointIds() -> list[tesseract_robotics.common.JointId]

getLinkIds

Python
getLinkIds() -> list[tesseract_robotics.common.LinkId]

getActiveLinkIds

Python
getActiveLinkIds() -> list[tesseract_robotics.common.LinkId]

getStaticLinkIds

Python
getStaticLinkIds() -> list[tesseract_robotics.common.LinkId]

isActiveLinkId

Python
isActiveLinkId(link_id: LinkId | str) -> bool

hasLinkId

Python
hasLinkId(link_id: LinkId | str) -> bool

getLimits

Python
getLimits() -> tesseract_robotics.common.KinematicLimits

setLimits

Python
setLimits(limits: KinematicLimits) -> None

getRedundancyCapableJointIndices

Python
getRedundancyCapableJointIndices() -> list[int]

numJoints

Python
numJoints() -> int

getBaseLinkId

Python
getBaseLinkId() -> tesseract_robotics.common.LinkId

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_ids: Sequence[JointId | str], inv_kin: InverseKinematics, scene_graph: SceneGraph, scene_state: SceneState) -> None

getAllValidWorkingFrames

Python
getAllValidWorkingFrames() -> list[tesseract_robotics.common.LinkId]

getAllPossibleTipLinkIds

Python
getAllPossibleTipLinkIds() -> list[tesseract_robotics.common.LinkId]

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

EmptyJacobianError

Bases: ValueError

A jacobian with no rows or no columns: it has no singular values.

NonFiniteJacobianError

Bases: ValueError

A jacobian with a NaN or infinite entry: its SVD reads rank 0, which is the answer for a singularity.

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

PositionerSampleResolution

A positioner joint's sample resolution, optionally with its own sampled range (by default the joint's limits).

name property writable

Python
name: str

Joint name

value property writable

Python
value: float

Sample resolution

min property writable

Python
min: float | None

Lower bound; None: the joint's lower limit

max property writable

Python
max: float | None

Upper bound; None: the joint's upper limit

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
EmptyJacobianError

jacobian has no rows or no columns.

NonFiniteJacobianError

jacobian has a NaN or infinite entry.

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:256-261).

Raises:

Type Description
EmptyJacobianError

jacobian has no columns.

NonFiniteJacobianError

jacobian has a NaN or infinite entry.

calcManipulabilityVolume

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

Yoshikawa manipulability of a 6-row jacobian: the product of its singular values.

Bitwise calcManipulability(jacobian).m.volume, from one SVD instead of three and without the six ellipsoids. Exactly 0.0 below full numerical row rank, so for every n < 6.

Parameters:

Name Type Description Default
jacobian Annotated[NDArray[float64], dict(shape=(6, None), writable=False)] | Sequence[Sequence[float]]

(6, n) jacobian, linear rows 0-2 over angular rows 3-5.

required

Returns:

Type Description
float

The volume of the velocity ellipsoid. Its unit mixes m/rad and rad/rad.

Raises:

Type Description
EmptyJacobianError

jacobian has no columns.

NonFiniteJacobianError

jacobian has a NaN or infinite entry.

calcManipulabilityVolume_each

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

Yoshikawa manipulability of each jacobian of a stack.

Row i is bitwise calcManipulabilityVolume(jacobians[i]). Releases the GIL.

Parameters:

Name Type Description Default
jacobians Annotated[NDArray[float64], dict(shape=(None, 6, None), order=C, writable=False)]

(M, 6, n) stack. A C-contiguous float64 array is read in place; any other layout or dtype is converted first.

required

Returns:

Type Description
Annotated[NDArray[float64], dict(shape=(None,), order=C)]

The (M,) volumes.

Raises:

Type Description
EmptyJacobianError

n is 0.

NonFiniteJacobianError

a jacobian has a NaN or infinite entry; the message names its index.

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.

gatherJointLimits

Python
gatherJointLimits(scene_graph: SceneGraph, joint_ids: Sequence[JointId | str]) -> Annotated[NDArray[numpy.float64], dict(shape=(None, 2), order=F)]

The joints' position limits as an (N, 2) array [lower, upper]; a continuous joint reports [-pi, pi].

Raises:

Type Description
RuntimeError

a joint is not in scene_graph, or a non-continuous joint has no limits.

buildSampleGrid

Python
buildSampleGrid(range: Annotated[NDArray[float64], dict(shape=(None, 2), order=F)] | Sequence[Sequence[float]], resolution: Annotated[NDArray[float64], dict(shape=(None,), order=C)] | Sequence[float]) -> list[Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]]

Per joint i, ceil((range[i, 1] - range[i, 0]) / resolution[i]) + 1 evenly spaced samples over range[i] (one array per joint, not their product).

Raises:

Type Description
RuntimeError

sizes disagree, a bound is non-finite or inverted, a resolution is not finite and > 0, or a joint would need more than 10 million samples.

computeChainReachUpperBound

Python
computeChainReachUpperBound(scene_graph: SceneGraph, base_link_id: LinkId | str, tip_link_id: LinkId | str) -> float

An upper bound, in metres, on the base-to-tip distance over every joint configuration: the sum of the joint origin offsets along the path plus each prismatic joint's largest |limit|.

Raises:

Type Description
RuntimeError

a link is missing, no path joins them, or a joint on it has unbounded translation.

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.