diff --git a/tesseract_motion_planners/examples/freespace_example.cpp b/tesseract_motion_planners/examples/freespace_example.cpp index 7490c45c6ba..59e4aa56310 100644 --- a/tesseract_motion_planners/examples/freespace_example.cpp +++ b/tesseract_motion_planners/examples/freespace_example.cpp @@ -29,25 +29,22 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include - #include - #include - -#include +#include +#include +#include +#include +#include +// OMPL #include - +#include +#include +// TrajOpt #include #include #include -#include -#include -#include - -#include -#include - using namespace tesseract_planning; using namespace tesseract_planning::profile_ns; @@ -89,7 +86,7 @@ int main(int /*argc*/, char** /*argv*/) tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400.srdf"); env->init(urdf_path, srdf_path, locator); - // Dynamically load ignition visualizer if exist + // Dynamically load ignition visualizer if it exists tesseract_visualization::VisualizationLoader loader; auto plotter = loader.get(); @@ -132,18 +129,18 @@ int main(int /*argc*/, char** /*argv*/) } // Create Profiles - auto ompl_plan_profile = std::make_shared(); - auto trajopt_plan_profile = std::make_shared(); - auto trajopt_composite_profile = std::make_shared(); - - // Create a seed - CompositeInstruction seed = generateSeed(program, cur_state, env); + auto ompl_composite_profile = std::make_shared(); + auto ompl_planner_profile = std::make_shared(); + ompl_planner_profile->params.planners.push_back(std::make_shared()); // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "DEFAULT", ompl_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile>(OMPL_DEFAULT_NAMESPACE, "DEFAULT", ompl_planner_profile); + profiles->addProfile>( + OMPL_DEFAULT_NAMESPACE, "DEFAULT", ompl_composite_profile); + + // Create a seed + CompositeInstruction seed = generateSeed(program, cur_state, env); // Create Planning Request PlannerRequest request; @@ -166,7 +163,15 @@ int main(int /*argc*/, char** /*argv*/) plotter->plotTrajectory(toJointTrajectory(ompl_response.results), *state_solver); } - // Update Seed + // Create the TrajOpt profiles + auto trajopt_plan_profile = std::make_shared(); + auto trajopt_composite_profile = std::make_shared(); + + // Add the TrajOpt profiles to the dictionary + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + + // Update the seed to be the OMPL trajecory request.seed = ompl_response.results; // Solve TrajOpt Plan