From b5c5d257776f667296e3b9692fe94aae37285673 Mon Sep 17 00:00:00 2001 From: Tyler Marr <41449746+marrts@users.noreply.github.com> Date: Wed, 21 Feb 2024 19:33:39 -0600 Subject: [PATCH] Feat/more verbose planning failures (#440) --- .../tesseract_motion_planners/core/planner.h | 3 +++ .../core/src/planner.cpp | 12 ++++++++-- .../impl/descartes_motion_planner.hpp | 2 +- .../ompl/src/ompl_motion_planner.cpp | 24 ++++++++++++------- .../simple/src/simple_motion_planner.cpp | 11 +++++---- .../trajopt/src/trajopt_motion_planner.cpp | 21 +++++++++------- .../trajopt/src/trajopt_utils.cpp | 18 +++++++++----- .../src/trajopt_ifopt_motion_planner.cpp | 7 +++--- .../planning/nodes/motion_planner_task.hpp | 3 +-- 9 files changed, 64 insertions(+), 37 deletions(-) diff --git a/tesseract_motion_planners/core/include/tesseract_motion_planners/core/planner.h b/tesseract_motion_planners/core/include/tesseract_motion_planners/core/planner.h index 128e6d96f56..dbe6ac2eb02 100644 --- a/tesseract_motion_planners/core/include/tesseract_motion_planners/core/planner.h +++ b/tesseract_motion_planners/core/include/tesseract_motion_planners/core/planner.h @@ -72,6 +72,9 @@ class MotionPlanner /** @brief Check planning request */ static bool checkRequest(const PlannerRequest& request); + /** @brief Check planning request and give reason for failure */ + static bool checkRequest(const PlannerRequest& request, std::string& reason); + /** @brief Assign a solution to the move instruction */ static void assignSolution(MoveInstructionPoly& mi, const std::vector& joint_names, diff --git a/tesseract_motion_planners/core/src/planner.cpp b/tesseract_motion_planners/core/src/planner.cpp index b401af7e489..862acc81c01 100644 --- a/tesseract_motion_planners/core/src/planner.cpp +++ b/tesseract_motion_planners/core/src/planner.cpp @@ -37,17 +37,25 @@ MotionPlanner::MotionPlanner(std::string name) : name_(std::move(name)) const std::string& MotionPlanner::getName() const { return name_; } bool MotionPlanner::checkRequest(const PlannerRequest& request) +{ + std::string reason; + return checkRequest(request, reason); +} + +bool MotionPlanner::checkRequest(const PlannerRequest& request, std::string& reason) { // Check that parameters are valid if (request.env == nullptr) { - CONSOLE_BRIDGE_logError("In TrajOptPlannerUniversalConfig: tesseract is a required parameter and has not been set"); + reason = "PlannerRequest environment is nullptr"; + CONSOLE_BRIDGE_logError(reason.c_str()); return false; } if (request.instructions.empty()) { - CONSOLE_BRIDGE_logError("TrajOptPlannerUniversalConfig requires at least one instruction"); + reason = "PlannerRequest instruction is empty"; + CONSOLE_BRIDGE_logError(reason.c_str()); return false; } diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp index 7b9654ca898..0221757fd68 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp @@ -48,7 +48,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include constexpr auto SOLUTION_FOUND{ "Found valid solution" }; -constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input" }; +constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input: " }; constexpr auto ERROR_FAILED_TO_BUILD_GRAPH{ "Failed to build graph" }; constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution" }; diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index 74dc5539c31..5bd42dfefae 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -46,8 +46,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include constexpr auto SOLUTION_FOUND{ "Found valid solution" }; -constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input" }; -constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution" }; +constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input: " }; +constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution: " }; namespace tesseract_planning { @@ -100,10 +100,11 @@ bool OMPLMotionPlanner::terminate() PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const { PlannerResponse response; - if (!checkRequest(request)) // NOLINT + std::string reason; + if (!checkRequest(request, reason)) { response.successful = false; - response.message = ERROR_INVALID_INPUT; + response.message = std::string(ERROR_INVALID_INPUT) + reason; return response; } std::vector problems; @@ -121,7 +122,7 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const { CONSOLE_BRIDGE_logError("OMPLPlanner failed to generate problem: %s.", e.what()); response.successful = false; - response.message = ERROR_INVALID_INPUT; + response.message = std::string(ERROR_INVALID_INPUT) + e.what(); return response; } @@ -171,7 +172,8 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const if (!pdef->hasOptimizationObjective()) { - CONSOLE_BRIDGE_logDebug("Terminating early since there is no optimization objective specified"); + reason = "Terminating early since there is no optimization objective specified"; + CONSOLE_BRIDGE_logDebug(reason.c_str()); break; } @@ -180,23 +182,27 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const if (pdef->getOptimizationObjective()->isSatisfied(obj_cost)) { - CONSOLE_BRIDGE_logDebug("Terminating early since solution path satisfies the optimization objective"); + reason = "Terminating early since solution path satisfies the optimization objective"; + CONSOLE_BRIDGE_logDebug(reason.c_str()); break; } if (pdef->getSolutionCount() >= static_cast(p->max_solutions)) { - CONSOLE_BRIDGE_logDebug("Terminating early since %u solutions were generated", p->max_solutions); + reason = "Terminating early since " + std::to_string(p->max_solutions) + " solutions were generated"; + CONSOLE_BRIDGE_logDebug(reason.c_str()); break; } } } + if (ompl::time::now() >= end) + reason = "Exceeded allowed time"; } if (status != ompl::base::PlannerStatus::EXACT_SOLUTION) { response.successful = false; - response.message = ERROR_FAILED_TO_FIND_VALID_SOLUTION; + response.message = std::string(ERROR_FAILED_TO_FIND_VALID_SOLUTION) + reason; return response; } diff --git a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp index 6da5737391b..907c054da53 100644 --- a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp +++ b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp @@ -41,8 +41,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include constexpr auto SOLUTION_FOUND{ "Found valid solution" }; -constexpr auto ERROR_INVALID_INPUT{ "Input to planner is invalid. Check that instructions and seed are compatible" }; -constexpr auto FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution" }; +constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input: " }; +constexpr auto FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution: " }; namespace tesseract_planning { @@ -61,10 +61,11 @@ MotionPlanner::Ptr SimpleMotionPlanner::clone() const { return std::make_shared< PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const { PlannerResponse response; - if (!checkRequest(request)) + std::string reason; + if (!checkRequest(request, reason)) // NOLINT { response.successful = false; - response.message = ERROR_INVALID_INPUT; + response.message = std::string(ERROR_INVALID_INPUT) + reason; return response; } @@ -93,7 +94,7 @@ PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const { CONSOLE_BRIDGE_logError("SimplePlanner failed to generate problem: %s.", e.what()); response.successful = false; - response.message = FAILED_TO_FIND_VALID_SOLUTION; + response.message = std::string(FAILED_TO_FIND_VALID_SOLUTION) + e.what(); return response; } diff --git a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp index 13905c9d6d0..85fa0f65e1a 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp @@ -45,8 +45,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include constexpr auto SOLUTION_FOUND{ "Found valid solution" }; -constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input" }; -constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution" }; +constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input: " }; +constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution: " }; using namespace trajopt; @@ -67,10 +67,11 @@ MotionPlanner::Ptr TrajOptMotionPlanner::clone() const { return std::make_shared PlannerResponse TrajOptMotionPlanner::solve(const PlannerRequest& request) const { PlannerResponse response; - if (!checkRequest(request)) + std::string reason; + if (!checkRequest(request, reason)) { response.successful = false; - response.message = ERROR_INVALID_INPUT; + response.message = std::string(ERROR_INVALID_INPUT) + reason; return response; } @@ -89,7 +90,7 @@ PlannerResponse TrajOptMotionPlanner::solve(const PlannerRequest& request) const { CONSOLE_BRIDGE_logError("TrajOptPlanner failed to generate problem: %s.", e.what()); response.successful = false; - response.message = ERROR_INVALID_INPUT; + response.message = std::string(ERROR_INVALID_INPUT) + e.what(); return response; } @@ -126,8 +127,12 @@ PlannerResponse TrajOptMotionPlanner::solve(const PlannerRequest& request) const if (opt->results().status != sco::OptStatus::OPT_CONVERGED) { response.successful = false; - response.message = ERROR_FAILED_TO_FIND_VALID_SOLUTION; - return response; + response.message = std::string(ERROR_FAILED_TO_FIND_VALID_SOLUTION) + sco::statusToString(opt->results().status); + } + else + { + response.successful = true; + response.message = SOLUTION_FOUND; } const std::vector joint_names = problem->GetKin()->getJointNames(); @@ -154,8 +159,6 @@ PlannerResponse TrajOptMotionPlanner::solve(const PlannerRequest& request) const move_instruction, joint_names, traj.row(static_cast(idx)), request.format_result_as_input); } - response.successful = true; - response.message = SOLUTION_FOUND; return response; } diff --git a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp index bed11643809..0c218964096 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp @@ -192,7 +192,8 @@ trajopt::TermInfo::Ptr createSmoothVelocityTermInfo(int start_index, int end_index, int n_joints, double coeff, trajopt::TermType type) { if ((end_index - start_index) < 1) - throw std::runtime_error("TrajOpt JointVelTermInfo requires at least two states!"); + throw std::runtime_error("TrajOpt JointVelTermInfo requires at least two states, failed with start_index " + + std::to_string(start_index) + " & end_index " + std::to_string(end_index) + "!"); std::shared_ptr jv = std::make_shared(); jv->coeffs = std::vector(static_cast(n_joints), coeff); @@ -210,7 +211,8 @@ trajopt::TermInfo::Ptr createSmoothVelocityTermInfo(int start_index, trajopt::TermType type) { if ((end_index - start_index) < 1) - throw std::runtime_error("TrajOpt JointVelTermInfo requires at least two states!"); + throw std::runtime_error("TrajOpt JointVelTermInfo requires at least two states, failed with start_index " + + std::to_string(start_index) + " & end_index " + std::to_string(end_index) + "!"); std::shared_ptr jv = std::make_shared(); jv->coeffs = std::vector(coeff.data(), coeff.data() + coeff.size()); @@ -226,7 +228,8 @@ trajopt::TermInfo::Ptr createSmoothAccelerationTermInfo(int start_index, int end_index, int n_joints, double coeff, trajopt::TermType type) { if ((end_index - start_index) < 2) - throw std::runtime_error("TrajOpt JointAccTermInfo requires at least three states!"); + throw std::runtime_error("TrajOpt JointAccTermInfo requires at least three states, failed with start_index " + + std::to_string(start_index) + " & end_index " + std::to_string(end_index) + "!"); std::shared_ptr ja = std::make_shared(); ja->coeffs = std::vector(static_cast(n_joints), coeff); @@ -244,7 +247,8 @@ trajopt::TermInfo::Ptr createSmoothAccelerationTermInfo(int start_index, trajopt::TermType type) { if ((end_index - start_index) < 2) - throw std::runtime_error("TrajOpt JointAccTermInfo requires at least three states!"); + throw std::runtime_error("TrajOpt JointAccTermInfo requires at least three states, failed with start_index " + + std::to_string(start_index) + " & end_index " + std::to_string(end_index) + "!"); std::shared_ptr ja = std::make_shared(); ja->coeffs = std::vector(coeff.data(), coeff.data() + coeff.size()); @@ -260,7 +264,8 @@ trajopt::TermInfo::Ptr createSmoothJerkTermInfo(int start_index, int end_index, int n_joints, double coeff, trajopt::TermType type) { if ((end_index - start_index) < 4) - throw std::runtime_error("TrajOpt JointJerkTermInfo requires at least five states!"); + throw std::runtime_error("TrajOpt JointJerkTermInfo requires at least five states, failed with start_index " + + std::to_string(start_index) + " & end_index " + std::to_string(end_index) + "!"); std::shared_ptr jj = std::make_shared(); jj->coeffs = std::vector(static_cast(n_joints), coeff); @@ -278,7 +283,8 @@ trajopt::TermInfo::Ptr createSmoothJerkTermInfo(int start_index, trajopt::TermType type) { if ((end_index - start_index) < 4) - throw std::runtime_error("TrajOpt JointJerkTermInfo requires at least five states!"); + throw std::runtime_error("TrajOpt JointJerkTermInfo requires at least five states, failed with start_index " + + std::to_string(start_index) + " & end_index " + std::to_string(end_index) + "!"); std::shared_ptr jj = std::make_shared(); jj->coeffs = std::vector(coeff.data(), coeff.data() + coeff.size()); diff --git a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp index 302c0daa76a..a2115c229ca 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp @@ -43,7 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include constexpr auto SOLUTION_FOUND{ "Found valid solution" }; -constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input" }; +constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input: " }; constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution" }; using namespace trajopt_ifopt; @@ -66,10 +66,11 @@ MotionPlanner::Ptr TrajOptIfoptMotionPlanner::clone() const PlannerResponse TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request) const { PlannerResponse response; - if (!checkRequest(request)) + std::string reason; + if (!checkRequest(request, reason)) { response.successful = false; - response.message = ERROR_INVALID_INPUT; + response.message = std::string(ERROR_INVALID_INPUT) + reason; return response; } diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp index 654f693eaa7..6b3c03a2e68 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp @@ -157,14 +157,13 @@ class MotionPlannerTask : public TaskComposerTask if (console_bridge::getLogLevel() == console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG) request.verbose = true; PlannerResponse response = planner_->solve(request); + context.data_storage->setData(output_keys_[0], response.results); // -------------------- // Verify Success // -------------------- if (response) { - context.data_storage->setData(output_keys_[0], response.results); - info->return_value = 1; info->color = "green"; info->message = response.message;