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.

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
from tesseract_robotics.tesseract_geometry import Mesh

# From file (STL, OBJ, DAE)
mesh = Mesh.fromFile("/path/to/model.stl")

# With scale
mesh = Mesh.fromFile("/path/to/model.stl", scale=[0.001, 0.001, 0.001])  # mm to m

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