Skip to content

tesseract_robotics.state_solver

Forward kinematics of a whole scene graph: joint values in, SceneState (joint values and every link and joint transform) out. OFKTStateSolver is the mutable solver an Environment uses; KDLStateSolver is built on a KDL tree. Both take the SceneGraph in their constructor.

Python
import numpy as np
from tesseract_robotics.state_solver import OFKTStateSolver

solver = OFKTStateSolver(scene_graph)
names = solver.getActiveJointIds()

solver.setState(np.zeros(len(names)))           # every active joint, in getActiveJointIds() order
solver.setState({names[0]: 0.3})                # by name
solver.setState(names[:1], np.array([0.3]))     # names and values
state = solver.getState()                       # current SceneState
probe = solver.getState({names[0]: 0.5})        # a state for other values; the solver is unchanged
jac = solver.getJacobian(names, np.full(len(names), 0.5), "tool0")  # values in getActiveJointIds() order
transforms = solver.getLinkTransforms(names, np.zeros(len(names)))  # dict[str, Isometry3d]

Overloads and validation

setState and getState take the joint values as a vector over the active joints, a dict by name, or names plus values, each with an optional floating_joint_values (dict[str, Isometry3d]); getJacobian takes names plus values, or a vector over the active joints, with the link. It has no dict form. setState(floating_joint_values) and getState(floating_joint_values) set only floating joints. A dict of floats selects the joint-value form and a dict of Isometry3d the floating form; an empty {} is the joint-value form.

Every overload checks its input before calling the solver and raises ValueError for a joint name that is not an active joint, a floating name that is not a floating joint, names and values of different lengths, or a vector whose length is not the number of active joints. An unknown link in getJacobian raises KeyError. Upstream OFKTStateSolver dereferences a null node for an unknown joint name (the GH #43 crash), so this validation is what keeps the process alive.

Note

KDLStateSolver ignores floating_joint_values, and setState(floating_joint_values) / getState(floating_joint_values) raise RuntimeError (upstream: "not supported").

SceneState

joints is a dict[str, float]; floating_joints, link_transforms and joint_transforms are dict[str, Isometry3d]. Each transform map is a copy on read and is assigned as a whole, so state.link_transforms["tool0"] = tf changes nothing.

Python
from tesseract_robotics.geometry import Box
from tesseract_robotics.scene_graph import Joint, JointType, Link, SceneGraph

graph = SceneGraph()
graph.addLink(Link("base_link"))
graph.addLink(Link("floater"))
floating = Joint("joint_f")
floating.type = JointType.FLOATING
floating.parent_link_id = "base_link"
floating.child_link_id = "floater"
graph.addJoint(floating)
solver = OFKTStateSolver(graph)
names = solver.getActiveJointIds()

solver.setState({"joint_f": Isometry3d.Identity()})
state = solver.getState()
state.floating_joints                         # {"joint_f": Isometry3d}
state.getFloatingJointValues(["joint_f"])     # KeyError for a name that is not a floating joint
state.getJointValues(names)                   # NDArray, in the given order
state == solver.getState()                    # True

== and != are upstream's comparison: exact joint values, and every transform within isApprox(1e-5), Eigen's relative test on the 4×4 matrix. SceneState is mutable and unhashable.

Inserting a scene graph

MutableStateSolver.insertSceneGraph(scene_graph, joint, prefix="") attaches another graph with joint, whose parent is a link of the solver and whose child is the inserted graph's root (with prefix already applied). It returns False, as upstream, when a link is missing or the joint name exists.

Reference

tesseract_state_solver Python bindings

SceneState

joints property writable

Python
joints: dict[str, float]

Joint values: joint name -> value [rad or m].

floating_joints property writable

Python
floating_joints: dict[str, Isometry3d]

Floating-joint transforms: joint name -> Isometry3d.

Python
link_transforms: dict[str, Isometry3d]

World transform of every link: link name -> Isometry3d (a copy).

joint_transforms property writable

Python
joint_transforms: dict[str, Isometry3d]

World transform of every joint: joint name -> Isometry3d (a copy).

__hash__ class-attribute instance-attribute

Python
__hash__: None = None

__init__

Python
__init__() -> None

getJointValues

Python
getJointValues(joint_ids: Sequence[JointId | str]) -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]

