Skip to content

tesseract_robotics.tesseract_environment

Environment management and scene modification commands.

Environment

Central class containing robot model, collision, and kinematics.

Python
from tesseract_robotics.tesseract_environment import Environment

# Create empty environment
env = Environment()

# Initialize from URDF/SRDF
env.init(urdf_string, srdf_string, locator)

# Or use Robot helper
from tesseract_robotics.planning import Robot
robot = Robot.from_urdf("robot.urdf", "robot.srdf")
env = robot.env

Scene Graph Access

Python
# Get scene graph (read-only)
scene = env.getSceneGraph()
links = scene.getLinks()
joints = scene.getJoints()

# Get specific elements
link = env.getLink("base_link")
joint = env.getJoint("joint_1")

State Management

Python
# Get current state
state = env.getState()
joint_positions = state.joints

# Set joint state
env.setState(["joint_1", "joint_2"], [0.5, -0.3])
# Or with dict
env.setState({"joint_1": 0.5, "joint_2": -0.3})

# Get link transform
tcp = env.getLinkTransform("tool0")

Kinematics

Python
# Get kinematic group (for FK/IK)
manip = env.getKinematicGroup("manipulator")

# Get joint group (FK only, no IK)
group = env.getJointGroup("manipulator")

# Available groups
groups = env.getGroupNames()

Collision

Python
# Get collision managers
discrete = env.getDiscreteContactManager()
continuous = env.getContinuousContactManager()

# Allowed collision matrix
acm = env.getAllowedCollisionMatrix()

Environment Info

Python
# Check initialization
if env.isInitialized():
    print(f"Root link: {env.getRootLinkName()}")
    print(f"Revision: {env.getRevision()}")

Commands

Modify the environment with commands. Commands are tracked for undo/redo.

AddLinkCommand

Add a new link to the scene.

Python
from tesseract_robotics.tesseract_environment import AddLinkCommand
from tesseract_robotics.tesseract_scene_graph import Link, Joint

link = Link("obstacle")
# ... configure link with visual/collision

joint = Joint("obstacle_joint")
joint.type = JointType.FIXED
joint.parent_link_name = "world"
joint.child_link_name = "obstacle"

cmd = AddLinkCommand(link, joint)
env.applyCommand(cmd)

RemoveLinkCommand

Python
from tesseract_robotics.tesseract_environment import RemoveLinkCommand

cmd = RemoveLinkCommand("obstacle")
env.applyCommand(cmd)

MoveLinkCommand

Move a link to a new parent.

Python
from tesseract_robotics.tesseract_environment import MoveLinkCommand

joint = Joint("new_joint")
# ... configure joint

cmd = MoveLinkCommand(joint)
env.applyCommand(cmd)

ChangeJointOriginCommand

Change a joint's transform.

Python
from tesseract_robotics.tesseract_environment import ChangeJointOriginCommand

new_origin = Isometry3d.Identity()
new_origin.translate([0.1, 0, 0])

cmd = ChangeJointOriginCommand("obstacle_joint", new_origin)
env.applyCommand(cmd)

Joint Limit Commands

Python
from tesseract_robotics.tesseract_environment import (
    ChangeJointPositionLimitsCommand,
    ChangeJointVelocityLimitsCommand,
    ChangeJointAccelerationLimitsCommand,
)

# Position limits
cmd = ChangeJointPositionLimitsCommand("joint_1", -2.0, 2.0)

# Velocity limits
cmd = ChangeJointVelocityLimitsCommand("joint_1", 1.5)

# Acceleration limits
cmd = ChangeJointAccelerationLimitsCommand("joint_1", 5.0)

ChangeLinkCollisionEnabledCommand

Enable/disable collision for a link.

Python
from tesseract_robotics.tesseract_environment import ChangeLinkCollisionEnabledCommand

cmd = ChangeLinkCollisionEnabledCommand("gripper", False)  # disable
env.applyCommand(cmd)

ModifyAllowedCollisionsCommand

Update the allowed collision matrix.

Python
from tesseract_robotics.tesseract_environment import (
    ModifyAllowedCollisionsCommand, ModifyAllowedCollisionsType
)
from tesseract_robotics.tesseract_common import AllowedCollisionMatrix

