Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix ifopt continuous collision evaluator to distinguish CONTINUOUS and LVS_CONTINUOUS #342

Merged
merged 4 commits into from
Aug 1, 2023
Merged

Fix ifopt continuous collision evaluator to distinguish CONTINUOUS and LVS_CONTINUOUS #342

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
a4b4dda
Fix ifopt continuous collision evaluator to distinguish CONTINUOUS an…
Levi-Armstrong Jul 31, 2023
faf7f5b
Fix unit tests
Levi-Armstrong Jul 31, 2023
563cfa9
Fix benchmarks
Levi-Armstrong Jul 31, 2023
8a27303
Fix unit tests
Levi-Armstrong Jul 31, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 8 additions & 2 deletions trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,10 @@ LVSContinuousCollisionEvaluator::LVSContinuousCollisionEvaluator(
, collision_config_(std::move(collision_config))
, dynamic_environment_(dynamic_environment)
{
if (collision_config_->type != tesseract_collision::CollisionEvaluatorType::CONTINUOUS &&
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is this check necessary? You don't do this in the LVSDiscreteCollisionEvaluator or the SingleTimestepCollisionEvaluator either. Not that it's bad to have an extra check, but for performance reasons. Or add the check everywhere.

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.
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down
1 change: 1 addition & 0 deletions trajopt_ifopt/test/cast_cost_unit.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions trajopt_ifopt/test/continuous_collision_gradient_unit.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion trajopt_optimizers/trajopt_sqp/test/benchmarks/solve_benchmarks.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down
2 changes: 2 additions & 0 deletions trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;

Expand Down
1 change: 1 addition & 0 deletions trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down
Loading