Other Action Modes for Imitation Learning

[vonHartz]

Hi,

thanks for your great work!

I was wondering how to extract actions for other action modes from the imitation learning example.

Specifically for EE_POSE_EE_FRAME. The observation contains the gripper_pose, but it's in the world frame, isn't it? How can I get it for the EE_POSE_EE_FRAME?

Thanks in advance!

[vonHartz]

In case anyone else has this problem, here's how i solved it:

  for t in range(config["episodes"]):
      for s in range(len(demos[t]) - 1):
          obs = demos[t][s]
          next_obs = demos[t][s+1]
          current_pose = obs.gripper_pose
          next_pose = next_obs.gripper_pose

          e2w = obs.gripper_matrix
          w2e = np.linalg.inv(e2w)

          position_world = next_pose[:3]
          position_ee = np.dot(w2e, np.append(position_world, 1))[:3]

          quat_diff = quaternion_multiply(next_pose[3:],
                                          conjugate_quat(current_pose[3:]))
          rotation_world = quaternion_to_euler(quat_diff)
          rotation_matrix_world = euler_to_matrix(rotation_world)
          rotation_matrix_ee = np.dot(np.dot(e2w, rotation_matrix_world),
                                      w2e)[:3, :3]

          rotation_ee = matrix_to_euler(rotation_matrix_ee)
          pose_ee = np.append(position_ee, rotation_ee)
          action = np.append(pose_ee, [obs.gripper_open])

[zwbx]

In case anyone else has this problem, here's how i solved it:

  for t in range(config["episodes"]):
      for s in range(len(demos[t]) - 1):
          obs = demos[t][s]
          next_obs = demos[t][s+1]
          current_pose = obs.gripper_pose
          next_pose = next_obs.gripper_pose

          e2w = obs.gripper_matrix
          w2e = np.linalg.inv(e2w)

          position_world = next_pose[:3]
          position_ee = np.dot(w2e, np.append(position_world, 1))[:3]

          quat_diff = quaternion_multiply(next_pose[3:],
                                          conjugate_quat(current_pose[3:]))
          rotation_world = quaternion_to_euler(quat_diff)
          rotation_matrix_world = euler_to_matrix(rotation_world)
          rotation_matrix_ee = np.dot(np.dot(e2w, rotation_matrix_world),
                                      w2e)[:3, :3]

          rotation_ee = matrix_to_euler(rotation_matrix_ee)
          pose_ee = np.append(position_ee, rotation_ee)
          action = np.append(pose_ee, [obs.gripper_open])

Hey, that is very useful to me. Thanks! By the way, could I ask you, based on this code, how can I transfer absolute EE_POSE_EE_FRAME to the delta EE_POSE_EE_FRAME.

[zwbx]

I am a little confused that the rotation_ee is a delta value and the position_ee seem to be a absolute one. Could do explain it, thanks!

[vonHartz]

The answer to both your questions is the same one: in EE frame the EE delta and absolute next EE pose are the same. ;)

[zwbx]

Thank you, I found it empirically. I also noticed another related thing. By your code, I tried to collect the delta EE_POSE_EE_FRAME data. It works well for the simple task where the orientation of the gripper barely changes, but fails in the hard cases. The IK solution seems not to be stable and accurate in the EE frame. As time goes by, the gripper falls into a chaotic state until the IK warning rise. In the meantime, all things go well with delta EE_POSE_WORLD_FRAME frame. So I guess the failure on hard case could be caused by accumulated IK errors, which are also exacerbated by frame transformation errors.

[vonHartz]

Yes, I think there might be some issue with RLBench's IK solver. I've also had some unrelated problems with it, but did not have time to look into it and open a proper issue. (Could also have to do with different frames, such as gripper vs TCP, but I haven't looked into it.)