Generate motion with more strict collision check and add test method on real robot