Convert urdf robot model to OpenRAVE's robot xml format

urdf_to_ravexml.py

# -*- coding: utf-8 -*-
# /// script
# dependencies = ["yattag", "urdfpy", "scipy"]
# ///

# Usage:
# pipx run urdf_to_ravexml.py --urdf path/to/urdffile.urdf --robotxmldir $(pwd)/robots/ --meshprefix robots/meshes/

from scipy.spatial.transform import Rotation as R
from urdfpy import URDF
import argparse
import numpy as np
import pathlib
import xml.dom.minidom
import yattag

parser = argparse.ArgumentParser()
parser.add_argument('--urdf', type=str)
parser.add_argument('--robotxmldir', type=str)
parser.add_argument('--meshprefix', type=str, default='')
args = parser.parse_args()

doc, tag, text = yattag.Doc().tagtext()
urdfrobot = URDF.load(args.urdf)

def to_rotvec(rotmat):
    r = R.from_matrix(rotmat)
    rotvec = r.as_rotvec(degrees=True)
    deg = np.linalg.norm(rotvec)
    return (rotvec / deg, deg)


def to_ravejointtype(urdftype: str):
    """
    Returns
    str: urdf joint type
    bool: is a moving joint
    """

    if urdftype == 'revolute':
        return 'hinge', True
    elif urdftype == 'fixed':
        return 'hinge', False  # NOTE: set limits to zero
    elif urdftype == 'continuous':
        return 'hinge', True  # NOTE: set limits to -180 180
    elif urdftype == 'prismatic':
        return 'slider', True
        pass

with tag('Robot', name=urdfrobot.name):
    with tag('KinBody', name=urdfrobot.name):
        for link in urdfrobot.links:
            joint = next(filter(lambda j: j.child == link.name, urdfrobot.joints), None)
            print(joint)
            with tag('Body', name=link.name, type='dynamic'):
                if joint is not None:
                    with tag('offsetfrom'):
                        text(joint.parent)
                    with tag('translation'):
                        text(' '.join(map(str, joint.origin[0:3,3])))
                    rotvec, deg = to_rotvec(joint.origin[0:3,0:3])
                    if np.abs(deg) > 1e-4:
                        with tag('rotationaxis'):
                            text(' '.join(map(str, rotvec)) + ' ' + str(deg))
                if len(link.collisions) > 0 or len(link.visuals) > 0:
                    with tag('Geom', type='trimesh'):
                        for col, vis in zip(link.collisions, link.visuals):
                            with tag('translation'):
                                text(' '.join(map(str, col.origin[0:3,3])))
                            rotvec, deg = to_rotvec(col.origin[0:3,0:3])
                            if np.abs(deg) > 1e-4:
                                with tag('rotationaxis'):
                                    text(' '.join(map(str, rotvec)) + ' ' + str(deg))
                            with tag('Data'):
                                text(f'{args.meshprefix}{col.geometry.mesh.filename} {col.geometry.mesh.scale[0]}')
                            with tag('Render'):
                                text(f'{args.meshprefix}{vis.geometry.mesh.filename} {vis.geometry.mesh.scale[0]}')
                # TODO mass/inertia
        for joint in urdfrobot.joints:
            jointtype, moving = to_ravejointtype(joint.joint_type)
            with tag('Joint', name=joint.name, type=jointtype):
                with tag('Body'):
                    text(joint.parent)
                with tag('Body'):
                    text(joint.child)
                with tag('offsetfrom'):

                    text(joint.child)
                with tag('weight'):
                    text(1)  # TODO  set an approrpriate weight for distance metrics
                if not moving:
                    with tag('limits'):
                        text("0 0")
                elif joint.limit is not None:
                    with tag('limits'):
                        text(' '.join(map(str, [joint.limit.lower, joint.limit.upper])))
                    with tag('maxvel'):
                        text(joint.limit.velocity)
                    with tag('maxtorque'):
                        text(joint.limit.effort)
                with tag('axis'):
                    text(' '.join(map(str, joint.axis)))
                # TODO mimic
                # TODO resolution for interpolation
                # TODO joint.dynamics.damping and joint.dynamics.friction
    #with tag('Manipulator', name='arm'):
    #    # with tag('effector'):
    #    #     # TODO assuming the last joint would be the last joint
    #    #     text(urdfrobot.joints[-1].child)
    #    with tag('base'):
    #        text(urdfrobot.joints[0].parent)
    #    with tag('joints'):
    #        text(' '.join([joint.name for joint in urdfrobot.joints]))

dom = xml.dom.minidom.parseString(doc.getvalue())
pretty_xml = dom.toprettyxml()
with open(pathlib.Path(args.robotxmldir) / (urdfrobot.name + '.robot.xml'), 'w') as f:
    f.write(pretty_xml)