tessl/pypi-pybullet

Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning

—

Pending

Overview

Eval results

Files

Collision Detection

Name: tessl/pypi-pybullet
Author: tessl

Advanced collision queries, contact point analysis, and ray intersection testing for sensor simulation, collision avoidance, and spatial reasoning. Essential for robotics applications requiring environmental awareness and safe motion planning.

Capabilities

Contact Point Queries

Get detailed information about contacts between objects in the simulation.

def getContactPoints(bodyA=None, bodyB=None, linkIndexA=-1, linkIndexB=-1, physicsClientId=0):
    """
    Get contact points between objects.
    
    Args:
        bodyA (int, optional): First body unique ID. If None, gets all contacts.
        bodyB (int, optional): Second body unique ID. If None, gets contacts with bodyA.
        linkIndexA (int, optional): Link index on bodyA (-1 for base). Defaults to -1.
        linkIndexB (int, optional): Link index on bodyB (-1 for base). Defaults to -1.
        physicsClientId (int, optional): Physics client ID. Defaults to 0.
    
    Returns:
        list: List of contact point tuples, each containing:
            - contactFlag (int): Contact type flag
            - bodyUniqueIdA (int): First body ID
            - bodyUniqueIdB (int): Second body ID
            - linkIndexA (int): Link index on first body
            - linkIndexB (int): Link index on second body
            - positionOnA (list): Contact position on bodyA [x, y, z]
            - positionOnB (list): Contact position on bodyB [x, y, z]
            - contactNormalOnB (list): Contact normal on bodyB [x, y, z]
            - contactDistance (float): Penetration distance (negative) or separation (positive)
            - normalForce (float): Normal force magnitude
            - lateralFriction1 (float): Lateral friction force 1
            - lateralFrictionDir1 (list): Lateral friction direction 1 [x, y, z]
            - lateralFriction2 (float): Lateral friction force 2
            - lateralFrictionDir2 (list): Lateral friction direction 2 [x, y, z]
    """

def getClosestPoints(bodyA, bodyB, distance, linkIndexA=-1, linkIndexB=-1, physicsClientId=0):
    """
    Compute closest points between two objects.
    
    Args:
        bodyA (int): First body unique ID
        bodyB (int): Second body unique ID
        distance (float): Maximum distance to compute closest points
        linkIndexA (int, optional): Link index on bodyA (-1 for base). Defaults to -1.
        linkIndexB (int, optional): Link index on bodyB (-1 for base). Defaults to -1.
        physicsClientId (int, optional): Physics client ID. Defaults to 0.
    
    Returns:
        list: List of closest point tuples (same format as getContactPoints)
        
    Note:
        Unlike getContactPoints, this computes closest points even when objects
        are not in contact, up to the specified maximum distance.
    """

def getOverlappingObjects(aabbMin, aabbMax, physicsClientId=0):
    """
    Get objects overlapping with axis-aligned bounding box.
    
    Args:
        aabbMin (list): Minimum corner of AABB [x, y, z]
        aabbMax (list): Maximum corner of AABB [x, y, z]
        physicsClientId (int, optional): Physics client ID. Defaults to 0.
    
    Returns:
        list: List of tuples (bodyUniqueId, linkIndex) for overlapping objects
    """

Ray Casting

Cast rays through the simulation environment for distance sensing and collision detection.

def rayTest(rayFromPosition, rayToPosition, physicsClientId=0):
    """
    Cast single ray (deprecated - use rayTestBatch for better performance).
    
    Args:
        rayFromPosition (list): Ray start position [x, y, z]
        rayToPosition (list): Ray end position [x, y, z]
        physicsClientId (int, optional): Physics client ID. Defaults to 0.
    
    Returns:
        list: Ray hit information (same format as rayTestBatch)
    """

