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.position}")  # [x, y, z] in meters
    print(f"Rotation (quat): {tool0_pose.quaternion}")  # [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

The Python API uses scalar-last quaternions [x, y, z, w] to match scipy. C++ Eigen uses scalar-first [w, x, y, z] — reorder when porting C++ examples.

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

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