trying to reduce casadi de size
still trying to reduce point explore casadi
about to add option for multiple robots
works with multiple robots now