From 70d64d39d89a2677ffca271461a3472afbb0b4bf Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 31 Jul 2023 19:00:23 -0500 Subject: [PATCH] Fix ifopt continuous collision evaluator to distinguish CONTINUOUS and LVS_CONTINUOUS (#342) --- .../collision/continuous_collision_evaluators.cpp | 10 ++++++++-- trajopt_ifopt/test/cast_cost_unit.cpp | 1 + .../test/continuous_collision_gradient_unit.cpp | 1 + .../trajopt_sqp/test/benchmarks/solve_benchmarks.cpp | 2 +- .../trajopt_sqp/test/cast_cost_attached_unit.cpp | 2 ++ .../trajopt_sqp/test/cast_cost_octomap_unit.cpp | 1 + trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp | 1 + .../trajopt_sqp/test/cast_cost_world_unit.cpp | 1 + trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp | 2 +- 9 files changed, 17 insertions(+), 4 deletions(-) diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp index 396a41ef..c6b090ab 100644 --- 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 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(std::ceil(dist / collision_config_->longest_valid_segment_length)) + 1; @@ -435,7 +440,8 @@ void LVSDiscreteCollisionEvaluator::CalcCollisionsHelper(const Eigen::Ref 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(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 index 8c174fab..df2b5bcd 100644 --- 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(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 index ad530e48..f59a6d33 100644 --- 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(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(100); diff --git a/trajopt_optimizers/trajopt_sqp/test/benchmarks/solve_benchmarks.cpp b/trajopt_optimizers/trajopt_sqp/test/benchmarks/solve_benchmarks.cpp index e053172b..3edb51c1 100644 --- 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(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 index f4d768fa..fcfeb0ee 100644 --- 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(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(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 index 361fc872..4983e9f3 100644 --- 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(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 index e12b9a0f..8d207c88 100644 --- 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(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 index 1416f311..ca3d5166 100644 --- 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(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 index 0abee9b2..cceb3e07 100644 --- 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(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 {