Skip to content

Basic Examples

Fundamental examples demonstrating kinematics, collision detection, and scene graph operations.

All examples install as console scripts. Run them with:

Bash
# Installed console script (preferred)
tesseract_kinematics_example

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

Kinematics Example

Forward and inverse kinematics with the KUKA LBR IIWA 14 R820 (7-DOF). The snippet below is the core of the example — loaded directly from the shipped source.

tesseract_kinematics_example.py
    # =========================================================================
    # STEP 1: Load Robot with Kinematic Solver
    # =========================================================================
    # KUKA LBR IIWA 14 R820 is a 7-DOF collaborative robot
    # 7 joints = redundant (more DOF than needed for 6D pose) -> multiple IK solutions
    # KDL (Kinematics and Dynamics Library) is the default solver
    robot = Robot.from_tesseract_support("lbr_iiwa_14_r820")

    # =========================================================================
    # STEP 2: Forward Kinematics
    # =========================================================================
    # Joint configuration: 7 joint angles in radians
    # This pose bends the elbow (-1.57 rad = -90 deg on joint 4)
    # and rotates the wrist (1.57 rad = 90 deg on joint 6)
    joint_pos = np.array([0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0])

    # robot.fk() computes pose of tip_link given joint angles
    # "manipulator" is the kinematic group defined in SRDF
    # "tool0" is the flange frame at end of arm
    tool0_pose = robot.fk("manipulator", joint_pos, tip_link="tool0")

    print(f"Tool0 transform at joint position {joint_pos}:")
    print(f"Translation: {tool0_pose.translation}")  # [x, y, z] in meters
    print(f"Rotation (quat): {Quaterniond(tool0_pose.linear).coeffs()}")  # [x, y, z, w] scalar-last

    # =========================================================================
    # STEP 3: Inverse Kinematics
    # =========================================================================
    # Define target pose for end effector
    # Pose.from_xyz_quat([x, y, z], [qx, qy, qz, qw]) - note scalar-last quaternion
    # This quaternion represents a rotation about the z-axis
    target_pose = Pose.from_xyz_quat([0.7, -0.1, 1.0], [0.7071, 0, 0.7071, 0])

    # robot.ik() solves for joint angles that achieve target pose
    # seed: initial guess for iterative solver (affects which solution is found)
    # tip_link: end of kinematic chain (must match FK tip_link for consistency)
    ik_solutions = []
    ik_result = robot.ik("manipulator", target_pose, seed=joint_pos, tip_link="tool0")

    # IK returns None if no solution found (pose unreachable or solver failed)
    if ik_result is not None:
        ik_solutions.append(ik_result)

    # Report results
    # For 7-DOF robot, infinite solutions exist for a 6D pose (self-motion)
    # The solver returns the solution closest to the seed
    print(f"\nFound {len(ik_solutions)} solution(s)")
    for i, sol in enumerate(ik_solutions):
        print(f"Solution {i}: {sol}")

Run it:

Bash
tesseract_kinematics_example

Quaternion convention — scalar-last everywhere

Project canonical order is scalar-last [qx, qy, qz, qw]. Build with Quaterniond.from_xyzw(qx, qy, qz, qw); access components via q.x, q.y, q.z, q.w properties. The Eigen-style Quaterniond(w, x, y, z) ctor exists for direct interop but is not the preferred call shape.

Collision Detection Example

Check for collisions in the environment:

Bash
tesseract_collision_example

See the source at tesseract_collision_example.py.

Scene Graph Example

Manipulate the scene graph (add/remove objects):

Bash
tesseract_scene_graph_example

See the source at scene_graph_example.py.

Building a scene graph

The example's second part builds a SceneGraph directly, without an Environment: upstream's five-link test graph (tesseract_scene_graph_unit.cpp:40–93, build_scene_graph_example.cpp), with one joint of each kind and JointLimits on the planar and revolute joints.