acm = AllowedCollisionMatrix()
acm.addAllowedCollision("link_a", "link_b", "custom reason")

cmd = ModifyAllowedCollisionsCommand(acm, ModifyAllowedCollisionsType.ADD)
env.applyCommand(cmd)
ModifyAllowedCollisionsType Description
ADD Add entries to existing ACM
REMOVE Remove entries from ACM
REPLACE Replace entire ACM

ChangeCollisionMarginsCommand

Update collision margins.

Python
from tesseract_robotics.tesseract_environment import ChangeCollisionMarginsCommand
from tesseract_robotics.tesseract_common import CollisionMarginData

margins = CollisionMarginData()
margins.setDefaultCollisionMargin(0.05)

cmd = ChangeCollisionMarginsCommand(margins)
env.applyCommand(cmd)

Events

Subscribe to environment changes.

Python
from tesseract_robotics.tesseract_environment import (
    Events, Events_COMMAND_APPLIED, Events_SCENE_STATE_CHANGED,
    cast_CommandAppliedEvent, cast_SceneStateChangedEvent
)

def on_event(event):
    if event.type == Events_COMMAND_APPLIED:
        cmd_event = cast_CommandAppliedEvent(event)
        print(f"Command applied: revision {cmd_event.revision}")
    elif event.type == Events_SCENE_STATE_CHANGED:
        state_event = cast_SceneStateChangedEvent(event)
        print("State changed")

env.addEventCallback(Events_COMMAND_APPLIED, on_event)

Auto-generated API Reference

Events_COMMAND_APPLIED module-attribute

Python
Events_COMMAND_APPLIED: Events = COMMAND_APPLIED

Events_SCENE_STATE_CHANGED module-attribute

Python
Events_SCENE_STATE_CHANGED: Events = SCENE_STATE_CHANGED

ModifyAllowedCollisionsType_ADD module-attribute

Python
ModifyAllowedCollisionsType_ADD: ModifyAllowedCollisionsType = ADD

ModifyAllowedCollisionsType_REMOVE module-attribute

Python
ModifyAllowedCollisionsType_REMOVE: ModifyAllowedCollisionsType = REMOVE

ModifyAllowedCollisionsType_REPLACE module-attribute

Python
ModifyAllowedCollisionsType_REPLACE: ModifyAllowedCollisionsType = REPLACE

Events

Bases: Enum

COMMAND_APPLIED class-attribute instance-attribute

Python
COMMAND_APPLIED = 0

SCENE_STATE_CHANGED class-attribute instance-attribute

Python
SCENE_STATE_CHANGED = 1

Event

type property

Python
type: Events

CommandAppliedEvent

Bases: Event

revision property

Python
revision: int

SceneStateChangedEvent

Bases: Event

state property

Python
state: SceneState

EventCallbackFn

__init__

Python
__init__(callback: Callable) -> None

Command

getType

Python
getType() -> 'tesseract_environment::CommandType'

RemoveJointCommand

Bases: Command

__init__

Python
__init__(joint_name: str) -> None

getJointName

Python
getJointName() -> str

AddLinkCommand

Bases: Command

Python
getLink() -> tesseract_robotics.tesseract_scene_graph._tesseract_scene_graph.Link

getJoint

Python
getJoint() -> tesseract_robotics.tesseract_scene_graph._tesseract_scene_graph.Joint

replaceAllowed

Python
replaceAllowed() -> bool

RemoveLinkCommand

Bases: Command

__init__

Python
__init__(link_name: str) -> None

getLinkName

Python
getLinkName() -> str

AddSceneGraphCommand

Bases: Command

getSceneGraph

Python
getSceneGraph() -> tesseract_robotics.tesseract_scene_graph._tesseract_scene_graph.SceneGraph

getJoint

Python
getJoint() -> tesseract_robotics.tesseract_scene_graph._tesseract_scene_graph.Joint

getPrefix

Python
getPrefix() -> str

ModifyAllowedCollisionsType

Bases: Enum

ADD class-attribute instance-attribute

Python
ADD = 0

REMOVE class-attribute instance-attribute

Python
REMOVE = 1

