Plugged in ACADO Toolkit to RRT* code to solve BVPs with dynamical systems. Ready for testing with collsions and without collisions