agent state to 2D pose

❓ Questions and Help

I was trying to convert agent state into a 2D pose tuple (x, y, theta). I followed some existent code, but the result doesn’t seem to be correct.

Here is the code.

position = sim.get_agent_state().position
rot = sim.get_agent_state().rotation
pose = [0.0, 0.0, 0.0]
pose[0] = position[0] # for x
pose[1] = position[2] # for y

direction_vector = np.array([0, 0, -1])
heading_vector = quaternion_rotate_vector(rot.inverse(), direction_vector)
pose[2] = cartesian_to_polar(-heading_vector[2], heading_vector[0])[1] # for theta

[erikwijmans]

You’ll need to correct the angle like here: https://github.com/facebookresearch/habitat-api/issues/109#issuecomment-499668103

[erikwijmans]

Can you give more information about why “the result doesn’t seem to be correct”? This is the 2D pose of the agent in some coordinate system, but maybe not the coordinate system you are expecting.