URDFs for robots missing tip

[vonHartz]

#201 describes how to export the URDFs for the robots.
However, these URDFs are missing the IK tip.

Step 4 says 'In the generated urdf file, manually convert Pandatip from a visual to a link.'
However, even for the Panda the tip is not part of the exported URDF at all.

What am I missing?

Coppeliasim version is 4.8

[ARoefer]

Hello,

Since nobody bothered to reply and the solution described in #201 does not seem to work, I offer another solution to this problem: Reconstructing the kinematics from RLBench by querying the sim.

import numpy as np

from collections import defaultdict
from dataclasses import dataclass

from pyrep.objects.object import ObjectType
from scipy.spatial.transform import Rotation


@dataclass
class Joint:
    name : str
    type : str
    parent : str
    child  : str
    position_xyz : tuple[float, float, float]
    rotation_rpy : tuple[float, float, float]
    q_limits : tuple[float, float] = None
    qd_limit : float = None

@dataclass
class Link:
    name : str

def make_flat_kinematic(fk_chain):
    links = {'ROOT_LINK': Link('ROOT_LINK')}
    joints    = {}

    for parent, obj in zip([None] + fk_chain[:-1], fk_chain):
        parent_T_child = obj.get_matrix(relative_to=parent)

        position_xyz = tuple(parent_T_child[:3, 3])
        # Need to reverse the tuple because we store the order of rotation left-to-right
        rotation_rpy = tuple(Rotation.from_matrix(parent_T_child[:3, :3]).as_euler('ZYX'))[::-1]

        link = Link(f'{obj.get_name()}_LINK')
        links[link.name] = link

        if obj.get_type() == ObjectType.JOINT:
            joint = Joint(f'{obj.get_name()}_JOINT',
                          'revolute',
                          f'{parent.get_name()}_LINK' if parent is not None else 'ROOT_LINK',
                          link.name,
                          position_xyz,
                          rotation_rpy,
                          # REALLY STUPID: COPPELLIA RETURNS A FLAG THAT SEEMS TO INDICATE ABSOLUTENESS OF UPPER BOUND. WAT?!
                          (((lb:=obj.get_joint_interval()[1][0]), lb+obj.get_joint_interval()[1][1]) if not obj.get_joint_interval()[0] else obj.get_joint_interval()[1][1]),
                          obj.get_joint_upper_velocity_limit())
        else:
            joint = Joint(f'{obj.get_name()}_JOINT',
                          'fixed',
                          f'{parent.get_name()}_LINK' if parent is not None else 'ROOT_LINK',
                          link.name,
                          position_xyz,
                          rotation_rpy,
                          )
        
        joints[joint.name] = joint
    
    return links, joints

def get_robot_ee_kinematic(pyrep_robot):
    pyrep_robot.set_joint_positions(np.zeros(len(pyrep_robot.joints)), disable_dynamics=True)

    ik_tip = pyrep_robot.get_tip()
    inv_fk_chain = [ik_tip]
    while (parent:=inv_fk_chain[-1].get_parent()) is not None:
        inv_fk_chain.append(parent)

    filtered_chain = [ik_tip]
    for body in inv_fk_chain[1:]:
        if body.get_type() == ObjectType.JOINT:
            filtered_chain.append(body)

    fk_chain = list(reversed(filtered_chain))

    # Reversal should not really matter...
    return make_flat_kinematic(fk_chain) + (fk_chain,)

def make_urdf(name : str, links : dict[str, Link], joints : dict[str, Joint]) -> str:
    out = f'<robot name="{name}">\n'

    for link in links.values():
        out += f'  <link name="{link.name}"/>\n'
    
    for j in joints.values():
        out += f'  <joint name="{j.name}" type="{j.type}">\n'
        out += f'    <origin xyz="{j.position_xyz[0]} {j.position_xyz[1]} {j.position_xyz[2]}" rpy="{j.rotation_rpy[0]} {j.rotation_rpy[1]} {j.rotation_rpy[2]}"/>\n'
        out += f'    <parent link="{j.parent}" />\n    <child link="{j.child}" />\n'
        
        if j.type == 'revolute':
            out +=  '    <axis xyz="0 0 1" />\n'
            out += f'    <limit effort="1000" upper="{j.q_limits[1]}" lower="{j.q_limits[0]}" velocity="{j.qd_limit}" />\n'
        out += '  </joint>\n'

    out += '</robot>'
    return out

Using this code you can use urdf_str = make_urdf(robot.get_name(), *(get_robot_ee_kinematic(arm))) to generate a URDF of the forward kinematic for the body identified by arm.get_tip(). I have not bothered to implement prismatic joints, as the Panda does not contain any that I am interested in.

NOTE

• URDF and Coppellia seem to handle frames differently, so it does not generate a correct FK for any intermittent bodies.
• I have tried it only with the Panda, but find other FK-libs such as roboticstoolbox produce the exact same frame for the endeffector given the same q-pose.
• It does not support prismatic joints as is, because I have not bothered to implement them, as I do not need them.

To the devs: Feel free to include this code in RLBench, if you want to. As someone from a more classical robotics background I found it very surprising/troubling/infuriating that there's no way to obtain the actual FK of the robots short of querying the simulator. This bars researchers from using any external MPC tool to control the robot.

[vonHartz]

To add some more context to Adrian's response: the kinematics generated by RLBench (CoppeliaSim) are different than the ones generated, for example, by RTB when using the exported URDF.
For example, passing the same joint pose to RTB's forward kinematic (with RLBench's URDF) and RLBench's internal forward kinematics, yields slightly different results. And the difference do not seem to be systematic - ie. the difference is not constant joint poses.

Hope this is helpful to anymore struggling with this.