#rudimentary MuJoCo mjcf to ROS URDF converter using the UrdfEditor

from pybullet_utils import bullet_client as bc
import pybullet_data as pd

import pybullet_utils.urdfEditor as ed
import argparse
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('--mjcf',
                    help='MuJoCo xml file to be converted to URDF',
                    default='mjcf/humanoid.xml')
args = parser.parse_args()

p = bc.BulletClient()
p.setAdditionalSearchPath(pd.getDataPath())
objs = p.loadMJCF(args.mjcf, flags=p.URDF_USE_IMPLICIT_CYLINDER)

for o in objs:
  #print("o=",o, p.getBodyInfo(o), p.getNumJoints(o))
  humanoid = objs[o]
  ed0 = ed.UrdfEditor()
  ed0.initializeFromBulletBody(humanoid, p._client)
  robotName = str(p.getBodyInfo(o)[1], 'utf-8')
  partName = str(p.getBodyInfo(o)[0], 'utf-8')

  print("robotName=", robotName)
  print("partName=", partName)

  saveVisuals = False
  ed0.saveUrdf(robotName + "_" + partName + ".urdf", saveVisuals)