Repository: stepjam/RLBench
Issue: Forward kinematics for robot arm
Root cause: Unimplemented Feature Gap

Hi,

I'm wondering if it is possible to add support for computing forward kinematics for robot arms in RLBench, so that we can compute the actual required end-effector pose from `executed_demo_joint_position_action`.

Thanks!

You can compute forward kinematics using another package like [pytorch_kinematics](https://github.com/UM-ARM-Lab/pytorch_kinematics) with the urdf provided:
https://github.com/stepjam/RLBench/tree/master/urdfs/panda

Hi, did u solve the problem?

I tried the pytorch_kinematics library, but found that the results of the forward kinematic do not match with results returned by the simulator.

Here is my code. It loads the low dim observations, which is of class rlbench.backend.observations.Observation, use its gripper joint positions as input to the forward kinematics to get the calculated output.
```python
import pytorch_kinematics as pk
import pickle
import numpy as np
import torch
import glob

urdf = "racer_datagen/panda.urdf"
chain = pk.build_chain_from_urdf(open(urdf, mode="rb").read())
chain.print_tree()

obs_paths = "racer_datagen/runs/val/meat_off_grill/1/video/*/low_dim_obs_end.pkl"
obs_paths = glob.glob(obs_paths)
for obs_path in obs_paths:
    obs = pickle.load(open(obs_path, "rb"))

    th = np.concatenate([obs.joint_positions, obs.gripper_joint_positions], axis=0).tolist()
    ret = chain.forward_kinematics(th)["Pandagripper"]

    m = ret.get_matrix()
    pos = m[:, :3, 3]
    rot = pk.matrix_to_quaternion(m[:, :3, :3])
    
    pose_diff = pos - torch.tensor(obs.gripper_pose[:3], dtype=torch.float32)
    rot_diff = rot - torch.tensor(obs.gripper_pose[3:], dtype=torch.float32)
    print(f"pos_diff: {pose_diff}, rot_diff: {rot_diff}")
```


And the output is as follows. Besides the warning on unknown attributes, there are significant differences between calculated values and the values from obs.
```
Unknown attribute "name" in /robot[@name='Panda']/link[@name='robot_base']/collision[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='robot_base']/visual[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='robot_base']/visual[2]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink1respondable']/collision[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink1respondable']/visual[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink2respondable']/collision[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink2respondable']/visual[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink3respondable']/collision[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink3respondable']/visual[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink3respondable']/visual[2]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink4respondable']/collision[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink4respondable']/visual[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink4respondable']/visual[2]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink5respondable']/collision[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink5respondable']/collision[2]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink5respondable']/collision[3]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink5respondable']/collision[4]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink5respondable']/collision[5]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink5respondable']/collision[6]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink5respondable']/collision[7]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink5respondable']/visual[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink5respondable']/visual[2]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink6respondable']/collision[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink6respondable']/visual[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink6respondable']/visual[2]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink6respondable']/visual[3]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink6respondable']/visual[4]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink6respondable']/visual[5]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink6respondable']/visual[6]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink7respondable']/collision[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink7respondable']/visual[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink7respondable']/visual[2]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink7respondable']/visual[3]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink7respondable']/visual[4]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandalink7respondable']/visual[5]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandagripper']/collision[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandagripper']/visual[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandagripper']/visual[2]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandagripper']/visual[3]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandagripper']/visual[4]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandagripper']/visual[5]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandaleftfingerrespondable']/collision[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandaleftfingerrespondable']/visual[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandaleftfingerrespondable']/visual[2]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandaleftfingerforcecontact']/collision[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandarightfingerrespondable']/collision[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandarightfingerrespondable']/visual[1]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandarightfingerrespondable']/visual[2]
Unknown attribute "name" in /robot[@name='Panda']/link[@name='Pandarightfingerforcecontact']/collision[1]
robot_base
└── Pandalink1respondable
    └── Pandalink2respondable
        └── Pandalink3respondable
            └── Pandalink4respondable
                └── Pandalink5respondable
                    └── Pandalink6respondable
                        └── Pandalink7respondable
                            └── Pandagripper
                                ├── Pandaleftfingerrespondable
                                │   └── Pandaleftfingerforcecontact
                                ├── Pandarightfingerrespondable
                                │   └── Pandarightfingerforcecontact
                                └── Pandatip

pos_diff: tensor([[-0.0145, -0.0017,  0.0782]]), rot_diff: tensor([[ 0.1880, -1.5496,  0.8181,  0.0180]])
pos_diff: tensor([[-0.0071, -0.0025,  0.0776]]), rot_diff: tensor([[ 0.1863, -1.5490,  0.8187,  0.0139]])
pos_diff: tensor([[-0.0033,  0.0013,  0.0779]]), rot_diff: tensor([[ 0.1892, -1.5510,  0.8172,  0.0201]])
pos_diff: tensor([[-0.0043,  0.0012,  0.0779]]), rot_diff: tensor([[ 0.5956,  0.0356, -0.5297, -0.0323]])
pos_diff: tensor([[-0.0105,  0.0052,  0.0776]]), rot_diff: tensor([[ 0.1988, -1.5501,  0.8171,  0.0307]])
pos_diff: tensor([[-0.0107,  0.0055,  0.0779]]), rot_diff: tensor([[ 0.1999, -1.5511,  0.8163,  0.0316]])
pos_diff: tensor([[-0.0067,  0.0020,  0.0776]]), rot_diff: tensor([[ 0.2017, -1.5482,  0.8181,  0.0328]])
pos_diff: tensor([[-0.0154,  0.0072,  0.0783]]), rot_diff: tensor([[ 0.2001, -1.5508,  0.8170,  0.0305]])
pos_diff: tensor([[-0.0152, -0.0045,  0.0788]]), rot_diff: tensor([[ 0.1848, -1.5506,  0.8175,  0.0103]])
pos_diff: tensor([[-0.0153, -0.0051,  0.0787]]), rot_diff: tensor([[ 0.1857, -1.5489,  0.8186,  0.0129]])
pos_diff: tensor([[-0.0149,  0.0071,  0.0783]]), rot_diff: tensor([[ 0.2001, -1.5508,  0.8169,  0.0306]])
pos_diff: tensor([[-0.0165,  0.0075,  0.0787]]), rot_diff: tensor([[ 0.2003, -1.5508,  0.8170,  0.0304]])
pos_diff: tensor([[-0.0062, -0.0011,  0.0775]]), rot_diff: tensor([[ 0.1885, -1.5492,  0.8185,  0.0189]])
```

Is there anything wrong with my code? Thanks!