import pybullet as p
import time


class UrdfInertial(object):

  def __init__(self):
    self.mass = 1
    self.inertia_xxyyzz = [7, 8, 9]
    self.origin_rpy = [1, 2, 3]
    self.origin_xyz = [4, 5, 6]


class UrdfContact(object):

  def __init__(self):
    self.lateral_friction = 1
    self.rolling_friction = 0
    self.spinning_friction = 0


class UrdfLink(object):

  def __init__(self):
    self.link_name = "dummy"
    self.urdf_inertial = UrdfInertial()
    self.urdf_visual_shapes = []
    self.urdf_collision_shapes = []


class UrdfVisual(object):

  def __init__(self):
    self.origin_rpy = [1, 2, 3]
    self.origin_xyz = [4, 5, 6]
    self.geom_type = p.GEOM_BOX
    self.geom_radius = 1
    self.geom_extents = [7, 8, 9]
    self.geom_length = 10
    self.geom_meshfilename = "meshfile"
    self.geom_meshscale = [1, 1, 1]
    self.material_rgba = [1, 0, 0, 1]
    self.material_name = ""


class UrdfCollision(object):

  def __init__(self):
    self.origin_rpy = [1, 2, 3]
    self.origin_xyz = [4, 5, 6]
    self.geom_type = p.GEOM_BOX
    self.geom_radius = 1
    self.geom_length = 2
    self.geom_extents = [7, 8, 9]
    self.geom_meshfilename = "meshfile"
    self.geom_meshscale = [1, 1, 1]


class UrdfJoint(object):

  def __init__(self):
    self.link = UrdfLink()
    self.joint_name = "joint_dummy"
    self.joint_type = p.JOINT_REVOLUTE
    self.joint_lower_limit = 0
    self.joint_upper_limit = -1
    self.joint_max_force = 0
    self.joint_max_velocity = 0
    self.parent_name = "parentName"
    self.child_name = "childName"
    self.joint_origin_xyz = [1, 2, 3]
    self.joint_origin_rpy = [1, 2, 3]
    self.joint_axis_xyz = [1, 2, 3]


