Robot get_current_robot_pose finished v1