79 lines
1.8 KiB
Python
79 lines
1.8 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()
|