From f1f04aef539e58312c66c5909597e6149cbca02a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 4 Nov 2019 15:45:58 -0800 Subject: [PATCH] add simple sceneAABB example --- examples/pybullet/examples/sceneAabb.py | 39 +++++++++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 examples/pybullet/examples/sceneAabb.py diff --git a/examples/pybullet/examples/sceneAabb.py b/examples/pybullet/examples/sceneAabb.py new file mode 100644 index 000000000..aa11ff288 --- /dev/null +++ b/examples/pybullet/examples/sceneAabb.py @@ -0,0 +1,39 @@ +import pybullet as p +import time +import numpy as np + +p.connect(p.GUI) +p.loadURDF("plane.urdf") +p.loadURDF("sphere2.urdf",[0,0,2]) + +dt = 1./240. +p.setTimeStep(dt) + +def getSceneAABB(): + aabbMins=[] + aabbMaxs=[] + + for i in range(p.getNumBodies()): + uid = p.getBodyUniqueId(i) + aabb = p.getAABB(uid) + aabbMins.append(np.array(aabb[0])) + aabbMaxs.append(np.array(aabb[1])) + + if len(aabbMins): + sceneAABBMin = aabbMins[0] + sceneAABBMax = aabbMaxs[0] + + for aabb in aabbMins: + sceneAABBMin = np.minimum(sceneAABBMin,aabb) + for aabb in aabbMaxs: + sceneAABBMax = np.maximum(sceneAABBMax,aabb) + + print("sceneAABBMin=",sceneAABBMin) + print("sceneAABBMax=",sceneAABBMax) + +getSceneAABB() + +while (1): + p.stepSimulation() + time.sleep(dt) +