Skip to content

tesseract_robotics

Environment Examples

tesseract-robotics/tesseract_nanobind

Environment Examples¶

The Environment below the high-level Robot API: how it is built from commands, what state it keeps, and the plugin information it loads. Each example takes its scenario and parameters from upstream source, cited in the module docstring. Every value printed is also asserted in tests/examples.

Bash

# Installed console scripts
tesseract_environment_commands_example
tesseract_environment_state_example
tesseract_plugin_configuration_example

# Or via Python module invocation
pixi run python -m tesseract_robotics.examples.environment_commands_example

Environment Commands¶

Every change to an Environment is a command, and getCommandHistory() returns them in order. The example builds ABB IRB2400 environments from files, from an SRDF with a calibration, and from another environment's history.

Resource locators¶

GeneralResourceLocator resolves package://name/... through a directory named name that holds a package.xml. The default constructor reads the TESSERACT_RESOURCE_PATH, ROS_PACKAGE_PATH and AMENT_PREFIX_PATH variables; paths and environment_variables (keyword-only) choose the sources explicitly. A locator built with no variables resolves nothing until loadEnvironmentVariable adds one, which returns False for an unset variable. addPath registers the packages under a directory: here a temporary package holding the calibration file used below.

environment_commands_example.py

    # Default: the TESSERACT_RESOURCE_PATH, ROS_PACKAGE_PATH and AMENT_PREFIX_PATH variables
    locator = GeneralResourceLocator()
    urdf_path, srdf_path = locate(locator, URDF_URL), locate(locator, SRDF_URL)
    # The directory that holds the `tesseract` package, found through the package itself
    share = locate(locator, "package://tesseract/package.xml").parent.parent

    by_paths = GeneralResourceLocator(paths=[share], environment_variables=[])
    by_variable = GeneralResourceLocator(environment_variables=["TESSERACT_RESOURCE_PATH"])
    late = GeneralResourceLocator(environment_variables=[])
    unresolved_before_load = late.locateResource(URDF_URL) is None
    load_set_variable = late.loadEnvironmentVariable("TESSERACT_RESOURCE_PATH")
    load_unset_variable = late.loadEnvironmentVariable(UNSET_VARIABLE)

    write_fixture_package(fixture_root)
    add_path = locator.addPath(fixture_root)

The init history¶

init(urdf, srdf, locator) runs upstream's getInitCommands (environment.cpp:95–137): AddSceneGraph, then, when there is an SRDF, AddContactManagersPluginInfo, AddKinematicsInformation, one ChangeJointOrigin per calibrated joint and ChangeCollisionMargins if the SRDF sets margins. abb_irb2400.srdf sets neither, so its history is three commands; without an SRDF it is one.

environment_commands_example.py

    env = Environment()
    env.init(urdf_path, srdf_path, locator)  # a Path is a file; a str would be content
    init_history = history_types(env.getCommandHistory())

    urdf_only = Environment()
    urdf_only.init(urdf_path, locator)
    urdf_only_history = history_types(urdf_only.getCommandHistory())

`init` call	`getCommandHistory()` by `CommandType`
`init(Path(urdf), Path(srdf), locator)`	`ADD_SCENE_GRAPH`, `ADD_CONTACT_MANAGERS_PLUGIN_INFO`, `ADD_KINEMATICS_INFORMATION`
`init(Path(urdf), locator)`	`ADD_SCENE_GRAPH`
`init(scene_graph, srdf)`, SRDF with calibration	the first three, then `CHANGE_JOINT_ORIGIN` ×2

Calibration¶

The SRDF's calibration_config element names a YAML file of joint origins. The example adds one to the shipped abb_irb2400.srdf, pointing at upstream's unit-test calibration (tesseract_srdf_unit.cpp:672–690): joint_1 at (1, 2, 3) and joint_2 at (4, 5, 6), both unrotated. It is a fixture for the mechanism, not a calibration of a real IRB2400. Each calibrated joint becomes a ChangeJointOrigin command, and the environment's joint origins are exactly the calibrated ones. The calibration is a hash map, so the order of those commands is unspecified.

