Switch over from free space motion to constraint motion