From f9d0bddec1da2b3eb8634f477e82abf0945e3756 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sat, 18 Jan 2025 19:22:33 -0600 Subject: [PATCH] Fix incorrect evaluator type check in LVSDiscrete collision evaluator --- .../continuous_collision_evaluators.cpp | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp index 3e09e517..a726e355 100644 --- a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp +++ b/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp @@ -48,8 +48,12 @@ LVSContinuousCollisionEvaluator::LVSContinuousCollisionEvaluator( , collision_config_(std::move(collision_config)) , dynamic_environment_(dynamic_environment) { - assert(collision_config_->type == tesseract_collision::CollisionEvaluatorType::CONTINUOUS || - collision_config_->type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS); + if (collision_config_->type == tesseract_collision::CollisionEvaluatorType::DISCRETE || + collision_config_->type == tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE) + { + throw std::runtime_error("LVSContinuousCollisionEvaluator, has been configured with " + "CollisionEvaluatorType::DISCRETE or CollisionEvaluatorType::LVS_DISCRETE"); + } manip_active_link_names_ = manip_->getActiveLinkNames(); @@ -290,6 +294,13 @@ LVSDiscreteCollisionEvaluator::LVSDiscreteCollisionEvaluator( , collision_config_(std::move(collision_config)) , dynamic_environment_(dynamic_environment) { + if (collision_config_->type == tesseract_collision::CollisionEvaluatorType::CONTINUOUS || + collision_config_->type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS) + { + throw std::runtime_error("LVSDiscreteCollisionEvaluator, has been configured with " + "CollisionEvaluatorType::CONTINUOUS or CollisionEvaluatorType::LVS_CONTINUOUS"); + } + manip_active_link_names_ = manip_->getActiveLinkNames(); // If the environment is not expected to change, then the cloned state solver may be used each time. @@ -453,7 +464,7 @@ void LVSDiscreteCollisionEvaluator::CalcCollisionsHelper(tesseract_collision::Co // the longest valid segment length. const double dist = (dof_vals1 - dof_vals0).norm(); long cnt = 2; - if (collision_config_->type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS && + if (collision_config_->type == tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE && dist > collision_config_->longest_valid_segment_length) { // Calculate the number state to interpolate