Collision Detection Narrow-Phase Details

Narrow-phase PyBullet APIs, collision mode implementation details, and configuration reference.

For the overall system flow and object lifecycle, see Collision Detection Overview.

Narrow-Phase Details: PyBullet APIs

After broad-phase filtering, we use PyBullet’s APIs for exact collision detection.

API Comparison

Feature	`getContactPoints()`	`getClosestPoints()`
Requires stepSimulation()	✅ Yes	❌ No
Works with kinematics	⚠️ Limited	✅ Yes
Safety margin support	❌ No	✅ Yes (distance param)
Physics-accurate	✅ Yes	⚠️ Geometric only
Speed (typical)	Fast (cached)	Slightly slower (GJK)
Best for	Physics mode	Kinematics mode

`getClosestPoints(distance)` - Recommended for Kinematics

What it does: Geometric closest point query using GJK algorithm

Key Parameters:

closest_points = p.getClosestPoints(
    bodyA=obj1.body_id,
    bodyB=obj2.body_id,
    distance=0.02,  # Safety margin (2cm)
    physicsClientId=self.client
)

Behavior:

distance > 0: Returns points if objects are within margin
distance = 0: Returns points only if actually touching
len(closest_points) > 0: Collision detected

Advantages:

✅ Works with resetBasePositionAndOrientation() (kinematic)
✅ Stable for kinematic-kinematic pairs
✅ Safety margin support (detect “near miss”)
✅ No stepSimulation() required

Use cases:

Kinematics-focused planning
Collision avoidance with safety buffer
High-speed simulation (no physics overhead)

`getContactPoints()` - For Physics Mode

What it does: Query contact manifold from physics engine

Key Parameters:

contact_points = p.getContactPoints(
    bodyA=obj1.body_id,
    bodyB=obj2.body_id,
    physicsClientId=self.client
)

Behavior:

Returns contact manifold from last stepSimulation() call
Includes penetration depth, normal, contact position
Empty list if no contact

Advantages:

✅ Fast (contact cache already exists)
✅ Physics-accurate (actual contact forces)
✅ Contact normals and depths available

Limitations:

❌ Requires stepSimulation(): Cannot skip physics update
❌ Limited for kinematics: Designed for physics simulation with dynamic objects
❌ No safety margin support: Only reports actual penetration

Why not ideal for kinematics:

Kinematics mode uses resetBasePositionAndOrientation() to set positions directly
getContactPoints() relies on contact cache updated by stepSimulation()
Without physics simulation, contact information may be incomplete or outdated
You must call stepSimulation() even for kinematic objects, adding overhead
Physics mode
Contact force analysis
Debugging actual collisions

stepSimulation() - The Physics Update

stepSimulation() performs comprehensive physics update:

Time Integration: Update positions, velocities (F=ma)
Broad-Phase: Internal collision detection
Narrow-Phase: Exact collision testing
Contact Solving: Resolve penetrations, apply friction
Cache Update: Update contact manifold

Cost: Computationally expensive (scene-dependent)

When Required:

✅ Physics ON mode (dynamic objects, mass > 0)
❌ Kinematics mode (pure planning)

Two-Mode Design Philosophy

Mode 1: Kinematics (Default) ⚡

physics: false
collision_detection_method: CLOSEST_POINTS
collision_margin: 0.02

No stepSimulation() → Fast
getClosestPoints() → Stable
Use case: Planning, coordination, high-speed simulation

Mode 2: Physics 🔬

physics: true
collision_detection_method: CONTACT_POINTS  # or HYBRID
collision_margin: 0.02

stepSimulation() every frame → Accurate
getContactPoints() → Realistic contacts
Use case: Physics simulation, debugging, contact analysis

Hybrid Mode (Advanced):

if obj_is_physics:
    use getContactPoints()  # Accurate for dynamics
else:
    use getClosestPoints()  # Stable for kinematics

Collision Mode Details

This section provides comprehensive details for each collision mode.

NORMAL_3D - Full 3D Collision

Characteristics:

Neighbor Check: 27 cells (3×3×3 cube)
AABB Updates: Every movement
Grid Registration: Updated on movement
PyBullet Collision: Enabled

Use Cases:

Flying drones (need Z-axis collision)
3D manipulators
Multi-level warehouses
Any object that moves in 3D space

Configuration:

drone = SimObject(
    body_id=body_id,
    sim_core=sim_core,
    collision_mode=CollisionMode.NORMAL_3D,
    mass=0.0  # Kinematic
)

Performance:

Checks 27 neighbor cells vs 9 for 2D
~3x more cells, but filtered by AABB
Typical overhead: <10% vs NORMAL_2D

NORMAL_2D - 2D Collision (XY Plane)

Characteristics:

Neighbor Check: 9 cells (3×3×1, XY plane only)
AABB Updates: Every movement
Grid Registration: Updated on movement
Z-Axis: Ignored for neighbor search (but AABB Z must still overlap!)

Use Cases:

Ground robots (AGVs, AMRs)
Warehouse robots (single floor)
2D path planning
Most typical use cases

Configuration:

agv = SimObject(
    body_id=body_id,
    sim_core=sim_core,
    collision_mode=CollisionMode.NORMAL_2D,
    mass=0.0
)

STATIC - Fixed Objects

Characteristics:

Neighbor Check: 27 cells (when checked by moving objects)
AABB Updates: NEVER (calculated once at spawn)
Grid Registration: Once at spawn, never updated
Movement: Assumed to never move

