Remove target number of output states

tesseract-robotics · Dec 13, 2024 · f0bc626 · f0bc626
1 parent 82512cd
commit f0bc626
Show file tree

Hide file tree

Showing 7 changed files with 3 additions and 47 deletions.
diff --git a/tesseract_examples/src/glass_upright_ompl_example.cpp b/tesseract_examples/src/glass_upright_ompl_example.cpp
@@ -277,7 +277,6 @@ bool GlassUprightOMPLExample::run()
   ompl_config->collision_continuous = false;
   ompl_config->collision_check = false;
   ompl_config->simplify = false;
-  ompl_config->n_output_states = 50;
 
   if (use_trajopt_constraint_)
   {

diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h
@@ -100,7 +100,6 @@ class OMPLMotionPlanner : public MotionPlanner
                                      const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip,
                                      const MoveInstructionPoly& start_instruction,
                                      const MoveInstructionPoly& end_instruction,
-                                     int n_output_states,
                                      int index) const;
 };
 

diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_problem.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_problem.h
@@ -119,21 +119,9 @@ struct OMPLProblem
   /** @brief The max number of solutions. If max solutions are hit it will exit even if other threads are running. */
   int max_solutions = 10;
 
-  /**
-   * @brief Simplify trajectory.
-   *
-   * Note: If set to true it ignores n_output_states and returns the simplest trajectory.
-   */
+  /** @brief Simplify trajectory. */
   bool simplify = false;
 
-  /**
-   * @brief Number of states in the output trajectory
-   *   Note: This is ignored if the simplify is set to true.
-   *   Note: The trajectory can be longer if original trajectory is longer and reducing the number of states causes
-   *         the solution to be invalid.
-   */
-  int n_output_states = 20;
-
   /**
    * @brief This uses all available planning time to create the most optimized trajectory given the objective function.
    *

diff --git a/..._planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h b/..._planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h
@@ -78,11 +78,7 @@ class OMPLDefaultPlanProfile : public OMPLPlanProfile
   /** @brief The max number of solutions. If max solutions are hit it will exit even if other threads are running. */
   int max_solutions = 10;
 
-  /**
-   * @brief Simplify trajectory.
-   *
-   * Note: If set to true it ignores n_output_states and returns the simplest trajectory.
-   */
+  /** @brief Simplify trajectory. */
   bool simplify = false;
 
   /**

diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp
@@ -215,23 +215,6 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const
     {
       p->simple_setup->simplifySolution();
     }
-    else
-    {
-      // Interpolate the path if it shouldn't be simplified and there are currently fewer states than requested
-      auto num_output_states = static_cast<unsigned>(p->n_output_states);
-      if (p->simple_setup->getSolutionPath().getStateCount() < num_output_states)
-      {
-        p->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
-        p->simple_setup->simplifySolution();
-        if (p->simple_setup->getSolutionPath().getStateCount() < num_output_states)
-          p->simple_setup->getSolutionPath().interpolate(num_output_states);
-      }
-    }
   }
 
   // Flatten the results to make them easier to process
@@ -246,7 +229,6 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const
     tesseract_common::TrajArray traj = p->getTrajectory();
     assert(checkStartState(p->simple_setup->getProblemDefinition(), traj.row(0), p->extractor));
     assert(checkGoalState(p->simple_setup->getProblemDefinition(), traj.bottomRows(1).transpose(), p->extractor));
-    assert(traj.rows() >= p->n_output_states);
 
     const std::vector<std::string> joint_names = p->manip->getJointNames();
     const Eigen::MatrixX2d joint_limits = p->manip->getLimits().joint_limits;
@@ -326,7 +308,6 @@ OMPLMotionPlanner::createSubProblem(const PlannerRequest& request,
                                     const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip,
                                     const MoveInstructionPoly& start_instruction,
                                     const MoveInstructionPoly& end_instruction,
-                                    int n_output_states,
                                     int index) const
 {
   std::vector<std::string> joint_names = manip->getJointNames();
@@ -354,7 +335,6 @@ OMPLMotionPlanner::createSubProblem(const PlannerRequest& request,
   config.problem->contact_checker->setActiveCollisionObjects(active_link_names);
 
   cur_plan_profile->setup(*config.problem);
-  config.problem->n_output_states = n_output_states;
 
   if (end_instruction.getWaypoint().isJointWaypoint() || end_instruction.getWaypoint().isStateWaypoint())
   {
@@ -471,12 +451,10 @@ std::vector<OMPLProblemConfig> OMPLMotionPlanner::createProblems(const PlannerRe
 
   // Transform plan instructions into ompl problem
   int index = 0;
-  int num_output_states = 1;
   MoveInstructionPoly start_instruction = move_instructions.front().get().as<MoveInstructionPoly>();
 
   for (std::size_t i = 1; i < move_instructions.size(); ++i)
   {
-    ++num_output_states;
     const auto& instruction = move_instructions[i].get();
     assert(instruction.isMoveInstruction());
     const auto& move_instruction = instruction.as<MoveInstructionPoly>();
@@ -485,9 +463,8 @@ std::vector<OMPLProblemConfig> OMPLMotionPlanner::createProblems(const PlannerRe
         (waypoint.isJointWaypoint() && waypoint.as<JointWaypointPoly>().isConstrained()))
     {
       problems.push_back(createSubProblem(
-          request, composite_mi, manip, start_instruction, move_instruction, num_output_states, index++));
+          request, composite_mi, manip, start_instruction, move_instruction, index++));
       start_instruction = move_instruction;
-      num_output_states = 1;
     }
   }
 

diff --git a/tesseract_motion_planners/ompl/test/ompl_constrained_planner_tests.cpp b/tesseract_motion_planners/ompl/test/ompl_constrained_planner_tests.cpp
@@ -159,7 +159,6 @@ TEST(OMPLConstraintPlanner, OMPLConstraintPlannerUnit)  // NOLINT
   ompl_config->collision_continuous = true;
   ompl_config->collision_check = true;
   ompl_config->simplify = false;
-  ompl_config->n_output_states = 50;
   ompl_config->constraint = std::make_shared<GlassUprightConstraint>(normal, kin);
 
   // Set the planner configuration

diff --git a/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp b/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp
@@ -566,7 +566,6 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit)  // NOLINT
 //  ompl_config->collision_continuous = true;
 //  ompl_config->collision_check = true;
 //  ompl_config->simplify = false;
-//  ompl_config->n_output_states = 50;
 
 //  // Set the planner configuration
 //  ompl_planner.setConfiguration(ompl_config);
@@ -578,7 +577,6 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit)  // NOLINT
 //    CONSOLE_BRIDGE_logError("CI Error: %s", status.message().c_str());
 //  }
 //  EXPECT_TRUE(&status);
-//  EXPECT_EQ(ompl_planning_response.joint_trajectory.trajectory.rows(), ompl_config->n_output_states);
 
 //  // Check for start state in collision error
 //  swp = { 0, 0.7, 0.0, 0, 0.0, 0, 0.0 };