def rayTestBatch(rayFromPositions, rayToPositions, parentObjectUniqueId=None, parentLinkIndex=None, reportHitNumber=0, numThreads=0, physicsClientId=0):
    """
    Cast batch of rays efficiently for sensor simulation.
    
    Args:
        rayFromPositions (list): List of ray start positions [[x, y, z], ...]
        rayToPositions (list): List of ray end positions [[x, y, z], ...]
        parentObjectUniqueId (int, optional): Parent object to ignore in ray casting
        parentLinkIndex (int, optional): Parent link to ignore in ray casting
        reportHitNumber (int, optional): Report hit number (0=closest, 1=all). Defaults to 0.
        numThreads (int, optional): Number of threads for parallel processing. Defaults to 0 (auto).
        physicsClientId (int, optional): Physics client ID. Defaults to 0.
    
    Returns:
        list: List of ray hit results, each containing:
            - objectUniqueId (int): Hit object ID (-1 if no hit)
            - linkIndex (int): Hit link index (-1 for base)
            - hitFraction (float): Hit fraction along ray (0-1)
            - hitPosition (list): Hit position in world coordinates [x, y, z]
            - hitNormal (list): Surface normal at hit point [x, y, z]
    
    Note:
        Much more efficient than individual rayTest calls.
        Supports parallel processing for large ray batches.
    """

Collision Filtering

Control which objects can collide with each other for performance and simulation behavior.

def setCollisionFilterPair(bodyUniqueIdA, bodyUniqueIdB, linkIndexA, linkIndexB, enableCollision, physicsClientId=0):
    """
    Enable or disable collision between specific object pairs.
    
    Args:
        bodyUniqueIdA (int): First body unique ID
        bodyUniqueIdB (int): Second body unique ID
        linkIndexA (int): Link index on first body (-1 for base)
        linkIndexB (int): Link index on second body (-1 for base)
        enableCollision (int): 1 to enable collision, 0 to disable
        physicsClientId (int, optional): Physics client ID. Defaults to 0.
    
    Note:
        Useful for preventing self-collision or collision between
        objects that should pass through each other.
    """

def setCollisionFilterGroupMask(bodyUniqueId, linkIndex, collisionFilterGroup, collisionFilterMask, physicsClientId=0):
    """
    Set collision filter group and mask for bitwise collision filtering.
    
    Args:
        bodyUniqueId (int): Body unique ID
        linkIndex (int): Link index (-1 for base)
        collisionFilterGroup (int): Collision group bitmask
        collisionFilterMask (int): Collision mask bitmask
        physicsClientId (int, optional): Physics client ID. Defaults to 0.
    
    Note:
        Objects collide if (groupA & maskB) != 0 AND (groupB & maskA) != 0.
        Provides flexible collision filtering using bitwise operations.
    """

Usage Examples

Basic Contact Detection

import pybullet as p
import time

p.connect(p.GUI)

# Load objects
planeId = p.loadURDF("plane.urdf")
cubeId = p.loadURDF("cube.urdf", [0, 0, 1])

p.setGravity(0, 0, -10)

# Simulation loop with contact monitoring
for step in range(1000):
    p.stepSimulation()
    
    # Check for contacts every 10 steps
    if step % 10 == 0:
        contacts = p.getContactPoints(bodyA=cubeId, bodyB=planeId)
        
        if contacts:
            contact = contacts[0]  # First contact point
            print(f"Step {step}: Contact detected!")
            print(f"  Position: {contact[5]}")  # positionOnA
            print(f"  Normal force: {contact[9]:.2f}")  # normalForce
            print(f"  Distance: {contact[8]:.4f}")  # contactDistance
    
    time.sleep(1./240.)

Ray-Based Distance Sensor

import pybullet as p
import math
import numpy as np

p.connect(p.GUI)

# Load environment
planeId = p.loadURDF("plane.urdf")
cubeId = p.loadURDF("cube.urdf", [2, 0, 0.5])

# Sensor parameters
sensorPosition = [0, 0, 0.5]
maxRange = 5.0
numRays = 16  # 360-degree sensor

# Create ray directions in a circle
rayDirections = []
for i in range(numRays):
    angle = 2 * math.pi * i / numRays
    direction = [math.cos(angle), math.sin(angle), 0]
    rayDirections.append(direction)