environment_commands_example.py

    srdf_xml = srdf_path.read_text().replace("</robot>", CALIBRATION_ELEMENT + "</robot>")
    scene_graph = parseURDFFile(str(urdf_path), locator)
    srdf = SRDFModel()
    srdf.initString(scene_graph, srdf_xml, locator)
    joints = srdf.calibration_info.joints

    calibrated = Environment()
    calibrated.init(scene_graph, srdf)
    calibrated_history = calibrated.getCommandHistory()

CalibrationInfo.insert adds the other's joints and overwrites the ones both have:

environment_commands_example.py

    copy = CalibrationInfo()
    empty_before_insert = copy.empty()
    copy.insert(srdf.calibration_info)
    insert_copies = copy == srdf.calibration_info
    # insert overwrites an existing joint: joint_2 takes joint_1's origin
    override = CalibrationInfo()
    override.joints = {"joint_2": joints["joint_1"]}
    copy.insert(override)
    insert_overrides = {name: tf.translation.tolist() for name, tf in sorted(copy.joints.items())}
    copy.clear()

The same SRDF's contact_managers_plugin_info is read from contact_manager_plugins.yaml: the Bullet and FCL factory libraries, three discrete managers and two continuous ones, and it equals the environment's getContactManagersPluginInfo().

Applying commands¶

A URDF-only environment has no contact-manager plugin information, so it has no discrete manager (getDiscreteContactManager() returns None). AddContactManagersPluginInfoCommand adds the information, after which the default manager is BulletDiscreteBVHManager. applyCommands then applies the calibration as one batch. An event callback receives one CommandAppliedEvent per applyCommand or applyCommands call; its commands is the whole history, and its revision the history's length.

environment_commands_example.py

    events = []

    def on_event(event):
        # copy what is needed: the event is valid only inside the callback
        if event.type == Events.COMMAND_APPLIED:
            events.append((event.revision, history_types(event.commands)))

    urdf_only.addEventCallback(1, EventCallbackFn(on_event))

    # A URDF-only environment has no contact-manager plugins until the command adds them
    plugin_info_empty_before = urdf_only.getContactManagersPluginInfo().empty()
    manager_before = urdf_only.getDiscreteContactManager()
    command = AddContactManagersPluginInfoCommand(srdf.contact_managers_plugin_info)
    plugin_info_applied = urdf_only.applyCommand(command)
    manager_after = urdf_only.getDiscreteContactManager().getName()

    # The calibration as one batch: one event for both commands
    batch = [ChangeJointOriginCommand(name, origin) for name, origin in joints.items()]
    batch_applied = urdf_only.applyCommands(batch)

call	event `revision`	event `commands`
`applyCommand(AddContactManagersPluginInfoCommand(...))`	2	`ADD_SCENE_GRAPH`, `ADD_CONTACT_MANAGERS_PLUGIN_INFO`
`applyCommands([ChangeJointOriginCommand, ChangeJointOriginCommand])`	4	the above, then `CHANGE_JOINT_ORIGIN` ×2

Replay, clone and equality¶

init(commands) rebuilds an environment from another one's history. The histories are equal command by command, and so are the revisions and the state, but the two environments are not equal: Environment == also compares the time of the last change and of the last state change (environment.cpp:393–413), and the replay was built later. clone() copies both timestamps (environment.cpp:424–444), so a clone is equal to its original until a command is applied to it.

environment_commands_example.py

    replay = Environment()
    initialized = replay.init(calibrated.getCommandHistory())
    histories_equal = all(
        a == b for a, b in zip(calibrated.getCommandHistory(), replay.getCommandHistory())
    ) and len(replay.getCommandHistory()) == len(calibrated_history)
    # Everything but the timestamps matches, so the timestamps decide equality
    timestamps_differ = (
        replay.getTimestamp() != calibrated.getTimestamp()
        and replay.getCurrentStateTimestamp() != calibrated.getCurrentStateTimestamp()
    )
    replay_equals_env = replay == calibrated

    clone = calibrated.clone()
    clone_equals_env = clone == calibrated
    clone.applyCommand(ChangeLinkVisibilityCommand("link_1", False))
    clone_after_command_equals_env = clone == calibrated