REPLACE class-attribute instance-attribute

Python
REPLACE = 2

ModifyAllowedCollisionsCommand

Bases: Command

__init__

Python
__init__(acm: AllowedCollisionMatrix, type: ModifyAllowedCollisionsType) -> None

getModifyType

Python
getModifyType() -> ModifyAllowedCollisionsType

getAllowedCollisionMatrix

Python
getAllowedCollisionMatrix() -> tesseract_robotics.tesseract_common._tesseract_common.AllowedCollisionMatrix

RemoveAllowedCollisionLinkCommand

Bases: Command

__init__

Python
__init__(link_name: str) -> None

getLinkName

Python
getLinkName() -> str

ChangeJointPositionLimitsCommand

Bases: Command

getLimits

Python
getLimits() -> dict[str, tuple[float, float]]

ChangeJointVelocityLimitsCommand

Bases: Command

getLimits

Python
getLimits() -> dict[str, float]

ChangeJointAccelerationLimitsCommand

Bases: Command

getLimits

Python
getLimits() -> dict[str, float]

ChangeCollisionMarginsCommand

Bases: Command

getDefaultCollisionMargin

Python
getDefaultCollisionMargin() -> float | None

getCollisionMarginPairData

Python
getCollisionMarginPairData() -> tesseract_robotics.tesseract_common._tesseract_common.CollisionMarginPairData

getCollisionMarginPairOverrideType

Python
getCollisionMarginPairOverrideType() -> tesseract_robotics.tesseract_common._tesseract_common.CollisionMarginPairOverrideType

ChangeLinkCollisionEnabledCommand

Bases: Command

__init__

Python
__init__(link_name: str, enabled: bool) -> None

getLinkName

Python
getLinkName() -> str

getEnabled

Python
getEnabled() -> bool

ChangeLinkVisibilityCommand

Bases: Command

__init__

Python
__init__(link_name: str, visible: bool) -> None

getLinkName

Python
getLinkName() -> str

getEnabled

Python
getEnabled() -> bool

ChangeJointOriginCommand

Bases: Command

__init__

Python
__init__(joint_name: str, origin: Isometry3d) -> None

getJointName

Python
getJointName() -> str

getOrigin

Python
getOrigin() -> tesseract_robotics.tesseract_common._tesseract_common.Isometry3d

ChangeLinkOriginCommand

Bases: Command

__init__

Python
__init__(link_name: str, origin: Isometry3d) -> None

getLinkName

Python
getLinkName() -> str

getOrigin

Python
getOrigin() -> tesseract_robotics.tesseract_common._tesseract_common.Isometry3d

MoveJointCommand

Bases: Command

__init__

Python
__init__(joint_name: str, parent_link: str) -> None

getJointName

Python
getJointName() -> str
Python
getParentLink() -> str

MoveLinkCommand

Bases: Command

__init__

Python
__init__(joint: Joint) -> None

getJoint

Python
getJoint() -> tesseract_robotics.tesseract_scene_graph._tesseract_scene_graph.Joint

ReplaceJointCommand

Bases: Command

__init__

Python
__init__(joint: Joint) -> None

getJoint

Python
getJoint() -> tesseract_robotics.tesseract_scene_graph._tesseract_scene_graph.Joint

Environment

__init__

Python
__init__() -> None

initFromUrdf

Python
initFromUrdf(urdf_string: str, locator: ResourceLocator) -> bool

initFromUrdfSrdf

Python
initFromUrdfSrdf(urdf_string: str, srdf_string: str, locator: ResourceLocator) -> bool

isInitialized

Python
isInitialized() -> bool

reset

Python
reset() -> bool

clear

Python
clear() -> None

getRevision

Python
getRevision() -> int

getName

Python
getName() -> str

setName

Python
setName(name: str) -> None

getSceneGraph

Python
getSceneGraph() -> tesseract_robotics.tesseract_scene_graph._tesseract_scene_graph.SceneGraph

getState

Python
getState() -> tesseract_robotics.tesseract_state_solver._tesseract_state_solver.SceneState

getStateByMap

Python
getStateByMap(joints: Mapping[str, float]) -> tesseract_robotics.tesseract_state_solver._tesseract_state_solver.SceneState