# Prepare ray endpoints
rayFromPositions = [sensorPosition] * numRays
rayToPositions = []
for direction in rayDirections:
    endPoint = [
        sensorPosition[0] + direction[0] * maxRange,
        sensorPosition[1] + direction[1] * maxRange,
        sensorPosition[2] + direction[2] * maxRange
    ]
    rayToPositions.append(endPoint)

# Cast rays and get distances
for step in range(100):
    p.stepSimulation()
    
    # Cast all rays at once
    rayResults = p.rayTestBatch(rayFromPositions, rayToPositions)
    
    # Process results
    distances = []
    for i, result in enumerate(rayResults):
        if result[0] >= 0:  # Hit detected
            hitFraction = result[2]
            distance = hitFraction * maxRange
            distances.append(distance)
        else:
            distances.append(maxRange)  # No hit
    
    # Print sensor data every 50 steps
    if step % 50 == 0:
        print(f"Step {step}: Distances = {[f'{d:.2f}' for d in distances[:4]]}...")

Multi-Ray LIDAR Simulation

import pybullet as p
import numpy as np

p.connect(p.GUI)

# Load complex environment
planeId = p.loadURDF("plane.urdf")
for i in range(5):
    p.loadURDF("cube.urdf", [np.random.uniform(-3, 3), np.random.uniform(-3, 3), 0.5])

# LIDAR parameters
lidarPosition = [0, 0, 1.0]
lidarOrientation = [0, 0, 0, 1]
minAngle = -math.pi
maxAngle = math.pi
numRays = 360
maxRange = 10.0

# Generate LIDAR rays
angles = np.linspace(minAngle, maxAngle, numRays)
rayFromPositions = [lidarPosition] * numRays
rayToPositions = []

for angle in angles:
    endPoint = [
        lidarPosition[0] + maxRange * math.cos(angle),
        lidarPosition[1] + maxRange * math.sin(angle),
        lidarPosition[2]
    ]
    rayToPositions.append(endPoint)

# Simulate LIDAR scanning
for step in range(200):
    p.stepSimulation()
    
    if step % 20 == 0:  # Update LIDAR every 20 steps
        # Cast LIDAR rays
        rayResults = p.rayTestBatch(rayFromPositions, rayToPositions, numThreads=4)
        
        # Process LIDAR data
        lidarData = []
        for i, result in enumerate(rayResults):
            angle = angles[i]
            if result[0] >= 0:  # Hit detected
                distance = result[2] * maxRange
                hitPoint = result[3]
                lidarData.append((angle, distance, hitPoint))
            else:
                lidarData.append((angle, maxRange, None))
        
        # Create point cloud
        pointCloud = []
        for angle, distance, hitPoint in lidarData:
            if hitPoint:
                pointCloud.append(hitPoint)
        
        print(f"Step {step}: LIDAR detected {len(pointCloud)} points")

Collision-Free Path Planning Support

import pybullet as p

p.connect(p.GUI)

# Load robot and obstacles
robotId = p.loadURDF("pr2_gripper.urdf")
obstacleId = p.loadURDF("cube.urdf", [1, 1, 0.5])

def checkCollisionFreePath(robotId, startConfig, endConfig, numSteps=10):
    """
    Check if path between two configurations is collision-free.
    """
    for i in range(numSteps + 1):
        t = i / numSteps
        
        # Interpolate configuration
        currentConfig = []
        for j in range(len(startConfig)):
            value = startConfig[j] + t * (endConfig[j] - startConfig[j])
            currentConfig.append(value)
        
        # Set robot to interpolated configuration
        for j, angle in enumerate(currentConfig):
            p.resetJointState(robotId, j, angle)
        
        # Check for collisions
        p.performCollisionDetection()
        contacts = p.getContactPoints(bodyA=robotId)
        
        if contacts:
            # Filter out contacts with ground plane
            realContacts = [c for c in contacts if c[2] != planeId]  # Assuming planeId exists
            if realContacts:
                return False  # Collision detected
    
    return True  # Path is collision-free

