def create_urdf_from_mesh(meshfile, filename, position=(0., 0., 0.), orientation=(0., 0., 0.), scale=(1., 1., 1.),
                          color=(1, 1, 1, 1), texture=None, mass=0., inertia=(0., 0., 0., 0., 0., 0.),
                          lateral_friction=0.5, rolling_friction=0., spinning_friction=0., restitution=0.,
                          kp=None, kd=None):  # , cfm=0., erf=0.):
    """
    Create a URDF file and insert the specified mesh inside.

    Args:
        meshfile (str): path to the mesh file
        filename (str): filename of the urdf
        position (float[3]): position of the mesh
        orientation (float[3]): orientation (roll, pitch, yaw) of the mesh
        scale (float[3]): scale factor in the x, y, z directions
        color (float[4]): RGBA color where rgb=(0,0,0) is for black, rgb=(1,1,1) is for white, and a=1 means opaque.
        texture (str, None): path to the texture to be applied to the object. If None, provided it will use the
            given color.
        mass (float): mass in kg
        inertia (float[6]): upper/lower triangle of the inertia matrix (read from left to right, top to bottom)
        lateral_friction (float): friction coefficient
        rolling_friction (float): rolling friction coefficient orthogonal to contact normal
        spinning_friction (float): spinning friction coefficient around contact normal
        restitution (float): restitution coefficient
        kp (float, None): contact stiffness (useful to make surfaces soft). Set it to None/-1 if not using it.
        kd (float, None): contact damping (useful to make surfaces soft). Set it to None/-1 if not using it.
        #cfm: constraint force mixing
        #erp: error reduction parameter

    References:
        - "ROS URDF Tutorial": http://wiki.ros.org/urdf/Tutorials
        - "URDF: Link": http://wiki.ros.org/urdf/XML/link
        - "Tutorial: Using a URDF in Gazebo": http://gazebosim.org/tutorials/?tut=ros_urdf
        - SDF format: http://sdformat.org/spec
    """
    def get_str(lst):
        return ' '.join([str(i) for i in lst])

    position = get_str(position)
    orientation = get_str(orientation)
    color = get_str(color)
    scale = get_str(scale)
    name = meshfile.split('/')[-1][:-4]
    ixx, ixy, ixz, iyy, iyz, izz = [str(i) for i in inertia]

    with open(filename, 'w') as f:
        f.write('<?xml version="0.0" ?>')
        f.write('<robot name="'+name+'">')
        f.write('\t<link name="base">')

        f.write('\t\t<contact>')
        f.write('\t\t\t<lateral_friction value="' + str(lateral_friction) + '"/>')
        f.write('\t\t\t<rolling_friction value="' + str(rolling_friction) + '"/>')
        f.write('\t\t\t<spinning_friction value="' + str(spinning_friction) + '"/>')
        f.write('\t\t\t<restitution value="' + str(restitution) + '"/>')
        if kp is not None:
            f.write('\t\t\t<stiffness value="' + str(kp) + '"/>')
        if kd is not None:
            f.write('\t\t\t<damping value="' + str(kd) + '"/>')
        # f.write('\t\t\t<contact_cfm value="' + str(cfm) + '"/>')
        # f.write('\t\t\t<contact_erp value="' + str(erp) + '"/>')
        # f.write('\t\t\t<inertia_scaling value="' + str(inertia_scaling) + '"/>')
        f.write('\t\t</contact>')

        f.write('\t\t<inertial>')
        f.write('\t\t\t<origin rpy="' + orientation + '" xyz="' + position + '"/>')
        f.write('\t\t\t<mass value="' + str(mass) + '"/>')
        f.write('\t\t\t<inertia ixx="'+str(ixx)+'" ixy="'+str(ixy)+'" ixz="'+str(ixz)+'" iyy="'+str(iyy)+'" iyz="'+
                str(iyz)+'" izz="'+str(izz)+'"/>')
        f.write('\t\t</inertial>')

        f.write('\t\t<visual>')
        f.write('\t\t\t<origin rpy="' + orientation + '" xyz="' + position + '"/>')
        f.write('\t\t\t<geometry>')
        f.write('\t\t\t\t<mesh filename="' + meshfile + '" scale="' + scale + '"/>')
        f.write('\t\t\t</geometry>')
        f.write('\t\t\t<material name="color">')
        if texture is not None:
            f.write('\t\t\t\t<texture filename="' + texture + '"/>')
        else:
            f.write('\t\t\t\t<color rgba="' + color + '"/>')
        f.write('\t\t\t</material>')
        f.write('\t\t</visual>')

        f.write('\t\t<collision>')
        f.write('\t\t\t<origin rpy="' + orientation + '" xyz="' + position + '"/>')
        f.write('\t\t\t<geometry>')
        f.write('\t\t\t\t<mesh filename="' + meshfile + '" scale="' + scale + '"/>')
        f.write('\t\t\t</geometry>')
        f.write('\t\t</collision>')

        f.write('\t</link>')
        f.write('</robot>')