Comparing states

state_a == state_b is upstream's SceneState == (scene_state.cpp:53–67, bound in #232): exact joint values, transforms within isApprox(1e-5). The example spells the comparison out to show what it covers.

Bytes resources¶

A BytesResource is a resource held in memory, built from bytes or from a list of byte values; both constructors give equal resources. Its optional parent locator resolves a relative url next to the resource's own url, here package.xml beside the calibration file. Without a parent nothing resolves, and BytesResource == counts whether a parent is set, so the two are unequal.

environment_commands_example.py

    data = CALIBRATION_YAML.encode()
    from_bytes = BytesResource(FIXTURE_URL, data, locator)
    from_list = BytesResource(FIXTURE_URL, list(data), locator)
    # The parent resolves a relative url next to the resource's own url
    sibling = from_bytes.locateResource("package.xml")
    without_parent = BytesResource(FIXTURE_URL, data)

Environment State¶

The IIWA environment's state, with its first joint made FLOATING: every native setState / getState / getLinkTransforms overload, the joint and link queries, timestamps, and TCP lookup.

Floating joints¶

A ReplaceJointCommand turns joint_a1 into a FLOATING joint at x = 1.25, the first step of upstream's floating-joint test (state_solver_test_suite.h:766–820). A floating joint has no joint value, so it leaves getActiveJointNames(); its pose is read with getCurrentFloatingJointValues(), for all floating joints or for the names given. The command moves the revision from 3 to 4; the init revision stays 3.

environment_state_example.py

    joint = Joint(FLOATING_JOINT)
    joint.parent_to_joint_origin_transform = translation(ORIGIN_X)
    joint.parent_link_name = "base_link"
    joint.child_link_name = "link_1"
    joint.type = JointType.FLOATING
    env.applyCommand(ReplaceJointCommand(joint))

    names = env.getActiveJointNames()  # joint_a1 is no longer among them
    floating = env.getCurrentFloatingJointValues()
    floating_by_name = env.getCurrentFloatingJointValues([FLOATING_JOINT])

Each setState overload then moves the floating joint to the next origin of upstream's test (y = 1.5, then z = 1.5, then back to the first), starting from zero joint values and the previous origin, so a call that changed nothing would be caught. After each one, the joint values and the floating value read back, and link_1 sits at base_link ∘ origin: a floating joint adds no motion of its own.

environment_state_example.py

    origins = [
        translation(ORIGIN_X, ORIGIN_Y),
        translation(ORIGIN_X, ORIGIN_Y, ORIGIN_Z),
        translation(ORIGIN_X),
    ]
    joints = dict(zip(names, JOINT_VALUES))
    zeros = np.zeros(len(names))
    set_state = {
        "setState(floating)": lambda origin: env.setState({FLOATING_JOINT: origin}),
        "setState(dict, floating)": lambda origin: env.setState(joints, {FLOATING_JOINT: origin}),
        "setState(names, values, floating)": lambda origin: env.setState(
            names, JOINT_VALUES, {FLOATING_JOINT: origin}
        ),
    }
    steps = []
    previous = translation(ORIGIN_X)
    for (overload, set_with), origin in zip(set_state.items(), origins):
        # start from other joint values and the previous origin, so a no-op call is caught
        env.setState(names, zeros, {FLOATING_JOINT: previous})
        set_with(origin)
        # the floating-only overload leaves the joint values as they were
        expected = zeros if overload == "setState(floating)" else JOINT_VALUES
        steps.append(
            {
                "overload": overload,
                "origin": origin.translation.tolist(),
                "joints_read_back": np.array_equal(env.getCurrentJointValues(), expected),
                "floating_read_back": np.array_equal(
                    env.getCurrentFloatingJointValues()[FLOATING_JOINT].matrix, origin.matrix
                ),
                # a FLOATING joint has no motion: link_1 = base_link ∘ origin
                "link_1_at_origin": is_approx(
                    env.getLinkTransform("link_1"), env.getLinkTransform("base_link") * origin
                ),
            }
        )
        previous = origin