scene_graph_example.py
    graph = SceneGraph()
    for i in range(1, 6):
        graph.addLink(Link(f"link_{i}"))
    # tesseract_scene_graph_unit.cpp:57-90, limits (lower, upper, effort, velocity, acceleration, jerk)
    graph.addJoint(make_joint("joint_1", "link_1", "link_2", JointType.FIXED, 0))
    graph.addJoint(
        make_joint(
            "joint_2", "link_2", "link_3", JointType.PLANAR, 0, JointLimits(-1, 1, 0, 2, 3, 6)
        )
    )
    graph.addJoint(make_joint("joint_3", "link_3", "link_4", JointType.FLOATING, 0))
    graph.addJoint(
        make_joint(
            "joint_4", "link_2", "link_5", JointType.REVOLUTE, 1, JointLimits(-1, 1, 0, 2, 3, 4)
        )
    )
    for first, second in [
        ("link_1", "link_2"),
        ("link_2", "link_3"),
        ("link_3", "link_5"),
        ("link_2", "link_5"),
    ]:
        graph.addAllowedCollision(first, second, "Adjacent")

getAdjacencyMap(link_names) maps every link at or below the given links to the nearest of them above it: link_4 to link_3, link_5 to link_2.

scene_graph_example.py
    adjacent = graph.getAdjacentLinkNames("link_3")
    inv_adjacent = graph.getInvAdjacentLinkNames("link_3")
    children = sorted(graph.getLinkChildrenNames("link_2"))
    joint_children = graph.getJointChildrenNames("joint_4")
    # each link of the subtrees below link_2 and link_3, mapped to the nearest of the two above it
    adjacency_map = graph.getAdjacencyMap(["link_2", "link_3"])
    acyclic, tree = graph.isAcyclic(), graph.isTree()

str() prints what upstream's operator<< prints: JointType.REVOLUTE is Revolute, a JointLimits is lower=-1 upper=1 effort=0 velocity=2 acceleration=3 jerk=4, and a ShortestPath lists its links, joints and active joints. The JointType_* module constants are gone; use the enum (breaking changes).

scene_graph_example.py
    joint_types = {member.name: str(member) for member in JointType}
    shortest_path = str(graph.getShortestPath("link_1", "link_4"))
    joint_values = {
        "joint_2.limits": str(graph.getJoint("joint_2").limits),
        "joint_4.limits": str(graph.getJoint("joint_4").limits),
        # tesseract_scene_graph_joint_unit.cpp: the values each test sets
        "dynamics": str(JointDynamics(10, 5)),
        "safety": str(JointSafety(1, 2, 3, 4)),
        "calibration": str(JointCalibration(1, 2, 3)),
        "mimic": str(JointMimic(1, 2, "joint_name")),
    }

setAllowedCollisionMatrix replaces the graph's allowed collision matrix: after it, link_1/link_2 is no longer allowed. The graph keeps the matrix object itself (graph.cpp:746–749), so an edit to acm after the call shows up in isCollisionAllowed.

scene_graph_example.py
    initial_entries = len(graph.getAllowedCollisionMatrix().getAllAllowedCollisions())
    allowed_before = graph.isCollisionAllowed("link_1", "link_2")
    acm = AllowedCollisionMatrix()
    acm.addAllowedCollision("link_3", "link_4", "Adjacent")
    graph.setAllowedCollisionMatrix(acm)
    allowed_after = graph.isCollisionAllowed("link_1", "link_2")
    entries_after = len(graph.getAllowedCollisionMatrix().getAllAllowedCollisions())
    # the graph keeps `acm` itself: an edit after the call is an edit to the graph's matrix
    acm.addAllowedCollision("link_1", "link_5", "Never")

Link.visible and Link.collision_enabled are the flags that SceneGraph.setLinkVisibility / setLinkCollisionEnabled set and getLinkVisibility / getLinkCollisionEnabled read (graph.cpp:417–432). They default to True; set on a Link before addLink, the graph reads them back.

