Fix ifopt continuous collision evaluator to distinguish CONTINUOUS an…

…d LVS_CONTINUOUS (#342)
tesseract-robotics · Aug 1, 2023 · 70d64d3 · 70d64d3
1 parent c458814
commit 70d64d3
Show file tree

Hide file tree

Showing 9 changed files with 17 additions and 4 deletions.
diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp
@@ -39,6 +39,10 @@ LVSContinuousCollisionEvaluator::LVSContinuousCollisionEvaluator(
   , 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("LVSContinuousCollisionEvaluator, type must be CONTINUOUS or 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.
@@ -199,7 +203,8 @@ void LVSContinuousCollisionEvaluator::CalcCollisionsHelper(const Eigen::Ref<cons
     removeInvalidContactResults(pair.second, data); /** @todo Should this be removed? levi */
   };
 
-  if (dist > collision_config_->longest_valid_segment_length)
+  if (collision_config_->type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS &&
+      dist > collision_config_->longest_valid_segment_length)
   {
     // Calculate the number state to interpolate
     long cnt = static_cast<long>(std::ceil(dist / collision_config_->longest_valid_segment_length)) + 1;
@@ -435,7 +440,8 @@ void LVSDiscreteCollisionEvaluator::CalcCollisionsHelper(const Eigen::Ref<const
   // the longest valid segment length.
   double dist = (dof_vals1 - dof_vals0).norm();
   long cnt = 2;
-  if (dist > collision_config_->longest_valid_segment_length)
+  if (collision_config_->type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS &&
+      dist > collision_config_->longest_valid_segment_length)
   {
     // Calculate the number state to interpolate
     cnt = static_cast<long>(std::ceil(dist / collision_config_->longest_valid_segment_length)) + 1;

diff --git a/trajopt_ifopt/test/cast_cost_unit.cpp b/trajopt_ifopt/test/cast_cost_unit.cpp
@@ -128,6 +128,7 @@ TEST_F(CastTest, boxes)  // NOLINT
   double margin_coeff = 1;
   double margin = 0.02;
   auto trajopt_collision_config = std::make_shared<trajopt_ifopt::TrajOptCollisionConfig>(margin, margin_coeff);
+  trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
   trajopt_collision_config->collision_margin_buffer = 0.05;
 
   // 4) Add constraints

diff --git a/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp b/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp
@@ -128,6 +128,7 @@ void runContinuousGradientTest(const Environment::Ptr& env, double coeff)
   double margin_coeff = coeff;
   double margin = 0.02;
   auto trajopt_collision_config = std::make_shared<trajopt_ifopt::TrajOptCollisionConfig>(margin, margin_coeff);
+  trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
   trajopt_collision_config->collision_margin_buffer = 0.05;
 
   auto collision_cache = std::make_shared<trajopt_ifopt::CollisionCache>(100);

diff --git a/trajopt_optimizers/trajopt_sqp/test/benchmarks/solve_benchmarks.cpp b/trajopt_optimizers/trajopt_sqp/test/benchmarks/solve_benchmarks.cpp
@@ -260,8 +260,8 @@ static void BM_TRAJOPT_IFOPT_PLANNING_SOLVE(benchmark::State& state, Environment
     double margin_coeff = 20;
     double margin = 0.025;
     auto trajopt_collision_config = std::make_shared<trajopt_ifopt::TrajOptCollisionConfig>(margin, margin_coeff);
+    trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS;
     trajopt_collision_config->collision_margin_buffer = 0.01;
-    trajopt_collision_config->longest_valid_segment_length = 0.02;
 
     // Add costs
     {

diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp
@@ -167,6 +167,7 @@ void runCastAttachedLinkWithGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_probl
   double margin_coeff = 20;
   double margin = 0.3;
   auto trajopt_collision_config = std::make_shared<trajopt_ifopt::TrajOptCollisionConfig>(margin, margin_coeff);
+  trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
   trajopt_collision_config->collision_margin_buffer = 0.05;
 
   // 4) Add constraints
@@ -286,6 +287,7 @@ void runCastAttachedLinkWithoutGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_pr
   double margin_coeff = 10;
   double margin = 0.02;
   auto trajopt_collision_config = std::make_shared<trajopt_ifopt::TrajOptCollisionConfig>(margin, margin_coeff);
+  trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
   trajopt_collision_config->collision_margin_buffer = 0.01;
   trajopt_collision_config->longest_valid_segment_length = 0.05;
 

diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp
@@ -160,6 +160,7 @@ void runCastOctomapTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Env
   double margin_coeff = 10;
   double margin = 0.02;
   auto trajopt_collision_config = std::make_shared<trajopt_ifopt::TrajOptCollisionConfig>(margin, margin_coeff);
+  trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
   trajopt_collision_config->collision_margin_buffer = 0.05;
 
   // 4) Add constraints

diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp
@@ -115,6 +115,7 @@ void runCastTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Environmen
   double margin_coeff = 10;
   double margin = 0.02;
   auto trajopt_collision_config = std::make_shared<trajopt_ifopt::TrajOptCollisionConfig>(margin, margin_coeff);
+  trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
   trajopt_collision_config->collision_margin_buffer = 0.05;
 
   // 4) Add constraints

diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp
@@ -144,6 +144,7 @@ void runCastWorldTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Envir
   double margin_coeff = 10;
   double margin = 0.02;
   auto trajopt_collision_config = std::make_shared<trajopt_ifopt::TrajOptCollisionConfig>(margin, margin_coeff);
+  trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
   trajopt_collision_config->collision_margin_buffer = 0.05;
 
   // 4) Add constraints

diff --git a/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp
@@ -120,8 +120,8 @@ void runPlanningTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Enviro
   double margin_coeff = 20;
   double margin = 0.025;
   auto trajopt_collision_config = std::make_shared<trajopt_ifopt::TrajOptCollisionConfig>(margin, margin_coeff);
+  trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS;
   trajopt_collision_config->collision_margin_buffer = 0.01;
-  trajopt_collision_config->longest_valid_segment_length = 0.02;
 
   // Add costs
   {