overload	`joint_a1` origin	joint values afterwards
`setState(floating)`	(1.25, 1.5, 0)	unchanged (zero)
`setState(dict, floating)`	(1.25, 1.5, 1.5)	upstream's `jvals`
`setState(names, values, floating)`	(1.25, 0, 0)	upstream's `jvals`

Computing a state without setting it¶

getState with arguments computes a state for other joint values and floating values and leaves the environment's own state as it was. getLinkTransforms() lists the current state in getLinkNames() order; with joint names and values (and optionally floating values) it computes, like getState. Each agrees with getState within upstream's precision of 1e-6 (tesseract_environment_unit.cpp:155–158).

environment_state_example.py

    # The current state is JOINT_VALUES at origin x = 1.25. Each getState computes
    # another one, zero joint values and/or another origin, without setting it.
    other = {FLOATING_JOINT: translation(ORIGIN_X, ORIGIN_Y, ORIGIN_Z)}
    current = env.getState().link_transforms
    computed = {
        "getState(floating)": env.getState(other),  # the joints keep their current values
        "getState(dict, floating)": env.getState(dict(zip(names, zeros)), other),
        "getState(names, values, floating)": env.getState(names, zeros, other),
    }

environment_state_example.py

    link_transforms = {
        "getLinkTransforms()": dict(zip(env.getLinkNames(), env.getLinkTransforms())),
        "getLinkTransforms(names, values)": env.getLinkTransforms(names, zeros),
        "getLinkTransforms(names, values, floating)": env.getLinkTransforms(names, zeros, other),
    }

Queries¶

getCurrentJointValues(names) reads joint values in the order given. getActiveLinkNames(joints) lists the links those joints move, and getStaticLinkNames(joints) the rest. getJointGroup(name, joints) builds a JointGroup over any joints, whose forward kinematics match the environment's. getJointLimits returns the URDF limits; the URDF parser sets an unspecified acceleration to half the velocity and an unspecified jerk to 1000 (limits.cpp:59, 65). getLinkVisibility and getLinkCollisionEnabled follow their commands.

environment_state_example.py

    current_joint_values = env.getCurrentJointValues(["joint_a3", "joint_a2"])
    active_links = env.getActiveLinkNames(["joint_a5"])  # the links joint_a5 moves
    static_links = env.getStaticLinkNames(["joint_a5"])
    group = env.getJointGroup("active_joints", names)
    limits = env.getJointLimits("joint_a2")

    flags_before = (env.getLinkVisibility("link_1"), env.getLinkCollisionEnabled("link_1"))
    env.applyCommands(
        [
            ChangeLinkVisibilityCommand("link_1", False),
            ChangeLinkCollisionEnabledCommand("link_1", False),
        ]
    )
    flags_after = (env.getLinkVisibility("link_1"), env.getLinkCollisionEnabled("link_1"))

query	result
`getCurrentJointValues(["joint_a3", "joint_a2"])`	[−0.785398, 0.785398]
`getActiveLinkNames(["joint_a5"])`	`link_5`, `link_6`, `link_7`, `tool0`
`getStaticLinkNames(["joint_a5"])`	`base`, `base_link`, `link_1` … `link_4`
`getJointLimits("joint_a2")`	lower −2.0942, upper 2.0942, velocity 1.4834, acceleration 0.7417, jerk 1000

Timestamps

setState sets getTimestamp() and getCurrentStateTimestamp() from one system_clock::now() (environment.cpp:651–654), so the two are equal right after it; a command sets only getTimestamp(). system_clock can step backwards, so compare timestamps for equality, not order.

Tool center points¶

findTCPOffset(manip_info) resolves manip_info.tcp_offset (environment.cpp:832–862), as upstream's EnvFindTCPUnit test runs it:

