fixed setGoalPos constraints