0
# Collision Detection
1
2
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.
3
4
## Capabilities
5
6
### Contact Point Queries
7
8
Get detailed information about contacts between objects in the simulation.
9
10
```python { .api }
11
def getContactPoints(bodyA=None, bodyB=None, linkIndexA=-1, linkIndexB=-1, physicsClientId=0):
12
"""
13
Get contact points between objects.
14
15
Args:
16
bodyA (int, optional): First body unique ID. If None, gets all contacts.
17
bodyB (int, optional): Second body unique ID. If None, gets contacts with bodyA.
18
linkIndexA (int, optional): Link index on bodyA (-1 for base). Defaults to -1.
19
linkIndexB (int, optional): Link index on bodyB (-1 for base). Defaults to -1.
20
physicsClientId (int, optional): Physics client ID. Defaults to 0.
21
22
Returns:
23
list: List of contact point tuples, each containing:
24
- contactFlag (int): Contact type flag
25
- bodyUniqueIdA (int): First body ID
26
- bodyUniqueIdB (int): Second body ID
27
- linkIndexA (int): Link index on first body
28
- linkIndexB (int): Link index on second body
29
- positionOnA (list): Contact position on bodyA [x, y, z]
30
- positionOnB (list): Contact position on bodyB [x, y, z]
31
- contactNormalOnB (list): Contact normal on bodyB [x, y, z]
32
- contactDistance (float): Penetration distance (negative) or separation (positive)
33
- normalForce (float): Normal force magnitude
34
- lateralFriction1 (float): Lateral friction force 1
35
- lateralFrictionDir1 (list): Lateral friction direction 1 [x, y, z]
36
- lateralFriction2 (float): Lateral friction force 2
37
- lateralFrictionDir2 (list): Lateral friction direction 2 [x, y, z]
38
"""
39
40
def getClosestPoints(bodyA, bodyB, distance, linkIndexA=-1, linkIndexB=-1, physicsClientId=0):
41
"""
42
Compute closest points between two objects.
43
44
Args:
45
bodyA (int): First body unique ID
46
bodyB (int): Second body unique ID
47
distance (float): Maximum distance to compute closest points
48
linkIndexA (int, optional): Link index on bodyA (-1 for base). Defaults to -1.
49
linkIndexB (int, optional): Link index on bodyB (-1 for base). Defaults to -1.
50
physicsClientId (int, optional): Physics client ID. Defaults to 0.
51
52
Returns:
53
list: List of closest point tuples (same format as getContactPoints)
54
55
Note:
56
Unlike getContactPoints, this computes closest points even when objects
57
are not in contact, up to the specified maximum distance.
58
"""
59
60
def getOverlappingObjects(aabbMin, aabbMax, physicsClientId=0):
61
"""
62
Get objects overlapping with axis-aligned bounding box.
63
64
Args:
65
aabbMin (list): Minimum corner of AABB [x, y, z]
66
aabbMax (list): Maximum corner of AABB [x, y, z]
67
physicsClientId (int, optional): Physics client ID. Defaults to 0.
68
69
Returns:
70
list: List of tuples (bodyUniqueId, linkIndex) for overlapping objects
71
"""
72
```
73
74
### Ray Casting
75
76
Cast rays through the simulation environment for distance sensing and collision detection.
77
78
```python { .api }
79
def rayTest(rayFromPosition, rayToPosition, physicsClientId=0):
80
"""
81
Cast single ray (deprecated - use rayTestBatch for better performance).
82
83
Args:
84
rayFromPosition (list): Ray start position [x, y, z]
85
rayToPosition (list): Ray end position [x, y, z]
86
physicsClientId (int, optional): Physics client ID. Defaults to 0.
87
88
Returns:
89
list: Ray hit information (same format as rayTestBatch)
90
"""
91
92
def rayTestBatch(rayFromPositions, rayToPositions, parentObjectUniqueId=None, parentLinkIndex=None, reportHitNumber=0, numThreads=0, physicsClientId=0):
93
"""
94
Cast batch of rays efficiently for sensor simulation.
95
96
Args:
97
rayFromPositions (list): List of ray start positions [[x, y, z], ...]
98
rayToPositions (list): List of ray end positions [[x, y, z], ...]
99
parentObjectUniqueId (int, optional): Parent object to ignore in ray casting
100
parentLinkIndex (int, optional): Parent link to ignore in ray casting
101
reportHitNumber (int, optional): Report hit number (0=closest, 1=all). Defaults to 0.
102
numThreads (int, optional): Number of threads for parallel processing. Defaults to 0 (auto).
103
physicsClientId (int, optional): Physics client ID. Defaults to 0.
104
105
Returns:
106
list: List of ray hit results, each containing:
107
- objectUniqueId (int): Hit object ID (-1 if no hit)
108
- linkIndex (int): Hit link index (-1 for base)
109
- hitFraction (float): Hit fraction along ray (0-1)
110
- hitPosition (list): Hit position in world coordinates [x, y, z]
111
- hitNormal (list): Surface normal at hit point [x, y, z]
112
113
Note:
114
Much more efficient than individual rayTest calls.
115
Supports parallel processing for large ray batches.
116
"""
117
```
118
119
### Collision Filtering
120
121
Control which objects can collide with each other for performance and simulation behavior.
122
123
```python { .api }
124
def setCollisionFilterPair(bodyUniqueIdA, bodyUniqueIdB, linkIndexA, linkIndexB, enableCollision, physicsClientId=0):
125
"""
126
Enable or disable collision between specific object pairs.
127
128
Args:
129
bodyUniqueIdA (int): First body unique ID
130
bodyUniqueIdB (int): Second body unique ID
131
linkIndexA (int): Link index on first body (-1 for base)
132
linkIndexB (int): Link index on second body (-1 for base)
133
enableCollision (int): 1 to enable collision, 0 to disable
134
physicsClientId (int, optional): Physics client ID. Defaults to 0.
135
136
Note:
137
Useful for preventing self-collision or collision between
138
objects that should pass through each other.
139
"""
140
141
def setCollisionFilterGroupMask(bodyUniqueId, linkIndex, collisionFilterGroup, collisionFilterMask, physicsClientId=0):
142
"""
143
Set collision filter group and mask for bitwise collision filtering.
144
145
Args:
146
bodyUniqueId (int): Body unique ID
147
linkIndex (int): Link index (-1 for base)
148
collisionFilterGroup (int): Collision group bitmask
149
collisionFilterMask (int): Collision mask bitmask
150
physicsClientId (int, optional): Physics client ID. Defaults to 0.
151
152
Note:
153
Objects collide if (groupA & maskB) != 0 AND (groupB & maskA) != 0.
154
Provides flexible collision filtering using bitwise operations.
155
"""
156
```
157
158
## Usage Examples
159
160
### Basic Contact Detection
161
162
```python
163
import pybullet as p
164
import time
165
166
p.connect(p.GUI)
167
168
# Load objects
169
planeId = p.loadURDF("plane.urdf")
170
cubeId = p.loadURDF("cube.urdf", [0, 0, 1])
171
172
p.setGravity(0, 0, -10)
173
174
# Simulation loop with contact monitoring
175
for step in range(1000):
176
p.stepSimulation()
177
178
# Check for contacts every 10 steps
179
if step % 10 == 0:
180
contacts = p.getContactPoints(bodyA=cubeId, bodyB=planeId)
181
182
if contacts:
183
contact = contacts[0] # First contact point
184
print(f"Step {step}: Contact detected!")
185
print(f" Position: {contact[5]}") # positionOnA
186
print(f" Normal force: {contact[9]:.2f}") # normalForce
187
print(f" Distance: {contact[8]:.4f}") # contactDistance
188
189
time.sleep(1./240.)
190
```
191
192
### Ray-Based Distance Sensor
193
194
```python
195
import pybullet as p
196
import math
197
import numpy as np
198
199
p.connect(p.GUI)
200
201
# Load environment
202
planeId = p.loadURDF("plane.urdf")
203
cubeId = p.loadURDF("cube.urdf", [2, 0, 0.5])
204
205
# Sensor parameters
206
sensorPosition = [0, 0, 0.5]
207
maxRange = 5.0
208
numRays = 16 # 360-degree sensor
209
210
# Create ray directions in a circle
211
rayDirections = []
212
for i in range(numRays):
213
angle = 2 * math.pi * i / numRays
214
direction = [math.cos(angle), math.sin(angle), 0]
215
rayDirections.append(direction)
216
217
# Prepare ray endpoints
218
rayFromPositions = [sensorPosition] * numRays
219
rayToPositions = []
220
for direction in rayDirections:
221
endPoint = [
222
sensorPosition[0] + direction[0] * maxRange,
223
sensorPosition[1] + direction[1] * maxRange,
224
sensorPosition[2] + direction[2] * maxRange
225
]
226
rayToPositions.append(endPoint)
227
228
# Cast rays and get distances
229
for step in range(100):
230
p.stepSimulation()
231
232
# Cast all rays at once
233
rayResults = p.rayTestBatch(rayFromPositions, rayToPositions)
234
235
# Process results
236
distances = []
237
for i, result in enumerate(rayResults):
238
if result[0] >= 0: # Hit detected
239
hitFraction = result[2]
240
distance = hitFraction * maxRange
241
distances.append(distance)
242
else:
243
distances.append(maxRange) # No hit
244
245
# Print sensor data every 50 steps
246
if step % 50 == 0:
247
print(f"Step {step}: Distances = {[f'{d:.2f}' for d in distances[:4]]}...")
248
```
249
250
### Multi-Ray LIDAR Simulation
251
252
```python
253
import pybullet as p
254
import numpy as np
255
256
p.connect(p.GUI)
257
258
# Load complex environment
259
planeId = p.loadURDF("plane.urdf")
260
for i in range(5):
261
p.loadURDF("cube.urdf", [np.random.uniform(-3, 3), np.random.uniform(-3, 3), 0.5])
262
263
# LIDAR parameters
264
lidarPosition = [0, 0, 1.0]
265
lidarOrientation = [0, 0, 0, 1]
266
minAngle = -math.pi
267
maxAngle = math.pi
268
numRays = 360
269
maxRange = 10.0
270
271
# Generate LIDAR rays
272
angles = np.linspace(minAngle, maxAngle, numRays)
273
rayFromPositions = [lidarPosition] * numRays
274
rayToPositions = []
275
276
for angle in angles:
277
endPoint = [
278
lidarPosition[0] + maxRange * math.cos(angle),
279
lidarPosition[1] + maxRange * math.sin(angle),
280
lidarPosition[2]
281
]
282
rayToPositions.append(endPoint)
283
284
# Simulate LIDAR scanning
285
for step in range(200):
286
p.stepSimulation()
287
288
if step % 20 == 0: # Update LIDAR every 20 steps
289
# Cast LIDAR rays
290
rayResults = p.rayTestBatch(rayFromPositions, rayToPositions, numThreads=4)
291
292
# Process LIDAR data
293
lidarData = []
294
for i, result in enumerate(rayResults):
295
angle = angles[i]
296
if result[0] >= 0: # Hit detected
297
distance = result[2] * maxRange
298
hitPoint = result[3]
299
lidarData.append((angle, distance, hitPoint))
300
else:
301
lidarData.append((angle, maxRange, None))
302
303
# Create point cloud
304
pointCloud = []
305
for angle, distance, hitPoint in lidarData:
306
if hitPoint:
307
pointCloud.append(hitPoint)
308
309
print(f"Step {step}: LIDAR detected {len(pointCloud)} points")
310
```
311
312
### Collision-Free Path Planning Support
313
314
```python
315
import pybullet as p
316
317
p.connect(p.GUI)
318
319
# Load robot and obstacles
320
robotId = p.loadURDF("pr2_gripper.urdf")
321
obstacleId = p.loadURDF("cube.urdf", [1, 1, 0.5])
322
323
def checkCollisionFreePath(robotId, startConfig, endConfig, numSteps=10):
324
"""
325
Check if path between two configurations is collision-free.
326
"""
327
for i in range(numSteps + 1):
328
t = i / numSteps
329
330
# Interpolate configuration
331
currentConfig = []
332
for j in range(len(startConfig)):
333
value = startConfig[j] + t * (endConfig[j] - startConfig[j])
334
currentConfig.append(value)
335
336
# Set robot to interpolated configuration
337
for j, angle in enumerate(currentConfig):
338
p.resetJointState(robotId, j, angle)
339
340
# Check for collisions
341
p.performCollisionDetection()
342
contacts = p.getContactPoints(bodyA=robotId)
343
344
if contacts:
345
# Filter out contacts with ground plane
346
realContacts = [c for c in contacts if c[2] != planeId] # Assuming planeId exists
347
if realContacts:
348
return False # Collision detected
349
350
return True # Path is collision-free
351
352
# Test path planning
353
startConfig = [0, 0, 0, 0, 0, 0, 0]
354
endConfig = [1.5, 0.5, -0.8, 1.2, 0, 0.5, 0]
355
356
if checkCollisionFreePath(robotId, startConfig, endConfig):
357
print("Path is collision-free!")
358
else:
359
print("Path has collisions - need to replan")
360
```
361
362
### Advanced Contact Analysis
363
364
```python
365
import pybullet as p
366
import numpy as np
367
368
p.connect(p.GUI)
369
370
# Load objects for contact analysis
371
planeId = p.loadURDF("plane.urdf")
372
sphereId = p.loadURDF("sphere2.urdf", [0, 0, 2])
373
374
p.setGravity(0, 0, -10)
375
376
contactHistory = []
377
378
for step in range(1000):
379
p.stepSimulation()
380
381
# Detailed contact analysis
382
contacts = p.getContactPoints(bodyA=sphereId, bodyB=planeId)
383
384
if contacts:
385
for contact in contacts:
386
contactInfo = {
387
'step': step,
388
'bodyA': contact[1],
389
'bodyB': contact[2],
390
'positionA': contact[5],
391
'positionB': contact[6],
392
'normal': contact[7],
393
'distance': contact[8],
394
'normalForce': contact[9],
395
'friction1': contact[10],
396
'frictionDir1': contact[11],
397
'friction2': contact[12],
398
'frictionDir2': contact[13]
399
}
400
contactHistory.append(contactInfo)
401
402
# Analysis every 100 steps
403
if step % 100 == 0 and contactHistory:
404
recentContacts = [c for c in contactHistory if c['step'] > step - 100]
405
if recentContacts:
406
avgNormalForce = np.mean([c['normalForce'] for c in recentContacts])
407
maxNormalForce = max([c['normalForce'] for c in recentContacts])
408
print(f"Step {step}: Avg force = {avgNormalForce:.2f}, Max force = {maxNormalForce:.2f}")
409
```
410
411
### Collision Filtering Example
412
413
```python
414
import pybullet as p
415
416
p.connect(p.GUI)
417
418
# Load multiple objects
419
groupA_objects = []
420
groupB_objects = []
421
422
# Create group A objects (red)
423
for i in range(3):
424
objId = p.loadURDF("cube.urdf", [i, 0, 1], useFixedBase=False)
425
p.changeVisualShape(objId, -1, rgbaColor=[1, 0, 0, 1])
426
groupA_objects.append(objId)
427
428
# Create group B objects (blue)
429
for i in range(3):
430
objId = p.loadURDF("cube.urdf", [i, 2, 1], useFixedBase=False)
431
p.changeVisualShape(objId, -1, rgbaColor=[0, 0, 1, 1])
432
groupB_objects.append(objId)
433
434
# Set collision filter groups
435
# Group A: group=1, mask=2 (collides only with group B)
436
# Group B: group=2, mask=1 (collides only with group A)
437
for objId in groupA_objects:
438
p.setCollisionFilterGroupMask(objId, -1, collisionFilterGroup=1, collisionFilterMask=2)
439
440
for objId in groupB_objects:
441
p.setCollisionFilterGroupMask(objId, -1, collisionFilterGroup=2, collisionFilterMask=1)
442
443
# Objects in same group won't collide with each other
444
# but will collide with objects in the other group
445
446
p.setGravity(0, 0, -10)
447
448
for step in range(1000):
449
p.stepSimulation()
450
```
451
452
## Best Practices
453
454
1. **Use batch ray casting** - rayTestBatch is much more efficient than individual rayTest calls
455
2. **Limit contact queries** - Only call getContactPoints when needed to avoid performance overhead
456
3. **Filter unnecessary contacts** - Use bodyA/bodyB parameters to limit contact queries to relevant objects
457
4. **Enable collision filtering** - Use collision filters to prevent unnecessary collision computations
458
5. **Consider ray length** - Shorter rays are more efficient, use appropriate maximum ranges
459
6. **Parallel processing** - Use numThreads parameter in rayTestBatch for large ray batches
460
7. **Cache results** - Store contact and ray results when they don't change frequently
461
8. **Performance monitoring** - Monitor collision detection performance impact in real-time applications
462
463
### Geometry and Shape Inspection
464
465
```python { .api }
466
def getAABB(bodyUniqueId, linkIndex=-1, physicsClientId=0):
467
"""
468
Get axis-aligned bounding box (AABB) for object or link.
469
470
Args:
471
bodyUniqueId (int): Body unique ID
472
linkIndex (int, optional): Link index (-1 for base). Defaults to -1.
473
physicsClientId (int, optional): Physics client ID. Defaults to 0.
474
475
Returns:
476
tuple: (aabbMin, aabbMax) where each is [x, y, z] coordinates
477
"""
478
479
def getCollisionShapeData(bodyUniqueId, linkIndex=-1, physicsClientId=0):
480
"""
481
Get collision shape information for object or link.
482
483
Args:
484
bodyUniqueId (int): Body unique ID
485
linkIndex (int, optional): Link index (-1 for base). Defaults to -1.
486
physicsClientId (int, optional): Physics client ID. Defaults to 0.
487
488
Returns:
489
list: List of collision shape data for each shape on the link
490
"""
491
492
def getMeshData(bodyUniqueId, linkIndex=-1, flags=0, physicsClientId=0):
493
"""
494
Get mesh data (vertices, faces) for deformable bodies.
495
496
Args:
497
bodyUniqueId (int): Body unique ID
498
linkIndex (int, optional): Link index (-1 for base). Defaults to -1.
499
flags (int, optional): Flags for mesh data extraction. Defaults to 0.
500
physicsClientId (int, optional): Physics client ID. Defaults to 0.
501
502
Returns:
503
tuple: (vertices, faces) where vertices is list of [x,y,z] and faces is list of vertex indices
504
"""
505
506
def getTetraMeshData(bodyUniqueId, linkIndex=-1, physicsClientId=0):
507
"""
508
Get tetrahedral mesh data for soft bodies.
509
510
Args:
511
bodyUniqueId (int): Body unique ID
512
linkIndex (int, optional): Link index (-1 for base). Defaults to -1.
513
physicsClientId (int, optional): Physics client ID. Defaults to 0.
514
515
Returns:
516
tuple: Tetrahedral mesh data (vertices, tetrahedra)
517
"""
518
519
def resetMeshData(bodyUniqueId, linkIndex=-1, physicsClientId=0):
520
"""
521
Reset mesh data to original state (for deformable bodies).
522
523
Args:
524
bodyUniqueId (int): Body unique ID
525
linkIndex (int, optional): Link index (-1 for base). Defaults to -1.
526
physicsClientId (int, optional): Physics client ID. Defaults to 0.
527
"""
528
```
529
530
### Geometry Inspection Examples
531
532
```python
533
import pybullet as p
534
535
p.connect(p.GUI)
536
p.setGravity(0, 0, -9.81)
537
538
# Load objects
539
plane_id = p.loadURDF("plane.urdf")
540
cube_id = p.loadURDF("cube_small.urdf", [0, 0, 1])
541
542
# Get AABB for objects
543
plane_aabb = p.getAABB(plane_id)
544
cube_aabb = p.getAABB(cube_id)
545
546
print(f"Plane AABB: min={plane_aabb[0]}, max={plane_aabb[1]}")
547
print(f"Cube AABB: min={cube_aabb[0]}, max={cube_aabb[1]}")
548
549
# Check objects overlapping with region
550
region_min = [-1, -1, 0]
551
region_max = [1, 1, 2]
552
overlapping = p.getOverlappingObjects(region_min, region_max)
553
print(f"Objects in region: {overlapping}")
554
555
# Get collision shape information
556
collision_shapes = p.getCollisionShapeData(cube_id)
557
for i, shape in enumerate(collision_shapes):
558
print(f"Shape {i}: {shape}")
559
560
# For soft/deformable bodies, get mesh data
561
# Note: This example assumes a soft body is loaded
562
if hasattr(p, 'loadSoftBody'): # Check if soft body support available
563
try:
564
soft_body_id = p.loadSoftBody("cloth.obj", basePosition=[2, 0, 1])
565
566
# Get mesh vertices and faces
567
vertices, faces = p.getMeshData(soft_body_id)
568
print(f"Soft body has {len(vertices)} vertices and {len(faces)} faces")
569
570
# Get tetrahedral mesh for volume soft bodies
571
tetra_data = p.getTetraMeshData(soft_body_id)
572
if tetra_data:
573
print(f"Tetrahedral mesh data: {len(tetra_data[0])} vertices")
574
575
except Exception as e:
576
print(f"Soft body example failed: {e}")
577
```