Better debugging feedback on failed Descartes plan (#401)

Co-authored-by: Levi Armstrong <[email protected]>

.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

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

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<const Eigen::VectorXd>& pos);
+  tesseract_collision::ContactResultMap validate(const Eigen::Ref<const Eigen::VectorXd>& pos);
 
   /**
    * @brief This gets the distance to the closest object

tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_robot_sampler.h

@@ -67,6 +67,8 @@ class DescartesRobotSampler : public descartes_light::WaypointSampler<FloatType>
 
   std::vector<descartes_light::StateSample<FloatType>> 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<FloatType>
 
   /** @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<float>;

tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_robot_sampler.hpp

@@ -76,10 +76,15 @@ std::vector<descartes_light::StateSample<FloatType>> DescartesRobotSampler<Float
   // Generate all possible Cartesian poses
   tesseract_common::VectorIsometry3d target_poses = target_pose_sampler_(target_pose_);
 
+  bool found_ik_sol = false;
+  std::stringstream error_string_stream;
+
   // Generate the IK solutions for those poses
   std::vector<descartes_light::StateSample<FloatType>> 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<descartes_light::StateSample<FloatType>> DescartesRobotSampler<Float
     if (ik_solutions.empty())
       continue;
 
+    tesseract_collision::ContactTrajectoryResults traj_contacts(manip_->getJointNames(),
+                                                                static_cast<int>(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,19 +115,49 @@ std::vector<descartes_light::StateSample<FloatType>> DescartesRobotSampler<Float
       }
       else if (!allow_collision_)
       {
-        if (collision_->validate(sol))
+        tesseract_collision::ContactResultMap coll_results = collision_->validate(sol);
+        if (coll_results.empty())
+        {
           samples.push_back(descartes_light::StateSample<FloatType>{ state, 0.0 });
+        }
+        else if (console_bridge::getLogLevel() == console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG)
+        {
+          tesseract_collision::ContactTrajectoryStepResults step_contacts(static_cast<int>(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
       {
         const FloatType cost = static_cast<FloatType>(collision_->distance(sol));
         samples.push_back(descartes_light::StateSample<FloatType>{ 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<descartes_light::StateSample<FloatType>> DescartesRobotSampler<Float
     samples.insert(samples.end(), redundant_samples.begin(), redundant_samples.end());
   }
 
+  error_string_ = "Found at least 1 valid solution";
+
   return samples;
 }
 
+template <typename FloatType>
+void DescartesRobotSampler<FloatType>::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

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<const Eigen::VectorXd>& pos)
+tesseract_collision::ContactResultMap DescartesCollision::validate(const Eigen::Ref<const Eigen::VectorXd>& pos)
 {
   // Happens in two phases:
   // 1. Compute the transform of all objects
@@ -67,7 +67,7 @@ bool DescartesCollision::validate(const Eigen::Ref<const Eigen::VectorXd>& pos)
 
   tesseract_collision::ContactResultMap results;
   tesseract_environment::checkTrajectoryState(results, *contact_manager_, state, config);
-  return results.empty();
+  return results;
 }
 
 double DescartesCollision::distance(const Eigen::Ref<const Eigen::VectorXd>& pos)