80 lines
2.0 KiB
Python
80 lines
2.0 KiB
Python
import pybullet as p
|
|
draw = 1
|
|
printtext = 0
|
|
|
|
if (draw):
|
|
p.connect(p.GUI)
|
|
else:
|
|
p.connect(p.DIRECT)
|
|
r2d2 = p.loadURDF("r2d2.urdf")
|
|
|
|
|
|
def drawAABB(aabb):
|
|
f = [aabbMin[0], aabbMin[1], aabbMin[2]]
|
|
t = [aabbMax[0], aabbMin[1], aabbMin[2]]
|
|
p.addUserDebugLine(f, t, [1, 0, 0])
|
|
f = [aabbMin[0], aabbMin[1], aabbMin[2]]
|
|
t = [aabbMin[0], aabbMax[1], aabbMin[2]]
|
|
p.addUserDebugLine(f, t, [0, 1, 0])
|
|
f = [aabbMin[0], aabbMin[1], aabbMin[2]]
|
|
t = [aabbMin[0], aabbMin[1], aabbMax[2]]
|
|
p.addUserDebugLine(f, t, [0, 0, 1])
|
|
|
|
f = [aabbMin[0], aabbMin[1], aabbMax[2]]
|
|
t = [aabbMin[0], aabbMax[1], aabbMax[2]]
|
|
p.addUserDebugLine(f, t, [1, 1, 1])
|
|
|
|
f = [aabbMin[0], aabbMin[1], aabbMax[2]]
|
|
t = [aabbMax[0], aabbMin[1], aabbMax[2]]
|
|
p.addUserDebugLine(f, t, [1, 1, 1])
|
|
|
|
f = [aabbMax[0], aabbMin[1], aabbMin[2]]
|
|
t = [aabbMax[0], aabbMin[1], aabbMax[2]]
|
|
p.addUserDebugLine(f, t, [1, 1, 1])
|
|
|
|
f = [aabbMax[0], aabbMin[1], aabbMin[2]]
|
|
t = [aabbMax[0], aabbMax[1], aabbMin[2]]
|
|
p.addUserDebugLine(f, t, [1, 1, 1])
|
|
|
|
f = [aabbMax[0], aabbMax[1], aabbMin[2]]
|
|
t = [aabbMin[0], aabbMax[1], aabbMin[2]]
|
|
p.addUserDebugLine(f, t, [1, 1, 1])
|
|
|
|
f = [aabbMin[0], aabbMax[1], aabbMin[2]]
|
|
t = [aabbMin[0], aabbMax[1], aabbMax[2]]
|
|
p.addUserDebugLine(f, t, [1, 1, 1])
|
|
|
|
f = [aabbMax[0], aabbMax[1], aabbMax[2]]
|
|
t = [aabbMin[0], aabbMax[1], aabbMax[2]]
|
|
p.addUserDebugLine(f, t, [1.0, 0.5, 0.5])
|
|
f = [aabbMax[0], aabbMax[1], aabbMax[2]]
|
|
t = [aabbMax[0], aabbMin[1], aabbMax[2]]
|
|
p.addUserDebugLine(f, t, [1, 1, 1])
|
|
f = [aabbMax[0], aabbMax[1], aabbMax[2]]
|
|
t = [aabbMax[0], aabbMax[1], aabbMin[2]]
|
|
p.addUserDebugLine(f, t, [1, 1, 1])
|
|
|
|
|
|
aabb = p.getAABB(r2d2)
|
|
aabbMin = aabb[0]
|
|
aabbMax = aabb[1]
|
|
if (printtext):
|
|
print(aabbMin)
|
|
print(aabbMax)
|
|
if (draw == 1):
|
|
drawAABB(aabb)
|
|
|
|
for i in range(p.getNumJoints(r2d2)):
|
|
aabb = p.getAABB(r2d2, i)
|
|
aabbMin = aabb[0]
|
|
aabbMax = aabb[1]
|
|
if (printtext):
|
|
print(aabbMin)
|
|
print(aabbMax)
|
|
if (draw == 1):
|
|
drawAABB(aabb)
|
|
|
|
while (1):
|
|
a = 0
|
|
p.stepSimulation()
|