fix copy-pos problem and wait for testing on real robot