descartes bug fixes

Author: mpowelson

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/core/utils.h

@@ -262,7 +262,7 @@ inline bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMa
   if (config.type != tesseract_collision::CollisionEvaluatorType::CONTINUOUS &&
       config.type != tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS)
     throw std::runtime_error("contactCheckProgram was given an CollisionEvaluatorType that is inconsistent with the "
-                             "ContactManager type");
+                             "ContactManager type (Continuous)");
   bool found = false;
 
   if (config.type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS)
@@ -442,7 +442,7 @@ inline bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMa
   if (config.type != tesseract_collision::CollisionEvaluatorType::DISCRETE &&
       config.type != tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE)
     throw std::runtime_error("contactCheckProgram was given an CollisionEvaluatorType that is inconsistent with the "
-                             "ContactManager type");
+                             "ContactManager type (Discrete)");
   bool found = false;
 
   if (config.type == tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE)

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/impl/descartes_collision_edge_evaluator.hpp

@@ -94,8 +94,9 @@ std::pair<bool, FloatType> DescartesCollisionEdgeEvaluator<FloatType>::considerE
   if (!discrete_in_contact && !continuous_in_contact)
     return std::make_pair(true, 0);
 
-  // TODO: Update this
-  double collision_safety_margin_ = collision_check_config_.collision_margin_data.getMaxCollisionMargin();
+  // TODO: Update this to consider link pairs
+  FloatType collision_safety_margin_ =
+      static_cast<FloatType>(collision_check_config_.collision_margin_data.getMaxCollisionMargin());
 
   if (!discrete_in_contact && continuous_in_contact && allow_collision_)
     return std::make_pair(true, collision_safety_margin_ - continuous_results.begin()->begin()->second[0].distance);
@@ -146,6 +147,11 @@ bool DescartesCollisionEdgeEvaluator<FloatType>::continuousCollisionCheck(
   }
   mutex_.unlock();
   tesseract_collision::CollisionCheckConfig config = collision_check_config_;
+  if (config.type == tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE ||
+      config.type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS)
+    config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
+  else
+    config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS;
   config.contact_request.type =
       (find_best) ? tesseract_collision::ContactTestType::CLOSEST : tesseract_collision::ContactTestType::FIRST;
 
@@ -180,6 +186,11 @@ bool DescartesCollisionEdgeEvaluator<FloatType>::discreteCollisionCheck(
   mutex_.unlock();
 
   tesseract_collision::CollisionCheckConfig config = collision_check_config_;
+  if (config.type == tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE ||
+      config.type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS)
+    config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
+  else
+    config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE;
   config.contact_request.type =
       (find_best) ? tesseract_collision::ContactTestType::CLOSEST : tesseract_collision::ContactTestType::FIRST;