From 98c2a3a9c6b8f77b7064812d72dc006ea4349aa5 Mon Sep 17 00:00:00 2001 From: Tyler Marr <41449746+marrts@users.noreply.github.com> Date: Mon, 3 Jun 2024 13:45:39 -0500 Subject: [PATCH] Better debugging feedback on failed Descartes plan (#401) Co-authored-by: Levi Armstrong --- .github/workflows/windows_dependencies.repos | 2 +- dependencies.repos | 2 +- .../descartes/descartes_collision.h | 4 +- .../descartes/descartes_robot_sampler.h | 5 ++ .../impl/descartes_robot_sampler.hpp | 59 ++++++++++++++++++- .../descartes/src/descartes_collision.cpp | 4 +- 6 files changed, 67 insertions(+), 9 deletions(-) diff --git a/.github/workflows/windows_dependencies.repos b/.github/workflows/windows_dependencies.repos index 073e16d032..898e415749 100644 --- a/.github/workflows/windows_dependencies.repos +++ b/.github/workflows/windows_dependencies.repos @@ -13,7 +13,7 @@ - git: local-name: descartes_light uri: https://github.com/swri-robotics/descartes_light.git - version: 0.4.2 + version: 0.4.3 - git: local-name: opw_kinematics uri: https://github.com/Jmeyer1292/opw_kinematics.git diff --git a/dependencies.repos b/dependencies.repos index 5c840941f1..1dfaf0c299 100644 --- a/dependencies.repos +++ b/dependencies.repos @@ -5,7 +5,7 @@ - git: local-name: descartes_light uri: https://github.com/swri-robotics/descartes_light.git - version: 0.4.2 + version: 0.4.3 - git: local-name: ifopt uri: https://github.com/ethz-adrl/ifopt.git diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_collision.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_collision.h index fb985cbbf7..a2e2426ebd 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_collision.h +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_collision.h @@ -74,9 +74,9 @@ class DescartesCollision /** * @brief This check is the provided solution passes the collision test defined by this class * @param pos The joint values array to validate - * @return True if passes collision test, otherwise false + * @return ContactResultMap containing any contacts for the given solution */ - bool validate(const Eigen::Ref& pos); + tesseract_collision::ContactResultMap validate(const Eigen::Ref& pos); /** * @brief This gets the distance to the closest object diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_robot_sampler.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_robot_sampler.h index 7347e8997b..0d14cc9a90 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_robot_sampler.h +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_robot_sampler.h @@ -67,6 +67,8 @@ class DescartesRobotSampler : public descartes_light::WaypointSampler std::vector> sample() const override; + void print(std::ostream& os) const override; + private: /** @brief The target pose working frame */ std::string target_working_frame_; @@ -103,6 +105,9 @@ class DescartesRobotSampler : public descartes_light::WaypointSampler /** @brief Should redundant solutions be used */ bool use_redundant_joint_solutions_{ false }; + + /** @brief String message to print out with details about planning failure */ + mutable std::string error_string_; }; using DescartesRobotSamplerF = DescartesRobotSampler; diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_robot_sampler.hpp b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_robot_sampler.hpp index 63c53e24f1..4f16c431da 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_robot_sampler.hpp +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_robot_sampler.hpp @@ -76,10 +76,15 @@ std::vector> DescartesRobotSampler> samples; - for (const auto& pose : target_poses) + for (std::size_t i = 0; i < target_poses.size(); i++) { + const auto& pose = target_poses[i]; + // Get the transformation to the kinematic tip link Eigen::Isometry3d target_pose = pose * tcp_offset_.inverse(); @@ -90,9 +95,16 @@ std::vector> DescartesRobotSamplergetJointNames(), + static_cast(ik_solutions.size())); + + found_ik_sol = true; + // Check each individual joint solution - for (const auto& sol : ik_solutions) + for (std::size_t j = 0; j < ik_solutions.size(); j++) { + const auto& sol = ik_solutions[j]; + if ((is_valid_ != nullptr) && !(*is_valid_)(sol)) continue; @@ -103,8 +115,19 @@ std::vector> DescartesRobotSamplervalidate(sol)) + tesseract_collision::ContactResultMap coll_results = collision_->validate(sol); + if (coll_results.empty()) + { samples.push_back(descartes_light::StateSample{ state, 0.0 }); + } + else if (console_bridge::getLogLevel() == console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG) + { + tesseract_collision::ContactTrajectoryStepResults step_contacts(static_cast(j), sol, sol, 1); + tesseract_collision::ContactTrajectorySubstepResults substep_contacts(1, sol); + substep_contacts.contacts = coll_results; + step_contacts.substeps[0] = substep_contacts; + traj_contacts.steps[j] = step_contacts; + } } else { @@ -112,10 +135,29 @@ std::vector> DescartesRobotSampler{ state, cost }); } } + + if (console_bridge::getLogLevel() == console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG) + { + error_string_stream << "For sample " << i << " " << ik_solutions.size() + << " IK solutions were found, with a collision summary of:" << std::endl; + error_string_stream << traj_contacts.trajectoryCollisionResultsTable().str(); + } } if (samples.empty()) + { + std::stringstream ss; + ss << "Descartes vertex failure: "; + if (!found_ik_sol) + ss << "No IK solutions were found. "; + else + ss << "All IK solutions found were in collision or invalid. "; + ss << target_poses.size() << " samples tried." << std::endl; + if (found_ik_sol) + ss << error_string_stream.str(); + error_string_ = ss.str(); return samples; + } if (allow_collision_) { @@ -166,9 +208,20 @@ std::vector> DescartesRobotSampler +void DescartesRobotSampler::print(std::ostream& os) const +{ + os << "Working Frame: " << target_working_frame_ << ", TCP Frame: " << tcp_frame_ << "\n"; + os << "Target Pose:\n" << target_pose_.matrix() << "\n"; + os << "TCP Offset:\n" << tcp_offset_.matrix() << "\n"; + os << "Error string:\n" << error_string_; +} + } // namespace tesseract_planning #endif // TESSERACT_MOTION_PLANNERS_DESCARTES_ROBOT_SAMPLER_HPP diff --git a/tesseract_motion_planners/descartes/src/descartes_collision.cpp b/tesseract_motion_planners/descartes/src/descartes_collision.cpp index 434c234a65..0326c3d0f6 100644 --- a/tesseract_motion_planners/descartes/src/descartes_collision.cpp +++ b/tesseract_motion_planners/descartes/src/descartes_collision.cpp @@ -56,7 +56,7 @@ DescartesCollision::DescartesCollision(const DescartesCollision& collision_inter contact_manager_->applyContactManagerConfig(collision_check_config_.contact_manager_config); } -bool DescartesCollision::validate(const Eigen::Ref& pos) +tesseract_collision::ContactResultMap DescartesCollision::validate(const Eigen::Ref& pos) { // Happens in two phases: // 1. Compute the transform of all objects @@ -67,7 +67,7 @@ bool DescartesCollision::validate(const Eigen::Ref& pos) tesseract_collision::ContactResultMap results; tesseract_environment::checkTrajectoryState(results, *contact_manager_, state, config); - return results.empty(); + return results; } double DescartesCollision::distance(const Eigen::Ref& pos)