Finished integration OCPlanner with embodiment!