import pybullet as p
import time
useMaximalCoordinates = False

flags = p.URDF_ENABLE_SLEEPING

p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)

p.loadURDF("plane100.urdf", flags=flags, useMaximalCoordinates=useMaximalCoordinates)
#p.loadURDF("cube_small.urdf", [0,0,0.5], flags=flags)
r2d2 = -1
for k in range(5):
  for i in range(5):
    r2d2 = p.loadURDF("r2d2.urdf", [k * 2, i * 2, 1],
                      useMaximalCoordinates=useMaximalCoordinates,
                      flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES + flags)

    #enable sleeping: you can pass the flag during URDF loading, or do it afterwards
    #p.changeDynamics(r2d2,-1,activationState=p.ACTIVATION_STATE_ENABLE_SLEEPING)

    for j in range(p.getNumJoints(r2d2)):
      p.setJointMotorControl2(r2d2, j, p.VELOCITY_CONTROL, targetVelocity=0)
print("r2d2=", r2d2)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
timestep = 1. / 240.
p.setTimeStep(timestep)
p.setGravity(0, 0, -10)

while p.isConnected():
  p.stepSimulation()
  time.sleep(timestep)
  #force the object to wake up
  p.changeDynamics(r2d2, -1, activationState=p.ACTIVATION_STATE_WAKE_UP)