exit with -1 when error (for travis)
EOD
avoid collision between two robots seems ok
add collision with the environment/ big issue when start optim in collision/ need to know penetration distance