The values of joint_ids, in that order; an unknown id raises KeyError.

getFloatingJointValues

Python
getFloatingJointValues(joint_ids: Sequence[JointId | str]) -> dict[str, tesseract_robotics.common.Isometry3d]

__eq__

Python
__eq__(other: object) -> bool

__ne__

Python
__ne__(other: object) -> bool

StateSolver

getRandomState

Python
getRandomState() -> SceneState

A state at uniformly random joint values within the joint limits.

getJointIds

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

Every joint id of the scene graph.

getFloatingJointIds

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

The floating joint ids.

getActiveJointIds

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

The active (non-fixed, non-mimic) joint ids, in state-vector order.

getBaseLinkId

Python
getBaseLinkId() -> tesseract_robotics.common.LinkId

The root link id.

getLinkIds

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

Every link id.

getActiveLinkIds

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

Links moved by an active joint.

getStaticLinkIds

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

Links no active joint moves.

isActiveLinkId

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

True if an active joint moves link_id.

hasLinkId

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

True if link_id is a link of the scene graph.

getLinkTransform

Python
getLinkTransform(link_id: LinkId | str) -> tesseract_robotics.common.Isometry3d

The current world transform of link_id.

getRelativeLinkTransform

Python
getRelativeLinkTransform(from_link_id: LinkId | str, to_link_id: LinkId | str) -> tesseract_robotics.common.Isometry3d

The transform of to_link_id in the frame of from_link_id, at the current state.

getLimits

Python
getLimits() -> tesseract_robotics.common.KinematicLimits

The active joints' position, velocity, acceleration and jerk limits, in getActiveJointIds order.

clone

Python
clone() -> StateSolver

An independent copy at the same state.

MutableStateSolver

Bases: StateSolver

setRevision

Python
setRevision(revision: int) -> None

Set the revision this solver tracks (the environment's command count).

getRevision

Python
getRevision() -> int

The revision this solver tracks.

Python
addLink(link: Link, joint: Joint) -> bool

Add link attached by joint; False if the link or joint exists.

Python
moveLink(joint: Joint) -> bool

Re-attach a link by replacing its parent joint with joint; False on failure.

Python
removeLink(name: LinkId | str) -> bool

Remove the link name and its subtree; False if unknown.

replaceJoint

Python
replaceJoint(joint: Joint) -> bool

Replace the joint of the same name (same child link); False on failure.

removeJoint

Python
removeJoint(name: JointId | str) -> bool

Remove the joint name and its child subtree; False if unknown.

moveJoint

Python
moveJoint(name: JointId | str, parent_link: LinkId | str) -> bool

Re-parent the joint name onto parent_link; False on failure.

changeJointOrigin

Python
changeJointOrigin(name: JointId | str, new_origin: Isometry3d) -> bool

Set the origin of the joint name; False if unknown.

changeJointPositionLimits

Python
changeJointPositionLimits(name: JointId | str, lower: float, upper: float) -> bool

Set the position limits of the joint name; False if unknown.

changeJointVelocityLimits

Python
changeJointVelocityLimits(name: JointId | str, limit: float) -> bool

Set the velocity limit of the joint name; False if unknown.

changeJointAccelerationLimits

Python
changeJointAccelerationLimits(name: JointId | str, limit: float) -> bool

Set the acceleration limit of the joint name; False if unknown.

changeJointJerkLimits

Python
changeJointJerkLimits(name: JointId | str, limit: float) -> bool

Set the jerk limit of the joint name; False if unknown.

insertSceneGraph

Python
insertSceneGraph(scene_graph: SceneGraph, joint: Joint, prefix: str = '') -> bool

KDLStateSolver

Bases: StateSolver

State solver on a KDL tree. Floating joint values are ignored, and setState / getState with only floating_joint_values raise RuntimeError (upstream: not supported).

__init__

Python
__init__(scene_graph: SceneGraph) -> None

clone

Python
clone() -> StateSolver

An independent copy at the same state.

OFKTStateSolver

Bases: MutableStateSolver

clone

Python
clone() -> StateSolver

An independent copy at the same state.