Previously, math_pose_invert would apply a multiplication in the wrong order. This lead to the position of the 'original' pose being rotated. This patch fixes that, and adds a unit test to check this case.
