Obtain the gripper status from robot0_gripper_qpos

[GilgameshD]

Hi, I find that the observation dict contains the robot0_gripper_qpos, which is a 6-dimensional vector representing the angles of 6 joints. Is it possible to use this qpos to calculate the distance between two fingers of the gripper?

[abhihjoshi]

I think you can do something like the following to get the distance between two fingers of the grippers:

finger1_col = env.sim.data.geom_xpos[env.sim.model.geom_name2id("gripper0_finger1_collision")]
finger2_col = env.sim.data.geom_xpos[env.sim.model.geom_name2id("gripper0_finger2_collision")]
distance = np.sum((finger1_col - finger2_col) ** 2, axis=0)

This code is basically accessing the simulation data for the geoms in the environment and directly calculating the distance between them using the Euclidean distance. You can also find the position of any of the geoms by replace the string passed into geom_name2id.

Some other side notes:

• To find a list of all the valid geom names, you can use this: print(env.sim.model.geom_names)
• You can also find get get the position or rotation matrix for body elements instead of only geom elements. Just replace geom_xpos with <geom/body>_x<pos/xmat>

Let me know if this helped answer your question or if you have any further questions!