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.
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.
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
¶
Joint values: joint name -> value [rad or m].
floating_joints
property
writable
¶
Floating-joint transforms: joint name -> Isometry3d.
link_transforms
property
writable
¶
World transform of every link: link name -> Isometry3d (a copy).
joint_transforms
property
writable
¶
World transform of every joint: joint name -> Isometry3d (a copy).
getJointValues
¶
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
¶
getFloatingJointValues(joint_ids: Sequence[JointId | str]) -> dict[str, tesseract_robotics.common.Isometry3d]
StateSolver
¶
getRandomState
¶
A state at uniformly random joint values within the joint limits.
getJointIds
¶
Every joint id of the scene graph.
getFloatingJointIds
¶
The floating joint ids.
getActiveJointIds
¶
The active (non-fixed, non-mimic) joint ids, in state-vector order.
getActiveLinkIds
¶
Links moved by an active joint.
getStaticLinkIds
¶
Links no active joint moves.
isActiveLinkId
¶
True if an active joint moves link_id.
hasLinkId
¶
True if link_id is a link of the scene graph.
getLinkTransform
¶
The current world transform of link_id.
getRelativeLinkTransform
¶
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
¶
The active joints' position, velocity, acceleration and jerk limits, in getActiveJointIds order.
MutableStateSolver
¶
Bases: StateSolver
setRevision
¶
Set the revision this solver tracks (the environment's command count).
addLink
¶
Add link attached by joint; False if the link or joint exists.
moveLink
¶
Re-attach a link by replacing its parent joint with joint; False on failure.
removeLink
¶
Remove the link name and its subtree; False if unknown.
replaceJoint
¶
Replace the joint of the same name (same child link); False on failure.
removeJoint
¶
Remove the joint name and its child subtree; False if unknown.
moveJoint
¶
Re-parent the joint name onto parent_link; False on failure.
changeJointOrigin
¶
Set the origin of the joint name; False if unknown.
changeJointPositionLimits
¶
Set the position limits of the joint name; False if unknown.
changeJointVelocityLimits
¶
Set the velocity limit of the joint name; False if unknown.
changeJointAccelerationLimits
¶
Set the acceleration limit of the joint name; False if unknown.
changeJointJerkLimits
¶
Set the jerk limit of the joint name; False if unknown.
insertSceneGraph
¶
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).
OFKTStateSolver
¶
Bases: MutableStateSolver