# Test path planning
startConfig = [0, 0, 0, 0, 0, 0, 0]
endConfig = [1.5, 0.5, -0.8, 1.2, 0, 0.5, 0]

if checkCollisionFreePath(robotId, startConfig, endConfig):
    print("Path is collision-free!")
else:
    print("Path has collisions - need to replan")

Advanced Contact Analysis

import pybullet as p
import numpy as np

p.connect(p.GUI)

# Load objects for contact analysis
planeId = p.loadURDF("plane.urdf")
sphereId = p.loadURDF("sphere2.urdf", [0, 0, 2])

p.setGravity(0, 0, -10)

contactHistory = []

for step in range(1000):
    p.stepSimulation()
    
    # Detailed contact analysis
    contacts = p.getContactPoints(bodyA=sphereId, bodyB=planeId)
    
    if contacts:
        for contact in contacts:
            contactInfo = {
                'step': step,
                'bodyA': contact[1],
                'bodyB': contact[2],
                'positionA': contact[5],
                'positionB': contact[6],
                'normal': contact[7],
                'distance': contact[8],
                'normalForce': contact[9],
                'friction1': contact[10],
                'frictionDir1': contact[11],
                'friction2': contact[12],
                'frictionDir2': contact[13]
            }
            contactHistory.append(contactInfo)
    
    # Analysis every 100 steps
    if step % 100 == 0 and contactHistory:
        recentContacts = [c for c in contactHistory if c['step'] > step - 100]
        if recentContacts:
            avgNormalForce = np.mean([c['normalForce'] for c in recentContacts])
            maxNormalForce = max([c['normalForce'] for c in recentContacts])
            print(f"Step {step}: Avg force = {avgNormalForce:.2f}, Max force = {maxNormalForce:.2f}")

Collision Filtering Example

import pybullet as p

p.connect(p.GUI)

# Load multiple objects
groupA_objects = []
groupB_objects = []

# Create group A objects (red)
for i in range(3):
    objId = p.loadURDF("cube.urdf", [i, 0, 1], useFixedBase=False)
    p.changeVisualShape(objId, -1, rgbaColor=[1, 0, 0, 1])
    groupA_objects.append(objId)

# Create group B objects (blue)  
for i in range(3):
    objId = p.loadURDF("cube.urdf", [i, 2, 1], useFixedBase=False)
    p.changeVisualShape(objId, -1, rgbaColor=[0, 0, 1, 1])
    groupB_objects.append(objId)

# Set collision filter groups
# Group A: group=1, mask=2 (collides only with group B)
# Group B: group=2, mask=1 (collides only with group A)
for objId in groupA_objects:
    p.setCollisionFilterGroupMask(objId, -1, collisionFilterGroup=1, collisionFilterMask=2)

for objId in groupB_objects:
    p.setCollisionFilterGroupMask(objId, -1, collisionFilterGroup=2, collisionFilterMask=1)

# Objects in same group won't collide with each other
# but will collide with objects in the other group

p.setGravity(0, 0, -10)

for step in range(1000):
    p.stepSimulation()

Best Practices

Use batch ray casting - rayTestBatch is much more efficient than individual rayTest calls
Limit contact queries - Only call getContactPoints when needed to avoid performance overhead
Filter unnecessary contacts - Use bodyA/bodyB parameters to limit contact queries to relevant objects
Enable collision filtering - Use collision filters to prevent unnecessary collision computations
Consider ray length - Shorter rays are more efficient, use appropriate maximum ranges
Parallel processing - Use numThreads parameter in rayTestBatch for large ray batches
Cache results - Store contact and ray results when they don't change frequently
Performance monitoring - Monitor collision detection performance impact in real-time applications

Geometry and Shape Inspection

