Working StartPose and GoalPose