Fix ompl so it only simplifies when requested (#562)

tesseract-robotics · Jan 3, 2025 · ad51f03 · ad51f03
1 parent bd6b13e
commit ad51f03
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 16 deletions.
diff --git 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<bool, std::string> 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 if 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");
 }

diff --git a/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp b/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp
@@ -224,8 +224,8 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit)  // NOLINT
   }
 
   EXPECT_TRUE(&planner_response);
-  EXPECT_EQ(planner_response.results.getMoveInstructionCount(), 21);  // 10 per segment + start a instruction
-  EXPECT_EQ(planner_response.results.size(), 21);
+  EXPECT_GE(planner_response.results.getMoveInstructionCount(), 21);  // 10 per segment + start a instruction
+  EXPECT_GE(planner_response.results.size(), 21);
   EXPECT_TRUE(wp1.getPosition().isApprox(
       getJointPosition(planner_response.results.getFirstMoveInstruction()->getWaypoint()), 1e-5));
 
@@ -479,7 +479,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit)  // NOLINT
   }
 
   EXPECT_TRUE(&planner_response);
-  EXPECT_EQ(planner_response.results.getMoveInstructionCount(), 11);
+  EXPECT_GE(planner_response.results.getMoveInstructionCount(), 11);
   EXPECT_TRUE(wp2.getPosition().isApprox(
       getJointPosition(planner_response.results.getLastMoveInstruction()->getWaypoint()), 1e-5));