added setYaw and fix motion