def getAABB(bodyUniqueId, linkIndex=-1, physicsClientId=0):
    """
    Get axis-aligned bounding box (AABB) for object or link.
    
    Args:
        bodyUniqueId (int): Body unique ID
        linkIndex (int, optional): Link index (-1 for base). Defaults to -1.
        physicsClientId (int, optional): Physics client ID. Defaults to 0.
    
    Returns:
        tuple: (aabbMin, aabbMax) where each is [x, y, z] coordinates
    """

def getCollisionShapeData(bodyUniqueId, linkIndex=-1, physicsClientId=0):
    """
    Get collision shape information for object or link.
    
    Args:
        bodyUniqueId (int): Body unique ID
        linkIndex (int, optional): Link index (-1 for base). Defaults to -1.
        physicsClientId (int, optional): Physics client ID. Defaults to 0.
    
    Returns:
        list: List of collision shape data for each shape on the link
    """

def getMeshData(bodyUniqueId, linkIndex=-1, flags=0, physicsClientId=0):
    """
    Get mesh data (vertices, faces) for deformable bodies.
    
    Args:
        bodyUniqueId (int): Body unique ID
        linkIndex (int, optional): Link index (-1 for base). Defaults to -1.
        flags (int, optional): Flags for mesh data extraction. Defaults to 0.
        physicsClientId (int, optional): Physics client ID. Defaults to 0.
    
    Returns:
        tuple: (vertices, faces) where vertices is list of [x,y,z] and faces is list of vertex indices
    """

def getTetraMeshData(bodyUniqueId, linkIndex=-1, physicsClientId=0):
    """
    Get tetrahedral mesh data for soft bodies.
    
    Args:
        bodyUniqueId (int): Body unique ID
        linkIndex (int, optional): Link index (-1 for base). Defaults to -1.
        physicsClientId (int, optional): Physics client ID. Defaults to 0.
    
    Returns:
        tuple: Tetrahedral mesh data (vertices, tetrahedra)
    """

def resetMeshData(bodyUniqueId, linkIndex=-1, physicsClientId=0):
    """
    Reset mesh data to original state (for deformable bodies).
    
    Args:
        bodyUniqueId (int): Body unique ID
        linkIndex (int, optional): Link index (-1 for base). Defaults to -1.
        physicsClientId (int, optional): Physics client ID. Defaults to 0.
    """

Geometry Inspection Examples

import pybullet as p

p.connect(p.GUI)
p.setGravity(0, 0, -9.81)

# Load objects
plane_id = p.loadURDF("plane.urdf")
cube_id = p.loadURDF("cube_small.urdf", [0, 0, 1])

# Get AABB for objects
plane_aabb = p.getAABB(plane_id)
cube_aabb = p.getAABB(cube_id)

print(f"Plane AABB: min={plane_aabb[0]}, max={plane_aabb[1]}")
print(f"Cube AABB: min={cube_aabb[0]}, max={cube_aabb[1]}")

# Check objects overlapping with region
region_min = [-1, -1, 0]
region_max = [1, 1, 2]
overlapping = p.getOverlappingObjects(region_min, region_max)
print(f"Objects in region: {overlapping}")

# Get collision shape information
collision_shapes = p.getCollisionShapeData(cube_id)
for i, shape in enumerate(collision_shapes):
    print(f"Shape {i}: {shape}")

# For soft/deformable bodies, get mesh data
# Note: This example assumes a soft body is loaded
if hasattr(p, 'loadSoftBody'):  # Check if soft body support available
    try:
        soft_body_id = p.loadSoftBody("cloth.obj", basePosition=[2, 0, 1])
        
        # Get mesh vertices and faces
        vertices, faces = p.getMeshData(soft_body_id)
        print(f"Soft body has {len(vertices)} vertices and {len(faces)} faces")
        
        # Get tetrahedral mesh for volume soft bodies
        tetra_data = p.getTetraMeshData(soft_body_id)
        if tetra_data:
            print(f"Tetrahedral mesh data: {len(tetra_data[0])} vertices")
            
    except Exception as e:
        print(f"Soft body example failed: {e}")

Install with Tessl CLI