Use Cases:

Walls, pillars, structures
Shelves, racks
Fixed obstacles
Any object that never moves

Configuration:

wall = SimObject(
    body_id=body_id,
    sim_core=sim_core,
    collision_mode=CollisionMode.STATIC,
    is_static=True
)

Performance Benefits:

Many static walls (e.g., 1000):
  NORMAL_3D: All walls updated every frame → High overhead
  STATIC: 0 AABB updates/frame → No overhead ✓

  → Moving robots can still collide with walls!
  → Walls never trigger updates themselves

Important: If a STATIC object needs to move, change mode first:

wall.set_collision_mode(CollisionMode.NORMAL_3D)  # Enable updates
wall.set_pose([new_x, new_y, new_z])               # Move
wall.set_collision_mode(CollisionMode.STATIC)      # Re-optimize

DISABLED - No Collision

Characteristics:

Neighbor Check: NEVER (not in spatial grid)
AABB Updates: NEVER
Grid Registration: NEVER
PyBullet Collision: Disabled via setCollisionFilterPair()

Use Cases:

Visualization markers
Debug geometry
Temporary exclusion
Non-physical objects

Configuration:

marker = SimObject(
    body_id=body_id,
    sim_core=sim_core,
    collision_mode=CollisionMode.DISABLED,
)

Behavior:

# Object is completely invisible to collision system
marker.set_pose([x, y, z])  # Visual update only
pairs = sim_core.check_collisions()  # Marker ignored ✓

Performance:

Zero overhead (not processed at all)
Use liberally for visualization objects

Runtime Mode Changes

Supported Transitions:

# Any mode → Any mode
obj.set_collision_mode(new_mode)

# Example: Temporarily disable
obj.set_collision_mode(CollisionMode.DISABLED)
# ... do something ...
obj.set_collision_mode(CollisionMode.NORMAL_3D)  # Re-enable

What Happens Internally:

Remove from old mode’s data structures
Update AABB if needed
Register to new mode’s data structures
Update PyBullet collision filter if DISABLED ↔ other

Use Cases:

Dynamic environment changes
Temporary object exclusion
Performance optimization
A/B testing different modes

Implementation & Configuration

SimulationParams

from pybullet_fleet.core_simulation import SimulationParams
from pybullet_fleet.types import CollisionDetectionMethod

# Kinematics Mode (Recommended Default)
params = SimulationParams(
    physics=False,
    collision_detection_method=CollisionDetectionMethod.CLOSEST_POINTS,
    collision_margin=0.02,  # 2cm safety buffer
    spatial_hash_cell_size=2.0,  # 2m cells
    multi_cell_threshold=1.5,  # Large object threshold
)

# Physics Mode
params = SimulationParams(
    physics=True,
    collision_detection_method=CollisionDetectionMethod.CONTACT_POINTS,
    collision_margin=0.02,
    spatial_hash_cell_size=2.0,
)

# Hybrid Mode (Advanced)
params = SimulationParams(
    physics=True,
    collision_detection_method=CollisionDetectionMethod.HYBRID,
    collision_margin=0.05,  # Larger margin for kinematics safety
    spatial_hash_cell_size=2.0,
)

Per-Object Configuration

# Moving 3D robot
robot_3d = SimObject(
    body_id=body_id,
    sim_core=sim_core,
    collision_mode=CollisionMode.NORMAL_3D,
    mass=0.0,  # Kinematic
)

# Moving 2D AGV
agv = SimObject(
    body_id=body_id,
    sim_core=sim_core,
    collision_mode=CollisionMode.NORMAL_2D,
    mass=0.0,
)

# Fixed structure
wall = SimObject(
    body_id=body_id,
    sim_core=sim_core,
    collision_mode=CollisionMode.STATIC,
    is_static=True,
)

# Visualization marker
marker = SimObject(
    body_id=body_id,
    sim_core=sim_core,
    collision_mode=CollisionMode.DISABLED,
)

Config File (YAML)

# config.yaml
physics: false
collision_detection_method: "closest_points"
collision_margin: 0.02
spatial_hash_cell_size: 2.0
multi_cell_threshold: 1.5
ignore_static_collision: false

Auto-Selection Logic

Collision Detection Method:

physics=False → CLOSEST_POINTS (auto)
physics=True → CONTACT_POINTS (auto)
Explicit setting overrides auto-selection

Collision Mode:

Default: NORMAL_3D if not specified
Recommended: Explicitly set for clarity

Collision Margin Recommendations

Use Case	Margin	Rationale
Tight spaces	0.01-0.02m	Minimal clearance
Normal operation	0.02-0.05m	Comfortable safety
Conservative	0.05-0.10m	Extra safety for high-speed
Contact detection	0.0m	Actual touching only

Collision Detection Narrow-Phase Details

Narrow-Phase Details: PyBullet APIs

API Comparison

getClosestPoints(distance) - Recommended for Kinematics

getContactPoints() - For Physics Mode

stepSimulation() - The Physics Update

Two-Mode Design Philosophy

Collision Mode Details

NORMAL_3D - Full 3D Collision

NORMAL_2D - 2D Collision (XY Plane)

STATIC - Fixed Objects

DISABLED - No Collision

Runtime Mode Changes

Implementation & Configuration

SimulationParams

Per-Object Configuration

Config File (YAML)

Auto-Selection Logic

Collision Margin Recommendations

See Also

`getClosestPoints(distance)` - Recommended for Kinematics

`getContactPoints()` - For Physics Mode