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
remove extra conditon