From 76e2bc550727b79c1b3d6fd7c1f0b3df91c038e1 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Thu, 2 Jan 2025 19:04:52 -0600 Subject: [PATCH] Fix ompl so it only simplifies when requested --- .../ompl/src/ompl_motion_planner.cpp | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index c31eb93b0c..057b33cd43 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -181,21 +181,9 @@ std::pair parallelPlan(ompl::geometric::SimpleSetup& simple_s if (solver_config.simplify) simple_setup.simplifySolution(); - // Interpolate the path if it shouldn't be simplified and there are currently fewer states than requested + // Interpolate the path there are currently fewer states than requested if (simple_setup.getSolutionPath().getStateCount() < num_output_states) - { simple_setup.getSolutionPath().interpolate(num_output_states); - } - else - { - // Now try to simplify the trajectory to get it under the requested number of output states - // The interpolate function only executes if the current number of states is less than the requested - if (!solver_config.simplify) - simple_setup.simplifySolution(); - - if (simple_setup.getSolutionPath().getStateCount() < num_output_states) - simple_setup.getSolutionPath().interpolate(num_output_states); - } return std::make_pair(true, "SUCCESS"); }