Is the pose in 'traj_X/obs/extra/tcp_pose' the same as the action frame?

[jstmn]

Howdy!

I've trained an Imitation Learning model on the poses in 'traj_X/extra/tcp_pose' for the PickCubeV1 task for the Panda. During inference, i'm calling env.step(action) where action is the pose returned by the model. Are these in the same reference frame?

To be clear, 'these' are Tcp and whatever env.step() expects.

Is using the arm_pd_ee_pose controller and mimicking the 'traj_X/action' vector more typical?

Thanks

[jstmn]

For anyone reading this, they're definitely not the same! I'm guessing tcp_pose is in robot base frame, whereas action is in world frame or something like this

[StoneT2000]

pd_ee_pose probably refers to an absolute end-effector action space.

If you check this

print(env.unwrapped.agent.controllers["pd_ee_pose"].configs["arm"].frame)

it will print root_translation:root_aligned_body_rotation (the default) which defines the frames in which translation and rotation (applied separately) work. Recommend reading https://maniskill.readthedocs.io/en/latest/user_guide/concepts/controllers.html#pd-ee-end-effector-pose to see what that means and what it looks like

The value stored in the tcp_pose is the result of self.agent.tcp.pose.raw_pose in the PickCube-v1 code. agent.tcp is a articulation link and link pose data is always in world frame.

So you are correct, the frames are different (root translation means relative to the root of the robot, also known as the robot base frame where the base link is the 0)

Visual example:

applying the following action repeatedly (or just once followed by env.step(None) a bunch of times to let the sim converge to what you last set as the target pose)

action = np.zeros_like(env.action_space.sample())
action[0] = 0.0
action[2] = 0.5
action[3] = np.pi

results in:

0.mp4

action = np.zeros_like(env.action_space.sample())
action[0] = 0.5
action[2] = 0.5
action[3] = np.pi

results in

0.mp4

(note that the position of the end-effector at the start of the episode is approximately like 0.615, 0, 0.3 or something)

If your goal is to train an IL policy to predict absolute end-effector space actions, you should use the replay tool to replay actions in the end-effector space as so:

python -m mani_skill.trajectory.replay_trajectory   --traj-path ~/.maniskill/demos/PickCube-v1/motionplanning/trajectory.h5   --use-first-env-state -c pd_ee_pose -o state --vis --num-envs 1 -b physx_cpu # visualize the demos

python -m mani_skill.trajectory.replay_trajectory   --traj-path ~/.maniskill/demos/PickCube-v1/motionplanning/trajectory.h5   --use-first-env-state -c pd_ee_pose -o state --num-envs 10 -b physx_cpu # generate and save the dataset with pd_ee_pose actions

(you can copy the standard replay scripts we use for our baselines in https://github.com/haosulab/ManiSkill/blob/main/scripts/data_generation/replay_for_il_baselines.sh and modify the control mode which modifies the actions stored in the dataset, note this only works for the panda robot usually and is not designed to work for any robot)

[jstmn]

Fantastic. Thanks for the detailed response.

A clarifying follow up question - I'm generating demonstrations using the motionplanning solver in mani_skill.examples.motionplanning.panda.run with this gym env. Will I still need to run mani_skill.trajectory.replay_trajectory like you mentioned to convert the demonstrations to absolute EE pose if I have control_mode="arm_pd_ee_pose" in run.py?

[jmorgan-bdai]

Hey @StoneT2000 a follow up about this - during training i'm using traj["actions"][:, :7] to get the robot base_link frame TCP pose. How do I get this during execution?

I can access the Panda actor from env.unwrapped.scene.articulations["panda"], but I don't see any sort of forward kinematics functionality in Panda for BaseAgent. Thanks!

• jeremy

[StoneT2000]

the panda agent class defines the end effector link as the one called "panda_hand" i think. For convenience if you do env.unwrapped.agent.tcp you get that panda hand link for the panda robot.

Link objects than have a .pose property that you can access which does the forward kinematics already. But this is world frame pose.

probably smth like

agent = env.unwrapped.agent
tcppose = agent.tcp.pose * agent.robot.pose

to then get robot base frame tcp pose

[jstmn]

OK got it, makes sense.

I ended up adding this to get_proprioception():

        world__T__ee = self.robot.links_map[self.ee_link_name].pose
        obs["world__T__ee"] = world__T__ee.to_transformation_matrix()

[jstmn]

thanks!