diff --git a/tesseract_examples/src/glass_upright_ompl_example.cpp b/tesseract_examples/src/glass_upright_ompl_example.cpp index d3435c080d..78655292b0 100644 --- 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 index b8f57fb04a..05abe6b391 100644 --- 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& 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 index 5717095400..7e24ef58d4 100644 --- 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/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h index b65cfbef01..f190e076a1 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h +++ b/tesseract_motion_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 index 7bc7324499..61805b1d56 100644 --- 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(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 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& manip, const MoveInstructionPoly& start_instruction, const MoveInstructionPoly& end_instruction, - int n_output_states, int index) const { std::vector 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 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(); 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(); @@ -485,9 +463,8 @@ std::vector OMPLMotionPlanner::createProblems(const PlannerRe (waypoint.isJointWaypoint() && waypoint.as().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 index 16a93675bd..8c694ad72a 100644 --- 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(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 index 09e771152b..81593d81f3 100644 --- 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 };