class UrdfEditor(object):

  def __init__(self):
    self.initialize()

  def initialize(self):
    self.urdfLinks = []
    self.urdfJoints = []
    self.robotName = ""
    self.linkNameToIndex = {}
    self.jointTypeToName={p.JOINT_FIXED: "JOINT_FIXED" ,\
               p.JOINT_REVOLUTE: "JOINT_REVOLUTE",\
               p.JOINT_PRISMATIC: "JOINT_PRISMATIC" }

  def convertLinkFromMultiBody(self, bodyUid, linkIndex, urdfLink, physicsClientId):
    dyn = p.getDynamicsInfo(bodyUid, linkIndex, physicsClientId=physicsClientId)
    urdfLink.urdf_inertial.mass = dyn[0]
    urdfLink.urdf_inertial.inertia_xxyyzz = dyn[2]
    #todo
    urdfLink.urdf_inertial.origin_xyz = dyn[3]
    rpy = p.getEulerFromQuaternion(dyn[4])
    urdfLink.urdf_inertial.origin_rpy = rpy

    visualShapes = p.getVisualShapeData(bodyUid, physicsClientId=physicsClientId)
    matIndex = 0
    for v in visualShapes:
      if (v[1] == linkIndex):
        urdfVisual = UrdfVisual()
        urdfVisual.geom_type = v[2]
        if (v[2] == p.GEOM_BOX):
          urdfVisual.geom_extents = v[3]
        if (v[2] == p.GEOM_SPHERE):
          urdfVisual.geom_radius = v[3][0]
        if (v[2] == p.GEOM_MESH):
          urdfVisual.geom_meshfilename = v[4].decode("utf-8")
          urdfVisual.geom_meshscale = v[3]
        if (v[2] == p.GEOM_CYLINDER):
          urdfVisual.geom_length = v[3][0]
          urdfVisual.geom_radius = v[3][1]
        if (v[2] == p.GEOM_CAPSULE):
          urdfVisual.geom_length = v[3][0]
          urdfVisual.geom_radius = v[3][1]
        urdfVisual.origin_xyz = v[5]
        urdfVisual.origin_rpy = p.getEulerFromQuaternion(v[6])
        urdfVisual.material_rgba = v[7]
        name = 'mat_{}_{}'.format(linkIndex, matIndex)
        urdfVisual.material_name = name
        urdfLink.urdf_visual_shapes.append(urdfVisual)
        matIndex = matIndex + 1

    collisionShapes = p.getCollisionShapeData(bodyUid, linkIndex, physicsClientId=physicsClientId)
    for v in collisionShapes:
      urdfCollision = UrdfCollision()
      urdfCollision.geom_type = v[2]
      if (v[2] == p.GEOM_BOX):
        urdfCollision.geom_extents = v[3]
      if (v[2] == p.GEOM_SPHERE):
        urdfCollision.geom_radius = v[3][0]
      if (v[2] == p.GEOM_MESH):
        urdfCollision.geom_meshfilename = v[4].decode("utf-8")
        urdfCollision.geom_meshscale = v[3]
      if (v[2] == p.GEOM_CYLINDER):
        urdfCollision.geom_length = v[3][0]
        urdfCollision.geom_radius = v[3][1]
      if (v[2] == p.GEOM_CAPSULE):
        urdfCollision.geom_length = v[3][0]
        urdfCollision.geom_radius = v[3][1]
      pos,orn = p.multiplyTransforms(dyn[3],dyn[4],\
       v[5], v[6])
      urdfCollision.origin_xyz = pos
      urdfCollision.origin_rpy = p.getEulerFromQuaternion(orn)
      urdfLink.urdf_collision_shapes.append(urdfCollision)

  def initializeFromBulletBody(self, bodyUid, physicsClientId):
    self.initialize()

    #always create a base link
    baseLink = UrdfLink()
    baseLinkIndex = -1
    self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink, physicsClientId)
    baseLink.link_name = p.getBodyInfo(bodyUid, physicsClientId=physicsClientId)[0].decode("utf-8")
    self.robotName = p.getBodyInfo(bodyUid, physicsClientId=physicsClientId)[1].decode("utf-8")
    self.linkNameToIndex[baseLink.link_name] = len(self.urdfLinks)
    self.urdfLinks.append(baseLink)

    #optionally create child links and joints
    for j in range(p.getNumJoints(bodyUid, physicsClientId=physicsClientId)):
      jointInfo = p.getJointInfo(bodyUid, j, physicsClientId=physicsClientId)
      urdfLink = UrdfLink()
      self.convertLinkFromMultiBody(bodyUid, j, urdfLink, physicsClientId)
      urdfLink.link_name = jointInfo[12].decode("utf-8")
      self.linkNameToIndex[urdfLink.link_name] = len(self.urdfLinks)
      self.urdfLinks.append(urdfLink)

      urdfJoint = UrdfJoint()
      urdfJoint.link = urdfLink
      urdfJoint.joint_name = jointInfo[1].decode("utf-8")
      urdfJoint.joint_type = jointInfo[2]
      urdfJoint.joint_lower_limit = jointInfo[8]
      urdfJoint.joint_upper_limit = jointInfo[9]
      urdfJoint.joint_max_force = jointInfo[10]
      urdfJoint.joint_max_velocity = jointInfo[11]
      urdfJoint.joint_axis_xyz = jointInfo[13]
      orgParentIndex = jointInfo[16]
      if (orgParentIndex < 0):
        urdfJoint.parent_name = baseLink.link_name
      else:
        parentJointInfo = p.getJointInfo(bodyUid, orgParentIndex, physicsClientId=physicsClientId)
        urdfJoint.parent_name = parentJointInfo[12].decode("utf-8")
      urdfJoint.child_name = urdfLink.link_name

      #todo, compensate for inertia/link frame offset

      dynChild = p.getDynamicsInfo(bodyUid, j, physicsClientId=physicsClientId)
      childInertiaPos = dynChild[3]
      childInertiaOrn = dynChild[4]
      parentCom2JointPos = jointInfo[14]
      parentCom2JointOrn = jointInfo[15]
      tmpPos, tmpOrn = p.multiplyTransforms(childInertiaPos, childInertiaOrn, parentCom2JointPos,
                                            parentCom2JointOrn)
      tmpPosInv, tmpOrnInv = p.invertTransform(tmpPos, tmpOrn)
      dynParent = p.getDynamicsInfo(bodyUid, orgParentIndex, physicsClientId=physicsClientId)
      parentInertiaPos = dynParent[3]
      parentInertiaOrn = dynParent[4]

      pos, orn = p.multiplyTransforms(parentInertiaPos, parentInertiaOrn, tmpPosInv, tmpOrnInv)
      pos, orn_unused = p.multiplyTransforms(parentInertiaPos, parentInertiaOrn,
                                             parentCom2JointPos, [0, 0, 0, 1])

      urdfJoint.joint_origin_xyz = pos
      urdfJoint.joint_origin_rpy = p.getEulerFromQuaternion(orn)

      self.urdfJoints.append(urdfJoint)

  def writeInertial(self, file, urdfInertial, precision=5):
    file.write("\t\t<inertial>\n")
    str = '\t\t\t<origin rpy=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\" xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(\
    urdfInertial.origin_rpy[0],urdfInertial.origin_rpy[1],urdfInertial.origin_rpy[2],\
    urdfInertial.origin_xyz[0],urdfInertial.origin_xyz[1],urdfInertial.origin_xyz[2], prec=precision)
    file.write(str)
    str = '\t\t\t<mass value=\"{:.{prec}f}\"/>\n'.format(urdfInertial.mass, prec=precision)
    file.write(str)
    str = '\t\t\t<inertia ixx=\"{:.{prec}f}\" ixy=\"0\" ixz=\"0\" iyy=\"{:.{prec}f}\" iyz=\"0\" izz=\"{:.{prec}f}\"/>\n'.format(\
    urdfInertial.inertia_xxyyzz[0],\
    urdfInertial.inertia_xxyyzz[1],\
    urdfInertial.inertia_xxyyzz[2],prec=precision)
    file.write(str)
    file.write("\t\t</inertial>\n")

  def writeVisualShape(self, file, urdfVisual, precision=5):
    #we don't support loading capsule types from visuals, so auto-convert from collision shape
    if urdfVisual.geom_type == p.GEOM_CAPSULE:
      return

    file.write("\t\t<visual>\n")
    str = '\t\t\t<origin rpy="{:.{prec}f} {:.{prec}f} {:.{prec}f}" xyz="{:.{prec}f} {:.{prec}f} {:.{prec}f}"/>\n'.format(\
     urdfVisual.origin_rpy[0],urdfVisual.origin_rpy[1],urdfVisual.origin_rpy[2],
     urdfVisual.origin_xyz[0],urdfVisual.origin_xyz[1],urdfVisual.origin_xyz[2], prec=precision)
    file.write(str)
    file.write("\t\t\t<geometry>\n")
    if urdfVisual.geom_type == p.GEOM_BOX:
      str = '\t\t\t\t<box size=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfVisual.geom_extents[0],\
       urdfVisual.geom_extents[1],urdfVisual.geom_extents[2], prec=precision)
      file.write(str)
    if urdfVisual.geom_type == p.GEOM_SPHERE:
      str = '\t\t\t\t<sphere radius=\"{:.{prec}f}\"/>\n'.format(urdfVisual.geom_radius,\
       prec=precision)
      file.write(str)
    if urdfVisual.geom_type == p.GEOM_MESH:

      str = '\t\t\t\t<mesh filename=\"{}\" scale=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(
      urdfVisual.geom_meshfilename,urdfVisual.geom_meshscale[0],urdfVisual.geom_meshscale[1],urdfVisual.geom_meshscale[2],\
       prec=precision)
      file.write(str)
    if urdfVisual.geom_type == p.GEOM_CYLINDER:
      str = '\t\t\t\t<cylinder length=\"{:.{prec}f}\" radius=\"{:.{prec}f}\"/>\n'.format(\
       urdfVisual.geom_length, urdfVisual.geom_radius, prec=precision)
      file.write(str)
    if urdfVisual.geom_type == p.GEOM_CAPSULE:
      str = '\t\t\t\t<capsule length=\"{:.{prec}f}\" radius=\"{:.{prec}f}\"/>\n'.format(\
       urdfVisual.geom_length, urdfVisual.geom_radius, prec=precision)
      file.write(str)

    file.write("\t\t\t</geometry>\n")
    str = '\t\t\t<material name=\"{}\">\n'.format(urdfVisual.material_name)
    file.write(str)
    str = '\t\t\t\t<color rgba="{:.{prec}f} {:.{prec}f} {:.{prec}f} {:.{prec}f}" />\n'.format(urdfVisual.material_rgba[0],\
     urdfVisual.material_rgba[1],urdfVisual.material_rgba[2],urdfVisual.material_rgba[3],prec=precision)
    file.write(str)
    file.write("\t\t\t</material>\n")
    file.write("\t\t</visual>\n")

  def writeCollisionShape(self, file, urdfCollision, precision=5):
    file.write("\t\t<collision>\n")
    str = '\t\t\t<origin rpy="{:.{prec}f} {:.{prec}f} {:.{prec}f}" xyz="{:.{prec}f} {:.{prec}f} {:.{prec}f}"/>\n'.format(\
     urdfCollision.origin_rpy[0],urdfCollision.origin_rpy[1],urdfCollision.origin_rpy[2],
     urdfCollision.origin_xyz[0],urdfCollision.origin_xyz[1],urdfCollision.origin_xyz[2], prec=precision)
    file.write(str)
    file.write("\t\t\t<geometry>\n")
    if urdfCollision.geom_type == p.GEOM_BOX:
      str = '\t\t\t\t<box size=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfCollision.geom_extents[0],\
       urdfCollision.geom_extents[1],urdfCollision.geom_extents[2], prec=precision)
      file.write(str)
    if urdfCollision.geom_type == p.GEOM_SPHERE:
      str = '\t\t\t\t<sphere radius=\"{:.{prec}f}\"/>\n'.format(urdfCollision.geom_radius,\
       prec=precision)
      file.write(str)
    if urdfCollision.geom_type == p.GEOM_MESH:
      str = '\t\t\t\t<mesh filename=\"{}\" scale=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfCollision.geom_meshfilename,\
       urdfCollision.geom_meshscale[0],urdfCollision.geom_meshscale[1],urdfCollision.geom_meshscale[2],prec=precision)
      file.write(str)
    if urdfCollision.geom_type == p.GEOM_CYLINDER:
      str = '\t\t\t\t<cylinder length=\"{:.{prec}f}\" radius=\"{:.{prec}f}\"/>\n'.format(\
       urdfCollision.geom_length, urdfCollision.geom_radius, prec=precision)
      file.write(str)
    if urdfCollision.geom_type == p.GEOM_CAPSULE:
      str = '\t\t\t\t<capsule length=\"{:.{prec}f}\" radius=\"{:.{prec}f}\"/>\n'.format(\
       urdfCollision.geom_length, urdfCollision.geom_radius, prec=precision)
      file.write(str)
    file.write("\t\t\t</geometry>\n")
    file.write("\t\t</collision>\n")

  def writeLink(self, file, urdfLink, saveVisuals):
    file.write("\t<link name=\"")
    file.write(urdfLink.link_name)
    file.write("\">\n")

    self.writeInertial(file, urdfLink.urdf_inertial)
    hasCapsules = False
    for v in urdfLink.urdf_visual_shapes:
      if (v.geom_type == p.GEOM_CAPSULE):
        hasCapsules = True
    if (saveVisuals and not hasCapsules):
      for v in urdfLink.urdf_visual_shapes:
        self.writeVisualShape(file, v)
    for c in urdfLink.urdf_collision_shapes:
      self.writeCollisionShape(file, c)
    file.write("\t</link>\n")

  def writeJoint(self, file, urdfJoint, precision=5):
    jointTypeStr = "invalid"
    if urdfJoint.joint_type == p.JOINT_REVOLUTE:
      if urdfJoint.joint_upper_limit < urdfJoint.joint_lower_limit:
        jointTypeStr = "continuous"
      else:
        jointTypeStr = "revolute"
    if urdfJoint.joint_type == p.JOINT_FIXED:
      jointTypeStr = "fixed"
    if urdfJoint.joint_type == p.JOINT_PRISMATIC:
      jointTypeStr = "prismatic"
    str = '\t<joint name=\"{}\" type=\"{}\">\n'.format(urdfJoint.joint_name, jointTypeStr)
    file.write(str)
    str = '\t\t<parent link=\"{}\"/>\n'.format(urdfJoint.parent_name)
    file.write(str)
    str = '\t\t<child link=\"{}\"/>\n'.format(urdfJoint.child_name)
    file.write(str)

    if jointTypeStr == "prismatic" or jointTypeStr == "revolute":
      lowerLimit = urdfJoint.joint_lower_limit
      upperLimit = urdfJoint.joint_upper_limit
      maxForce = urdfJoint.joint_max_force
      maxVelocity = urdfJoint.joint_max_velocity
      
      str = '\t\t<limit effort="{:.{prec}f}" lower="{:.{prec}f}" upper="{:.{prec}f}" velocity="{:.{prec}f}"/>\n'.format(
          maxForce, lowerLimit, upperLimit, maxVelocity, prec=precision)
      file.write(str)

    file.write("\t\t<dynamics damping=\"1.0\" friction=\"0.0001\"/>\n")
    str = '\t\t<origin xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_origin_xyz[0],\
     urdfJoint.joint_origin_xyz[1],urdfJoint.joint_origin_xyz[2], prec=precision)
    str = '\t\t<origin rpy=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\" xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_origin_rpy[0],\
     urdfJoint.joint_origin_rpy[1],urdfJoint.joint_origin_rpy[2],\
     urdfJoint.joint_origin_xyz[0],\
     urdfJoint.joint_origin_xyz[1],urdfJoint.joint_origin_xyz[2], prec=precision)

    file.write(str)
    str = '\t\t<axis xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_axis_xyz[0],\
     urdfJoint.joint_axis_xyz[1],urdfJoint.joint_axis_xyz[2], prec=precision)
    file.write(str)
    file.write("\t</joint>\n")

  def saveUrdf(self, fileName, saveVisuals=True):
    file = open(fileName, "w")
    file.write("<?xml version=\"0.0\" ?>\n")
    file.write("<robot name=\"")
    file.write(self.robotName)
    file.write("\">\n")

    for link in self.urdfLinks:
      self.writeLink(file, link, saveVisuals)

    for joint in self.urdfJoints:
      self.writeJoint(file, joint)

    file.write("</robot>\n")
    file.close()


