From 18ed51d7f3e26dd092e665039f3319eec1bf20b3 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 30 Jun 2020 11:13:58 +0200 Subject: [PATCH] turn Clearance cost into parameterized struct There is a lot of variations for how to compute clearance. --- .../moveit/task_constructor/cost_terms.h | 20 ++++++++++++++-- core/src/cost_terms.cpp | 24 +++++++++++-------- 2 files changed, 32 insertions(+), 12 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 2c2d7af4..b8ac02c5 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -21,8 +21,24 @@ public: /// execution duration of the whole trajectory double PathLengthCost(const SubTrajectory& s); -/// distance to self-collision -double ClearanceCost(const SubTrajectory& s, std::string& comment); +/** inverse distance to collision + * + * \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 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, + std::string group_property = "group") + : interface(interface), with_world(with_world), group_property(group_property) {} + + double operator()(const SubTrajectory& s, std::string& comment); + + Interface::Direction interface; + bool with_world; + std::string group_property; +}; } } } diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 769060c9..2b3e6b92 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -52,23 +52,27 @@ double PathLengthCost(const SubTrajectory& s) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } -double ClearanceCost(const SubTrajectory& s, std::string& comment) { +double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) { collision_detection::DistanceRequest request; request.type = collision_detection::DistanceRequestType::GLOBAL; - // TODO: possibly parameterize hardcoded property name? - const std::string group{ "group" }; - auto& state_properties{ s.start()->properties() }; - auto& stage_properties{ s.creator()->properties() }; - request.group_name = state_properties.hasProperty(group) ? state_properties.get(group) : - stage_properties.get(group); + const auto& state = (interface == Interface::START) ? s.start() : s.end(); - request.enableGroup(s.start()->scene()->getRobotModel()); - request.acm = &s.start()->scene()->getAllowedCollisionMatrix(); + // prefer interface state property over stage property to find group_name + // TODO: This pattern is general enough to justify its own interface (in the properties?). + auto& state_properties{ state->properties() }; + auto& stage_properties{ s.creator()->properties() }; + request.group_name = state_properties.hasProperty(group_property) ? + state_properties.get(group_property) : + stage_properties.get(group_property); + + // look at all forbidden collisions involving group_name + request.enableGroup(state->scene()->getRobotModel()); + request.acm = &state->scene()->getAllowedCollisionMatrix(); collision_detection::DistanceResult result; - s.start()->scene()->getCollisionEnv()->distanceSelf(request, result, s.start()->scene()->getCurrentState()); + state->scene()->getCollisionEnv()->distanceSelf(request, result, state->scene()->getCurrentState()); const auto& links = result.minimum_distance.link_names;