an Isometry3d is returned as is;
a name must not be a link (tool0 raises RuntimeError);
a name is looked up in the manipulator's group_tcps in the SRDF (laser, welder);
otherwise each callback registered with addFindTCPOffsetCallback is asked in turn, and one that raises means "not found here";
if none finds it, RuntimeError.

environment_state_example.py

    explicit = env.findTCPOffset(
        ManipulatorInfo("manipulator", "unknown", "unknown", translation(z=TCP_Z))
    )
    laser = env.findTCPOffset(ManipulatorInfo("manipulator", "base_link", "tool0", "laser"))
    welder = env.findTCPOffset(ManipulatorInfo("manipulator", "base_link", "tool0", "welder"))

    by_callback = ManipulatorInfo("manipulator", "unknown", "unknown", "laser_callback")
    unresolved_before_callback = find_tcp_error(env, by_callback)
    env.addFindTCPOffsetCallback(laser_callback)
    callback = env.findTCPOffset(by_callback)

`tcp_offset`	result
`Isometry3d`, z = 0.1	that transform
`"laser"` (`xyz="1 .1 1" rpy="0 1.57 0"`)	translation (1, 0.1, 1), rotation 1.57 rad about y
`"welder"` (`xyz=".1 1 .2" wxyz="1 0 0 0"`)	translation (0.1, 1, 0.2), no rotation
`"laser_callback"`, before the callback	`RuntimeError: Could not find tcp by name laser_callback'!`
`"laser_callback"`, after the callback	z = 0.1
`"tool0"` (a link)	`RuntimeError: The tcp offset name 'tool0' should not be an existing link …`
`"unknown"`	`RuntimeError: Could not find tcp by name unknown'!`

Manipulator info¶

A ManipulatorInfo is empty while its manipulator, working frame or TCP frame is unset. getCombined(override) takes every field the override sets, and its TCP offset with its TCP frame, as upstream's ManipulatorInfo unit test shows:

environment_state_example.py

    manip_info = ManipulatorInfo()
    default_empty = manip_info.empty()
    override = ManipulatorInfo("manipulator", "world", "tool0", translation(z=OVERRIDE_TCP_Z))
    override.manipulator_ik_solver = "OPWInvKin"
    override.working_frame = "base_link"
    combined = manip_info.getCombined(override)

Plugin Configuration¶

The plugin information tesseract reads from YAML, built in Python: contact managers, profiles and the task composer. Each info type has search_paths, search_libraries and its plugins; insert merges one into another, empty and clear do what they say.

insert merges, it does not replace

insert(other) appends other's search paths and libraries, adds or overwrites its plugins by name, and takes its default plugin only when other names one (plugin_info.cpp:71–81, 162–184, 214–230). Inserting the same information twice lists its libraries twice.

Contact managers¶

The example builds contact_manager_plugins.yaml as a Bullet part, with the defaults, and an FCL part without one, and merges them. The result equals the information the IIWA SRDF loads from the shipped file. (Wheels strip search_paths from the shipped support YAMLs, so the example compares the other fields.)

plugin_configuration_example.py

    bullet = ContactManagersPluginInfo()
    bullet.search_paths = [SEARCH_PATH]
    bullet.search_libraries = [BULLET_LIBRARY]
    bullet.discrete_plugin_infos = container(
        {
            "BulletDiscreteBVHManager": plugin("BulletDiscreteBVHManagerFactory"),
            "BulletDiscreteSimpleManager": plugin("BulletDiscreteSimpleManagerFactory"),
        },
        default="BulletDiscreteBVHManager",
    )
    bullet.continuous_plugin_infos = container(
        {
            "BulletCastBVHManager": plugin("BulletCastBVHManagerFactory"),
            "BulletCastSimpleManager": plugin("BulletCastSimpleManagerFactory"),
        },
        default="BulletCastBVHManager",
    )
    # No default: insert keeps the Bullet one
    fcl = ContactManagersPluginInfo()
    fcl.search_libraries = [FCL_LIBRARY]
    fcl.discrete_plugin_infos = container(
        {"FCLDiscreteBVHManager": plugin("FCLDiscreteBVHManagerFactory")}
    )

    merged = ContactManagersPluginInfo()
    new_empty = merged.empty()
    merged.insert(bullet)
    merged.insert(fcl)

