Added more general computeCartesianPath, takes vector of waypoints