From a92753dafcf9af8e0bcaa0a915edcee31b7ac629 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 30 Jun 2020 11:31:17 +0200 Subject: [PATCH] add cumulative distance to ClearanceCost --- .../moveit/task_constructor/cost_terms.h | 4 ++- core/src/cost_terms.cpp | 25 +++++++++++++++---- 2 files changed, 23 insertions(+), 6 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index b8ac02c5..c32c192c 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -25,11 +25,12 @@ double PathLengthCost(const SubTrajectory& s); * * \arg interface compute distances using START or END interface of solution * \arg with_world check distances to world objects as well or only look at self-collisions + * \arg cumulative if true, compute clearance as aggregated distance of all bodies * \arg group_property the name of the property which defines the group to look at * */ struct ClearanceCost { - ClearanceCost(Interface::Direction interface = Interface::START, bool with_world = true, + ClearanceCost(Interface::Direction interface = Interface::START, bool with_world = true, bool cumulative = false, std::string group_property = "group") : interface(interface), with_world(with_world), group_property(group_property) {} @@ -37,6 +38,7 @@ struct ClearanceCost Interface::Direction interface; bool with_world; + bool cumulative; std::string group_property; }; } diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 2b3e6b92..e7edd2e2 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -54,7 +54,8 @@ double PathLengthCost(const SubTrajectory& s) { double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) { collision_detection::DistanceRequest request; - request.type = collision_detection::DistanceRequestType::GLOBAL; + request.type = + cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL; const auto& state = (interface == Interface::START) ? s.start() : s.end(); @@ -74,6 +75,16 @@ double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) { state->scene()->getCollisionEnv()->distanceSelf(request, result, state->scene()->getCurrentState()); + double distance{ 0.0 }; + if (cumulative) { + for (const auto& distance_of_pair : result.distances) { + assert(distance_of_pair.second.size() == 1); + distance += distance_of_pair.second[0].distance; + } + } else { + distance = result.minimum_distance.distance; + } + const auto& links = result.minimum_distance.link_names; if (result.minimum_distance.distance <= 0) { @@ -82,10 +93,14 @@ double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) { comment = desc.str(); return std::numeric_limits::infinity(); } else { - boost::format desc("ClearCost: distance %1% between'%2%' and '%3%'"); - desc % result.minimum_distance.distance % links[0] % links[1]; - comment = desc.str(); - return 1.0 / (result.minimum_distance.distance + 1e-5); + if (cumulative) { + comment = "ClearCost: cumulative distance " + std::to_string(distance); + } else { + boost::format desc("ClearCost: distance %1% between '%2%' and '%3%'"); + desc % result.minimum_distance.distance % links[0] % links[1]; + comment = desc.str(); + } + return 1.0 / (distance + 1e-5); } } }