Applied to a URDF-only environment with AddContactManagersPluginInfoCommand, it makes every manager it names available, each created by name:

plugin_configuration_example.py

    env = Environment()
    env.init(locate(locator, URDF_URL), locator)  # no SRDF: no contact-manager plugins
    managers_before = {
        "discrete": env.getDiscreteContactManager(),
        "continuous": env.getContinuousContactManager(),
    }
    applied = env.applyCommand(AddContactManagersPluginInfoCommand(merged))
    created = {
        name: env.getDiscreteContactManager(name).getName()
        for name in merged.discrete_plugin_infos.plugins
    }
    created.update(
        {
            name: env.getContinuousContactManager(name).getName()
            for name in merged.continuous_plugin_infos.plugins
        }
    )

Profiles and the task composer¶

There is no shipped profile-plugin YAML; the example uses the values of upstream's ProfilePluginInfoUnit test (tesseract_common_unit.cpp:1268–1298). plugin_infos maps a group to its plugins, and insert merges into a group rather than replacing it:

plugin_configuration_example.py

    fwd = ProfilesPluginInfo()
    fwd.search_paths = [SEARCH_PATH]
    fwd.search_libraries = ["tesseract_collision"]
    fwd.plugin_infos = {"manipulator": {"KDLFwdKin": plugin("Profile")}}
    inv = ProfilesPluginInfo()
    inv.plugin_infos = {"manipulator": {"KDLInvKin": plugin("Profile")}}

    merged = ProfilesPluginInfo()
    new_empty = merged.empty()
    merged.insert(fwd)
    merged.insert(inv)  # merges into "manipulator", does not replace it

TaskComposerPluginInfo mirrors task_composer_plugins.yaml's search libraries, its executor block (TaskflowExecutor, 5 threads) and two of its 36 task entries, each a PipelineTaskFactory:

plugin_configuration_example.py

    tasks = TaskComposerPluginInfo()
    tasks.search_paths = [SEARCH_PATH]
    tasks.search_libraries = TASK_LIBRARIES
    tasks.task_plugin_infos = container(
        {name: plugin("PipelineTaskFactory") for name in MIRRORED_TASKS}
    )
    executors = TaskComposerPluginInfo()
    executors.search_libraries = [TASKFLOW_LIBRARY]
    executors.executor_plugin_infos = container(
        {"TaskflowExecutor": plugin("TaskflowTaskComposerExecutorFactory", EXECUTOR_CONFIG)},
        default="TaskflowExecutor",
    )

    merged = TaskComposerPluginInfo()
    new_empty = merged.empty()
    merged.insert(tasks)
    merged.insert(executors)  # libraries in the YAML's order

Data only, for now

Upstream builds a ProfilePluginFactory from a ProfilesPluginInfo and a TaskComposerPluginFactory from a TaskComposerPluginInfo. Neither constructor is bound, so in Python these two types hold data but nothing consumes them yet. TaskComposerPluginFactory is built from a config file.

Configured and available task composer plugins¶

getTaskComposerNodePlugins() returns the configured nodes, name → PluginInfo: the names createTaskComposerNode accepts. getAvailableTaskComposerNodePlugins() returns the factory classes the search libraries export: the valid values of a PluginInfo.class_name. Every configured class is available, and TaskComposer.get_available_pipelines() lists only configured names, the ones whose node is created without error.

plugin_configuration_example.py

    composer = TaskComposer.from_config()
    factory = composer.factory
    configured = factory.getTaskComposerNodePlugins()  # name -> PluginInfo
    available = factory.getAvailableTaskComposerNodePlugins()  # factory class names
    pipelines = composer.get_available_pipelines()  # configured names that load

	count (macOS arm64, conda env)
configured node plugins	36, all `PipelineTaskFactory`
available node classes	33
loadable pipelines	36

get_available_pipelines() creates every configured pipeline once, so it loads the planner plugins: the first call in a process took 12 s here.