import pybullet as p
import time
from pybullet_utils import urdfEditor
import pybullet_data


##########################################
org2 = p.connect(p.DIRECT)
org = p.connect(p.SHARED_MEMORY)
if (org < 0):
  org = p.connect(p.DIRECT)

gui = p.connect(p.GUI)

p.setAdditionalSearchPath(pybullet_data.getDataPath())

p.resetSimulation(physicsClientId=org)

#door.urdf, hinge.urdf, duck_vhacd.urdf, r2d2.urdf, quadruped/quadruped.urdf

mb = p.loadURDF("r2d2.urdf", flags=p.URDF_USE_IMPLICIT_CYLINDER, physicsClientId=org)
for i in range(p.getNumJoints(mb, physicsClientId=org)):
  p.setJointMotorControl2(mb, i, p.VELOCITY_CONTROL, force=0, physicsClientId=org)

#print("numJoints:",p.getNumJoints(mb,physicsClientId=org))

#print("base name:",p.getBodyInfo(mb,physicsClientId=org))

#for i in range(p.getNumJoints(mb,physicsClientId=org)):
#	print("jointInfo(",i,"):",p.getJointInfo(mb,i,physicsClientId=org))
#	print("linkState(",i,"):",p.getLinkState(mb,i,physicsClientId=org))

parser = urdfEditor.UrdfEditor()
parser.initializeFromBulletBody(mb, physicsClientId=org)
parser.saveUrdf("test.urdf")

if (1):
  print("\ncreatingMultiBody...................n")

  obUid = parser.createMultiBody(physicsClientId=gui)

  parser2 = urdfEditor.UrdfEditor()
  print("\n###########################################\n")

  parser2.initializeFromBulletBody(obUid, physicsClientId=gui)
  parser2.saveUrdf("test2.urdf")

  for i in range(p.getNumJoints(obUid, physicsClientId=gui)):
    p.setJointMotorControl2(obUid,
                            i,
                            p.VELOCITY_CONTROL,
                            targetVelocity=0,
                            force=1,
                            physicsClientId=gui)
    print(p.getJointInfo(obUid, i, physicsClientId=gui))

  parser = 0

p.setRealTimeSimulation(1, physicsClientId=gui)

while (p.getConnectionInfo(physicsClientId=gui)["isConnected"]):
  p.stepSimulation(physicsClientId=gui)
  time.sleep(0.01)