scene_graph_example.py
    link_5 = graph.getLink("link_5")
    defaults = (link_5.visible, link_5.collision_enabled)
    graph.setLinkVisibility("link_5", False)
    graph.setLinkCollisionEnabled("link_5", False)
    link_6 = Link("link_6")
    link_6.visible = False
    link_6.collision_enabled = False
    graph.addLink(link_6)  # no joint: the graph is no longer a tree

Shortest path before the cycle

Upstream's example prints getShortestPath("link_1", "link_4") after joint_5 gives link_4 a second parent. Edges weigh the length of their joint origin (graph.cpp:599), and both paths then weigh 3.75 m, so which one is returned is a tie-break. The example takes the path while the graph is a tree, where it is unique.

Geometry Showcase

All available geometry types (Box, Sphere, Cylinder, Capsule, Cone, Mesh, ConvexMesh, Plane, Octree):

Bash
tesseract_geometry_showcase_example

See the source at geometry_showcase_example.py.

Loading Mesh Files
Python
import numpy as np

from tesseract_robotics.tesseract_geometry import createMeshFromPath

# From file (STL, OBJ, DAE, PLY, glTF): one Mesh per mesh in the file
meshes = createMeshFromPath("/path/to/model.stl")

# With scale
meshes = createMeshFromPath("/path/to/model.stl", np.array([0.001, 0.001, 0.001]))  # mm to m

Loading from bytes, normals and textures, and writing PLY: Geometry Examples.

Point cloud → Octree Example

Loads a binary PLY point cloud, builds a tesseract_geometry.Octree, attaches it as a collision obstacle in front of an ABB IRB2400, and then runs the same MotionProgram through TrajOpt twice — once with collision disabled (robot tints red, drags through the cloud) and once with the default collision-aware profile (robot tints green, routes around).

Bash
tesseract_pointcloud_octree_collision_example
# custom cloud / leaf size
tesseract_pointcloud_octree_collision_example --cloud path/to/scan.ply --resolution 0.01

Toggle in the viewer

The viewer starts on the colliding trajectory; press t + Enter in the terminal to swap between the colliding (red) and collision-aware (green) trajectories. Press Enter alone to exit.

Typical output:

Text Only
[start] 0 contact pair(s)
[goal]  0 contact pair(s)
Running plan_freespace (collision disabled)...
Collision-disabled trajectory: 4/11 waypoints collide with the cloud
Running plan_freespace (collision enabled)...
Collision-aware trajectory: 0/11 waypoints collide

Source: pointcloud_octree_collision_example.py. For the generic "octree as a collision obstacle" recipe, see the user-guide section Point Clouds as Collision Obstacles.

How the comparison is built

run() constructs a single two-waypoint MotionProgram and feeds it to plan_freespace(...) twice with different ProfileDictionary objects — the default create_trajopt_default_profiles() for the collision-aware run, and a copy with collision_cost_config.enabled / collision_constraint_config.enabled flipped off for the colliding run.

Same start/goal/waypoint count, same robot, same env; only the profile differs, which is what makes the result fair to eyeball.

Why a permissive ContactCheckProfile?

TrajOptPipeline runs a DiscreteContactCheckTask after the optimizer; that task aborts the whole pipeline if any waypoint is in contact. Disabling collision in the TrajOpt profile only changes what TrajOpt optimises for — the post-check still fires. The example sidesteps that by registering a ContactCheckProfile under namespace "DiscreteContactCheckTask" with default_margin = -10.0, so only penetrations deeper than 10 m would register and the colliding plan is allowed to finish and return a real CompositeInstruction.

Reeds-Shepp Example

Reeds-Shepp path for a differential-drive vehicle — a 2D motion planning demonstration using OMPL:

Bash
tesseract_reeds_shepp_example

See the source at reeds_shepp_example.py.

Next Steps