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()