#def addLink(...)

  def joinUrdf(self,
               childEditor,
               parentLinkIndex=0,
               jointPivotXYZInParent=[0, 0, 0],
               jointPivotRPYInParent=[0, 0, 0],
               jointPivotXYZInChild=[0, 0, 0],
               jointPivotRPYInChild=[0, 0, 0],
               parentPhysicsClientId=0,
               childPhysicsClientId=0):

    childLinkIndex = len(self.urdfLinks)
    insertJointIndex = len(self.urdfJoints)

    #combine all links, and add a joint

    for link in childEditor.urdfLinks:
      self.linkNameToIndex[link.link_name] = len(self.urdfLinks)
      self.urdfLinks.append(link)
    for joint in childEditor.urdfJoints:
      self.urdfJoints.append(joint)
    #add a new joint between a particular

    jointPivotQuatInChild = p.getQuaternionFromEuler(jointPivotRPYInChild)
    invJointPivotXYZInChild, invJointPivotQuatInChild = p.invertTransform(
        jointPivotXYZInChild, jointPivotQuatInChild)

    #apply this invJointPivot***InChild to all inertial, visual and collision element in the child link
    #inertial
    pos, orn = p.multiplyTransforms(self.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz,
                                    p.getQuaternionFromEuler(
                                        self.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy),
                                    invJointPivotXYZInChild,
                                    invJointPivotQuatInChild,
                                    physicsClientId=parentPhysicsClientId)
    self.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz = pos
    self.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy = p.getEulerFromQuaternion(orn)
    #all visual
    for v in self.urdfLinks[childLinkIndex].urdf_visual_shapes:
      pos, orn = p.multiplyTransforms(v.origin_xyz, p.getQuaternionFromEuler(v.origin_rpy),
                                      invJointPivotXYZInChild, invJointPivotQuatInChild)
      v.origin_xyz = pos
      v.origin_rpy = p.getEulerFromQuaternion(orn)
    #all collision
    for c in self.urdfLinks[childLinkIndex].urdf_collision_shapes:
      pos, orn = p.multiplyTransforms(c.origin_xyz, p.getQuaternionFromEuler(c.origin_rpy),
                                      invJointPivotXYZInChild, invJointPivotQuatInChild)
      c.origin_xyz = pos
      c.origin_rpy = p.getEulerFromQuaternion(orn)

    childLink = self.urdfLinks[childLinkIndex]
    parentLink = self.urdfLinks[parentLinkIndex]

    joint = UrdfJoint()
    joint.link = childLink
    joint.joint_name = "joint_dummy1"
    joint.joint_type = p.JOINT_REVOLUTE
    joint.joint_lower_limit = 0
    joint.joint_upper_limit = -1
    joint.parent_name = parentLink.link_name
    joint.child_name = childLink.link_name
    joint.joint_origin_xyz = jointPivotXYZInParent
    joint.joint_origin_rpy = jointPivotRPYInParent
    joint.joint_axis_xyz = [0, 0, 1]

    #the following commented line would crash PyBullet, it messes up the joint indexing/ordering
    #self.urdfJoints.append(joint)

    #so make sure to insert the joint in the right place, to links/joints match
    self.urdfJoints.insert(insertJointIndex, joint)
    return joint

  def createMultiBody(self,
                      basePosition=[0, 0, 0],
                      baseOrientation=[0, 0, 0, 1],
                      physicsClientId=0):
    #assume link[0] is base
    if (len(self.urdfLinks) == 0):
      return -1

    #for i in range (len(self.urdfLinks)):
    #	print("link", i, "=",self.urdfLinks[i].link_name)

    base = self.urdfLinks[0]

    #v.tmp_collision_shape_ids=[]
    baseMass = base.urdf_inertial.mass
    baseCollisionShapeIndex = -1
    baseShapeTypeArray = []
    baseRadiusArray = []
    baseHalfExtentsArray = []
    lengthsArray = []
    fileNameArray = []
    meshScaleArray = []
    basePositionsArray = []
    baseOrientationsArray = []

    for v in base.urdf_collision_shapes:
      shapeType = v.geom_type
      baseShapeTypeArray.append(shapeType)
      baseHalfExtentsArray.append(
          [0.5 * v.geom_extents[0], 0.5 * v.geom_extents[1], 0.5 * v.geom_extents[2]])
      baseRadiusArray.append(v.geom_radius)
      lengthsArray.append(v.geom_length)
      fileNameArray.append(v.geom_meshfilename)
      meshScaleArray.append(v.geom_meshscale)
      basePositionsArray.append(v.origin_xyz)
      orn = p.getQuaternionFromEuler(v.origin_rpy)
      baseOrientationsArray.append(orn)

    if (len(baseShapeTypeArray)):
      #print("fileNameArray=",fileNameArray)
      baseCollisionShapeIndex = p.createCollisionShapeArray(
          shapeTypes=baseShapeTypeArray,
          radii=baseRadiusArray,
          halfExtents=baseHalfExtentsArray,
          lengths=lengthsArray,
          fileNames=fileNameArray,
          meshScales=meshScaleArray,
          collisionFramePositions=basePositionsArray,
          collisionFrameOrientations=baseOrientationsArray,
          physicsClientId=physicsClientId)

    urdfVisuals = base.urdf_visual_shapes

    shapeTypes = [v.geom_type for v in urdfVisuals]
    halfExtents = [[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals]
    radii = [v.geom_radius for v in urdfVisuals]
    lengths = [v.geom_length for v in urdfVisuals]
    fileNames = [v.geom_meshfilename for v in urdfVisuals]
    meshScales = [v.geom_meshscale for v in urdfVisuals]
    rgbaColors = [v.material_rgba for v in urdfVisuals]
    visualFramePositions = [v.origin_xyz for v in urdfVisuals]
    visualFrameOrientations = [p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals]
    baseVisualShapeIndex = -1

    if (len(shapeTypes)):
      #print("len(shapeTypes)=",len(shapeTypes))
      #print("len(halfExtents)=",len(halfExtents))
      #print("len(radii)=",len(radii))
      #print("len(lengths)=",len(lengths))
      #print("len(fileNames)=",len(fileNames))
      #print("len(meshScales)=",len(meshScales))
      #print("len(rgbaColors)=",len(rgbaColors))
      #print("len(visualFramePositions)=",len(visualFramePositions))
      #print("len(visualFrameOrientations)=",len(visualFrameOrientations))

      baseVisualShapeIndex = p.createVisualShapeArray(
          shapeTypes=shapeTypes,
          halfExtents=halfExtents,
          radii=radii,
          lengths=lengths,
          fileNames=fileNames,
          meshScales=meshScales,
          rgbaColors=rgbaColors,
          visualFramePositions=visualFramePositions,
          visualFrameOrientations=visualFrameOrientations,
          physicsClientId=physicsClientId)

    linkMasses = []
    linkCollisionShapeIndices = []
    linkVisualShapeIndices = []
    linkPositions = []
    linkOrientations = []

    linkInertialFramePositions = []
    linkInertialFrameOrientations = []
    linkParentIndices = []
    linkJointTypes = []
    linkJointAxis = []

    for joint in self.urdfJoints:
      link = joint.link
      linkMass = link.urdf_inertial.mass
      linkCollisionShapeIndex = -1
      linkVisualShapeIndex = -1
      linkPosition = [0, 0, 0]
      linkOrientation = [0, 0, 0]
      linkInertialFramePosition = [0, 0, 0]
      linkInertialFrameOrientation = [0, 0, 0]
      linkParentIndex = self.linkNameToIndex[joint.parent_name]
      linkJointType = joint.joint_type
      linkJointAx = joint.joint_axis_xyz
      linkShapeTypeArray = []
      linkRadiusArray = []
      linkHalfExtentsArray = []
      lengthsArray = []
      fileNameArray = []
      linkMeshScaleArray = []
      linkPositionsArray = []
      linkOrientationsArray = []

      for v in link.urdf_collision_shapes:
        shapeType = v.geom_type
        linkShapeTypeArray.append(shapeType)
        linkHalfExtentsArray.append(
            [0.5 * v.geom_extents[0], 0.5 * v.geom_extents[1], 0.5 * v.geom_extents[2]])
        linkRadiusArray.append(v.geom_radius)
        lengthsArray.append(v.geom_length)
        fileNameArray.append(v.geom_meshfilename)
        linkMeshScaleArray.append(v.geom_meshscale)
        linkPositionsArray.append(v.origin_xyz)
        linkOrientationsArray.append(p.getQuaternionFromEuler(v.origin_rpy))

      if (len(linkShapeTypeArray)):
        linkCollisionShapeIndex = p.createCollisionShapeArray(
            shapeTypes=linkShapeTypeArray,
            radii=linkRadiusArray,
            halfExtents=linkHalfExtentsArray,
            lengths=lengthsArray,
            fileNames=fileNameArray,
            meshScales=linkMeshScaleArray,
            collisionFramePositions=linkPositionsArray,
            collisionFrameOrientations=linkOrientationsArray,
            physicsClientId=physicsClientId)

      urdfVisuals = link.urdf_visual_shapes
      linkVisualShapeIndex = -1
      shapeTypes = [v.geom_type for v in urdfVisuals]
      halfExtents = [[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals]
      radii = [v.geom_radius for v in urdfVisuals]
      lengths = [v.geom_length for v in urdfVisuals]
      fileNames = [v.geom_meshfilename for v in urdfVisuals]
      meshScales = [v.geom_meshscale for v in urdfVisuals]
      rgbaColors = [v.material_rgba for v in urdfVisuals]
      visualFramePositions = [v.origin_xyz for v in urdfVisuals]
      visualFrameOrientations = [p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals]

      if (len(shapeTypes)):
        print("fileNames=", fileNames)

        linkVisualShapeIndex = p.createVisualShapeArray(
            shapeTypes=shapeTypes,
            halfExtents=halfExtents,
            radii=radii,
            lengths=lengths,
            fileNames=fileNames,
            meshScales=meshScales,
            rgbaColors=rgbaColors,
            visualFramePositions=visualFramePositions,
            visualFrameOrientations=visualFrameOrientations,
            physicsClientId=physicsClientId)

      linkMasses.append(linkMass)
      linkCollisionShapeIndices.append(linkCollisionShapeIndex)
      linkVisualShapeIndices.append(linkVisualShapeIndex)
      linkPositions.append(joint.joint_origin_xyz)
      linkOrientations.append(p.getQuaternionFromEuler(joint.joint_origin_rpy))
      linkInertialFramePositions.append(link.urdf_inertial.origin_xyz)
      linkInertialFrameOrientations.append(p.getQuaternionFromEuler(link.urdf_inertial.origin_rpy))
      linkParentIndices.append(linkParentIndex)
      linkJointTypes.append(joint.joint_type)
      linkJointAxis.append(joint.joint_axis_xyz)
    obUid = p.createMultiBody(baseMass,\
       baseCollisionShapeIndex=baseCollisionShapeIndex,
                                          baseVisualShapeIndex=baseVisualShapeIndex,
       basePosition=basePosition,
       baseOrientation=baseOrientation,
       baseInertialFramePosition=base.urdf_inertial.origin_xyz,
       baseInertialFrameOrientation=p.getQuaternionFromEuler(base.urdf_inertial.origin_rpy),
       linkMasses=linkMasses,
       linkCollisionShapeIndices=linkCollisionShapeIndices,
       linkVisualShapeIndices=linkVisualShapeIndices,
       linkPositions=linkPositions,
       linkOrientations=linkOrientations,
       linkInertialFramePositions=linkInertialFramePositions,
       linkInertialFrameOrientations=linkInertialFrameOrientations,
       linkParentIndices=linkParentIndices,
       linkJointTypes=linkJointTypes,
       linkJointAxis=linkJointAxis,
       physicsClientId=physicsClientId)
    return obUid

  def __del__(self):
    pass