Basic Examples¶
Fundamental examples demonstrating kinematics, collision detection, and scene graph operations.
Kinematics Example¶
Forward and inverse kinematics with the ABB IRB2400:
tesseract_kinematics_example.py
"""
Kinematics Example
==================
Demonstrates forward and inverse kinematics using the high-level Robot API.
C++ Reference:
tesseract_kinematics/examples/kinematics_example.cpp
Overview
--------
This example shows the kinematics pipeline:
1. Load robot with kinematic solver (KDL by default)
2. Compute forward kinematics (FK): joint angles -> end effector pose
3. Compute inverse kinematics (IK): target pose -> joint angles
Key Concepts
------------
**Forward Kinematics (FK)**:
Given joint angles, compute the pose of any link in the kinematic chain.
FK is always unique - one set of joint angles produces exactly one pose.
Used for: visualization, collision checking, trajectory validation.
**Inverse Kinematics (IK)**:
Given a target pose, find joint angles that achieve that pose.
IK may have multiple solutions (redundant robots) or no solution (unreachable).
Used for: motion planning, task-space control, pose targeting.
**Kinematic Groups**:
SRDF defines named groups (e.g., "manipulator") specifying which joints
belong together as a kinematic chain. The group also defines the base
and tip links for the chain.
**Tip Link vs TCP Frame**:
- tip_link: The end link of the kinematic chain (e.g., "tool0")
- tcp_frame: Tool Center Point offset from tip_link (for attached tools)
IK solves for the tip_link pose, then applies TCP offset if specified.
**Seed State**:
IK solvers are iterative and require an initial guess (seed). Different
seeds may find different solutions. For redundant robots, nearby seeds
tend to find nearby solutions.
**Quaternion Convention**:
Python/Tesseract uses scalar-last quaternions: [x, y, z, w]
This differs from C++ Eigen which uses scalar-first: [w, x, y, z]
Related Examples
----------------
- basic_cartesian_example.py: Uses IK internally for Cartesian planning
- freespace_ompl_example.py: Uses FK for state validation
- pick_and_place_example.py: FK/IK for grasp pose computation
"""
import numpy as np
from tesseract_robotics.planning import Robot, Pose
def main():
# =========================================================================
# 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}")
if __name__ == "__main__":
main()
Collision Detection Example¶
Check for collisions in the environment:
tesseract_collision_example.py
"""
Collision Checking Example
==========================
Demonstrates collision detection using Tesseract's discrete contact manager.
C++ Reference:
tesseract_collision/examples/bullet_discrete_simple_example.cpp
Overview
--------
This example shows the collision checking workflow:
1. Load robot and add obstacle geometry to the environment
2. Get DiscreteContactManager from environment
3. Sweep through robot configurations checking for collisions
4. Interpret contact results (distances, link pairs)
Key Concepts
------------
**Contact Managers**:
- DiscreteContactManager: Check collision at single poses (this example)
- ContinuousContactManager: Check collision along trajectory (motion planning)
**Collision Margin**:
The margin expands collision geometry by specified distance. A margin of 0.1m
means objects report contact when within 10cm of each other. This provides:
- Safety buffer for planning
- Gradient information for optimizers (TrajOpt)
- Detection before actual collision
**Contact Result Fields**:
- distance: Signed penetration depth (negative = overlap, positive = separation)
- link_names: Pair of colliding link names [link_a, link_b]
- nearest_points: Closest points on each geometry
- normal: Contact normal vector
Pipeline
--------
1. Environment Setup:
- Load URDF/SRDF defining robot links and collision geometry
- Add obstacle objects with collision components
2. Contact Manager Configuration:
- setActiveCollisionObjects(): Which links to check
- setCollisionMarginData(): Safety buffer distance
3. Query Loop:
- Update robot joint state
- setCollisionObjectsTransform(): Sync manager with new link poses
- contactTest(): Run collision query
- Iterate ContactResultMap for collision pairs
Related Examples
----------------
- geometry_showcase_example.py: Geometry types for collision
- freespace_ompl_example.py: Collision-free motion planning
- basic_cartesian_example.py: TrajOpt uses collision gradients
"""
import numpy as np
from tesseract_robotics.planning import Robot, Pose, sphere, create_fixed_joint
from tesseract_robotics.tesseract_scene_graph import Link, Visual, Collision
from tesseract_robotics.tesseract_collision import (
ContactResultMap,
ContactTestType_ALL,
ContactRequest,
ContactResultVector,
)
from tesseract_robotics.tesseract_common import CollisionMarginData
def main():
# =========================================================================
# STEP 1: Environment Setup
# =========================================================================
# Load ABB IRB2400 robot - a common 6-axis industrial manipulator
robot = Robot.from_tesseract_support("abb_irb2400")
# Create an obstacle sphere in the robot's workspace
# This sphere will be positioned where the robot may collide during motion
sphere_link = Link("sphere_link")
sphere_geom = sphere(0.1) # 10cm radius sphere
# Visual component for rendering (optional but helpful for debugging)
visual = Visual()
visual.geometry = sphere_geom
sphere_link.addVisual(visual)
# Collision component - this is what the contact manager actually checks
# Without this, the sphere would be visible but not cause collisions
collision = Collision()
collision.geometry = sphere_geom
sphere_link.addCollision(collision)
# Attach sphere to environment at fixed position
# Position chosen to be within robot reach for collision demonstration
sphere_joint = create_fixed_joint(
name="sphere_joint",
parent_link="base_link",
child_link="sphere_link",
origin=Pose.from_xyz(0.7, 0, 1.5), # 70cm forward, 1.5m high
)
robot.add_link(sphere_link, sphere_joint)
# =========================================================================
# STEP 2: Contact Manager Configuration
# =========================================================================
# DiscreteContactManager checks collision at instantaneous poses
# (vs ContinuousContactManager which checks along motion paths)
manager = robot.env.getDiscreteContactManager()
# Specify which links to include in collision checking
# getActiveLinkNames() returns all links with collision geometry
manager.setActiveCollisionObjects(robot.env.getActiveLinkNames())
# Collision margin: report contacts when objects are within 10cm
# Larger margin = earlier detection but more false positives
# TrajOpt uses margin for gradient-based optimization
manager.setCollisionMarginData(CollisionMarginData(0.1))
# =========================================================================
# STEP 3: Collision Query Loop
# =========================================================================
# ABB IRB2400 has 6 revolute joints named joint_1 through joint_6
joint_names = [f"joint_{i + 1}" for i in range(6)]
joint_pos = np.zeros(6)
# Sweep joint_1 (base rotation) to move arm toward/away from obstacle
for i in range(-5, 5):
joint_pos[0] = i * np.deg2rad(5) # -25 to +20 degrees
print(f"Contact check at robot position: {joint_pos}")
# Update environment state with new joint positions
# This recomputes all link transforms through forward kinematics
robot.set_joints(joint_pos, joint_names=joint_names)
scene_state = robot.env.getState()
# CRITICAL: Sync contact manager with updated link transforms
# The manager caches transforms for performance - must update manually
manager.setCollisionObjectsTransform(scene_state.link_transforms)
# Debug: show current poses of relevant links
print(f"Link 6 Pose:\n{scene_state.link_transforms['link_6'].matrix()}")
print(
f"Sphere Link Pose:\n{scene_state.link_transforms['sphere_link'].matrix()}"
)
# Execute collision query
# ContactTestType_ALL finds all collision pairs (vs FIRST for early-out)
contact_result_map = ContactResultMap()
manager.contactTest(contact_result_map, ContactRequest(ContactTestType_ALL))
# Flatten nested map structure into simple vector for iteration
result_vector = ContactResultVector()
contact_result_map.flattenMoveResults(result_vector)
# Interpret results
print(f"Found {len(result_vector)} contact results")
for j in range(len(result_vector)):
contact_result = result_vector[j]
print(f"Contact {j}:")
# distance < 0: penetration, distance > 0: within margin but separated
print(f"\tDistance: {contact_result.distance}")
print(f"\tLink A: {contact_result.link_names[0]}")
print(f"\tLink B: {contact_result.link_names[1]}")
print()
if __name__ == "__main__":
main()
Scene Graph Example¶
Manipulate the scene graph (add/remove objects):
scene_graph_example.py
from tesseract_robotics.planning import Robot
from tesseract_robotics.tesseract_geometry import Box, Sphere
from tesseract_robotics.tesseract_scene_graph import (
Link, Joint, JointType, Visual, Collision, Material
)
from tesseract_robotics.tesseract_environment import (
AddLinkCommand, RemoveLinkCommand
)
from tesseract_robotics.tesseract_common import Isometry3d
import numpy as np
robot = Robot.from_tesseract_support("abb_irb2400")
env = robot.env
scene = env.getSceneGraph()
# Print existing links
print("Existing links:")
for link in scene.getLinks():
print(f" {link.getName()}")
# Create obstacle link
obstacle_link = Link("obstacle_box")
# Add visual
visual = Visual()
visual.geometry = Box(0.3, 0.3, 0.3) # 30cm cube
visual.origin = Isometry3d.Identity()
material = Material("red")
material.color = np.array([0.8, 0.2, 0.2, 1.0])
visual.material = material
obstacle_link.addVisual(visual)
# Add collision
collision = Collision()
collision.geometry = Box(0.3, 0.3, 0.3)
collision.origin = Isometry3d.Identity()
obstacle_link.addCollision(collision)
# Create joint (fixed to world)
obstacle_joint = Joint("obstacle_joint")
obstacle_joint.type = JointType.FIXED
obstacle_joint.parent_link_name = "base_link"
obstacle_joint.child_link_name = "obstacle_box"
obstacle_joint.parent_to_joint_origin_transform = Isometry3d.Identity()
obstacle_joint.parent_to_joint_origin_transform.translate([0.8, 0.0, 0.3])
# Add to environment
cmd = AddLinkCommand(obstacle_link, obstacle_joint)
env.applyCommand(cmd)
print("\nAdded obstacle_box to scene")
# Verify obstacle affects collision
joints = np.array([0.5, 0, 0, 0, 0, 0]) # Reach toward obstacle
is_safe = robot.check_collision(joints)
print(f"Collision with obstacle: {not is_safe}")
# Remove obstacle
env.applyCommand(RemoveLinkCommand("obstacle_box"))
print("Removed obstacle_box from scene")
# Verify no collision after removal
is_safe = robot.check_collision(joints)
print(f"Collision after removal: {not is_safe}")
Geometry Showcase¶
All available geometry types:
geometry_showcase_example.py
from tesseract_robotics.tesseract_geometry import (
Box, Sphere, Cylinder, Capsule, Cone,
Mesh, ConvexMesh, Plane, Octree
)
from tesseract_robotics.tesseract_common import VectorVector3d
import numpy as np
# Primitive shapes
box = Box(0.5, 0.3, 0.2) # length, width, height
sphere = Sphere(0.15) # radius
cylinder = Cylinder(0.1, 0.4) # radius, length
capsule = Capsule(0.1, 0.3) # radius, length
cone = Cone(0.15, 0.3) # radius, length
print("Primitive shapes:")
print(f" Box: {box.getX()} x {box.getY()} x {box.getZ()}")
print(f" Sphere radius: {sphere.getRadius()}")
print(f" Cylinder: r={cylinder.getRadius()}, l={cylinder.getLength()}")
print(f" Capsule: r={capsule.getRadius()}, l={capsule.getLength()}")
print(f" Cone: r={cone.getRadius()}, l={cone.getLength()}")
# Mesh from vertices (simple tetrahedron)
vertices = VectorVector3d()
vertices.append(np.array([0, 0, 0]))
vertices.append(np.array([1, 0, 0]))
vertices.append(np.array([0.5, 1, 0]))
vertices.append(np.array([0.5, 0.5, 1]))
triangles = np.array([
0, 1, 2, # Base
0, 1, 3, # Side 1
1, 2, 3, # Side 2
0, 2, 3 # Side 3
], dtype=np.int32)
mesh = Mesh(vertices, triangles)
print(f"\nMesh: {mesh.getVertexCount()} vertices, {mesh.getTriangleCount()} triangles")
# Plane (infinite, used for ground)
plane = Plane(0, 0, 1, 0) # Normal (0,0,1), distance 0
print(f"Plane normal: ({plane.getA()}, {plane.getB()}, {plane.getC()})")
Loading Mesh Files
Transforms and Poses¶
Working with Isometry3d transforms:
transforms_example.py
from tesseract_robotics.tesseract_common import Isometry3d
import numpy as np
# Identity transform
pose = Isometry3d.Identity()
# Translation
pose.translate([0.5, 0.2, 0.3])
print(f"Position: {pose.translation()}")
# Rotation from axis-angle
from scipy.spatial.transform import Rotation
R = Rotation.from_euler('z', 45, degrees=True)
pose.rotate(R.as_matrix())
# Get components
position = pose.translation() # np.array([x, y, z])
rotation = pose.rotation() # 3x3 rotation matrix
matrix = pose.matrix() # 4x4 homogeneous matrix
# Compose transforms
pose2 = Isometry3d.Identity()
pose2.translate([0.1, 0, 0])
combined = pose * pose2 # pose2 in pose's frame
# Inverse
inverse = pose.inverse()
# From 4x4 matrix
matrix = np.eye(4)
matrix[:3, 3] = [1, 2, 3] # Translation
pose_from_matrix = Isometry3d(matrix)
State Management¶
Working with environment state:
state_example.py
from tesseract_robotics.planning import Robot
import numpy as np
robot = Robot.from_tesseract_support("abb_irb2400")
env = robot.env
# Get current state
state = env.getState()
print(f"Joint names: {state.joint_names}")
print(f"Positions: {state.position}")
# Set state (dict interface)
env.setState({
"joint_1": 0.5,
"joint_2": -0.3,
"joint_3": 0.2
})
# Set state (array interface)
joint_names = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"]
joint_values = np.array([0.5, -0.3, 0.2, 0.0, 0.5, 0.0])
env.setState(joint_names, joint_values)
# Get link transform in current state
tcp = env.getLinkTransform("tool0")
print(f"TCP position: {tcp.translation()}")
Next Steps¶
- Planning Examples - Motion planning
- Online Planning - Real-time replanning