StartPose and GoalPose with samll changes