Matching Frames of Reference - Absolute Position of Joints

[caio-freitas]

The absolute joint positions being used now in the graph_dataset.py are being calculated with forward kinematics from the franka panda robot. However, they don't necessary match the absolute position given by the Objects in the scene (are likely to be expressed in different frames of reference). This is particularly bad for the transport environment, where the positions are calculated for 2 different manipulators, and the transformation between the base links would be relevant.

One idea (very work-intensive) is to get these absolute positions from the mujoco environment somehow, reusing the datasets with the Robomimic playback_human_demonstration. Another quick fix is to find out the transformations for base link in these environments and add them when training.

[caio-freitas]

https://robosuite.ai/docs/modeling/mujoco_model.html and https://robosuite.ai/docs/modeling/robot_model.html can also be useful

[caio-freitas]

The whole mujoco model file can be get by playing back the dataset files and getting the env.get_state()["model"]. There all positions and orientations of objects in the Mujoco environment can be found.

For the lift dataset, name="robot0_base" pos="-0.56 0 0.912" (no quat defined, must be not rotated)

[caio-freitas]

For the Square, it's the same transform

[caio-freitas]

For the Transport, it's robot0_base pos="0 -0.81 0.912" quat="0.707107 0 0 0.707107"

robot1_base pos="0 0.81 0.912" quat="0.707107 0 0 -0.707107"

[caio-freitas]

The function

def get_pos_from_xml(xml):
    '''
    Get position of robot base link from xml file
    '''
    filtered_lines = []
    lines = xml.split("\n")
    filtered_lines = [line for line in lines if "robot" in line and "pos=" in line and "link" in line]
    # extract positions from the filtered lines
    positions = []
    for line in filtered_lines:
        position = line.split("pos=")[1].split(" ")[:3]
        positions.append(position)
    return positions

Gets the joint positions from the XML file. They are, however, not absolute. The forward tree must be calculated from the transformations (with quaternions and positions) from baseline to EEF.