This is incomplete, a better solution has been implemented in RobotPose