getStateByNamesAndValues

Python
getStateByNamesAndValues(joint_names: Sequence[str], joint_values: Annotated[NDArray[float64], dict(shape=(None,), writable=False)]) -> tesseract_robotics.tesseract_state_solver._tesseract_state_solver.SceneState

setStateByNamesAndValues

Python
setStateByNamesAndValues(joint_names: Sequence[str], joint_values: Annotated[NDArray[float64], dict(shape=(None,), writable=False)]) -> None

addEventCallback

Python
addEventCallback(hash: int, fn: EventCallbackFn) -> None

removeEventCallback

Python
removeEventCallback(hash: int) -> None

clearEventCallbacks

Python
clearEventCallbacks() -> None

getStateSolver

Python
getStateSolver() -> tesseract_robotics.tesseract_state_solver._tesseract_state_solver.StateSolver

getJointNames

Python
getJointNames() -> list[str]

getActiveJointNames

Python
getActiveJointNames() -> list[str]

getLinkNames

Python
getLinkNames() -> list[str]

getActiveLinkNames

Python
getActiveLinkNames() -> list[str]

getStaticLinkNames

Python
getStaticLinkNames() -> list[str]

getRootLinkName

Python
getRootLinkName() -> str

getCurrentJointValues

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

getCurrentJointValuesByNames

Python
getCurrentJointValuesByNames(joint_names: Sequence[str]) -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order=C)]

getLinkTransform

Python
getLinkTransform(link_name: str) -> tesseract_robotics.tesseract_common._tesseract_common.Isometry3d

getRelativeLinkTransform

Python
getRelativeLinkTransform(from_link_name: str, to_link_name: str) -> tesseract_robotics.tesseract_common._tesseract_common.Isometry3d
Python
getLink(name: str) -> tesseract_robotics.tesseract_scene_graph._tesseract_scene_graph.Link

getJoint

Python
getJoint(name: str) -> tesseract_robotics.tesseract_scene_graph._tesseract_scene_graph.Joint

getGroupNames

Python
getGroupNames() -> set[str]

getGroupJointNames

Python
getGroupJointNames(group_name: str) -> list[str]

getJointGroup

Python
getJointGroup(group_name: str) -> tesseract_robotics.tesseract_kinematics._tesseract_kinematics.JointGroup

getKinematicGroup

Python
getKinematicGroup(group_name: str, ik_solver_name: str = '') -> tesseract_robotics.tesseract_kinematics._tesseract_kinematics.KinematicGroup

findTCPOffset

Python
findTCPOffset(manip_info: ManipulatorInfo) -> tesseract_robotics.tesseract_common._tesseract_common.Isometry3d

getDiscreteContactManager

Python
getDiscreteContactManager() -> 'tesseract_collision::DiscreteContactManager'

getContinuousContactManager

Python
getContinuousContactManager() -> 'tesseract_collision::ContinuousContactManager'

setActiveDiscreteContactManager

Python
setActiveDiscreteContactManager(name: str) -> bool

setActiveContinuousContactManager

Python
setActiveContinuousContactManager(name: str) -> bool

getAllowedCollisionMatrix

Python
getAllowedCollisionMatrix() -> tesseract_robotics.tesseract_common._tesseract_common.AllowedCollisionMatrix

getCollisionMarginData

Python
getCollisionMarginData() -> tesseract_robotics.tesseract_common._tesseract_common.CollisionMarginData

setResourceLocator

Python
setResourceLocator(locator: ResourceLocator) -> None

getResourceLocator

Python
getResourceLocator() -> tesseract_robotics.tesseract_common._tesseract_common.ResourceLocator

getKinematicsInformation

Python
getKinematicsInformation() -> tesseract_robotics.tesseract_srdf._tesseract_srdf.KinematicsInformation

cast_CommandAppliedEvent

Python
cast_CommandAppliedEvent(arg: Event) -> CommandAppliedEvent

Cast Event to CommandAppliedEvent

cast_SceneStateChangedEvent

Python
cast_SceneStateChangedEvent(arg: Event) -> SceneStateChangedEvent

Cast Event to SceneStateChangedEvent