From f9ba3027287ca5d503cfe715e08f13b284902d17 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 30 Jun 2020 09:53:58 +0200 Subject: [PATCH] enable costs to (optionally) return a comment With more complicated costs (such as the ClearanceCost), it's useful to get comments. --- .../moveit/task_constructor/cost_terms.h | 2 +- core/include/moveit/task_constructor/stage.h | 5 +++- core/src/cost_terms.cpp | 18 +++++++++------ core/src/stage.cpp | 23 +++++++++++++++---- 4 files changed, 34 insertions(+), 14 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 821afd94..91c0ab8f 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -23,7 +23,7 @@ private: double PathLengthCost(const SubTrajectory& s); /// distance to self-collision -double ClearanceCost(const SubTrajectory& s); +double ClearanceCost(const SubTrajectory& s, std::string& comment); } } } diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 5a229f9d..857f2012 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -213,7 +213,9 @@ public: /// remove function callback void removeSolutionCallback(SolutionCallbackList::const_iterator which); - using CostTerm = std::function; + using CostTerm = std::function; + using CostTermShort = std::function; + using CostTransform = std::function; /* \brief set method to determine costs for solutions of this stage @@ -227,6 +229,7 @@ public: * See setCostTransform */ void setCostTerm(const CostTerm& term); + void setCostTerm(const CostTermShort& term); /* \brief set CostTransform to be applied to costs of this stage's solution costs * diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 00f3754f..769060c9 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -42,6 +42,8 @@ #include +#include + namespace moveit { namespace task_constructor { namespace cost { @@ -50,7 +52,7 @@ double PathLengthCost(const SubTrajectory& s) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } -double ClearanceCost(const SubTrajectory& s) { +double ClearanceCost(const SubTrajectory& s, std::string& comment) { collision_detection::DistanceRequest request; request.type = collision_detection::DistanceRequestType::GLOBAL; @@ -68,15 +70,17 @@ double ClearanceCost(const SubTrajectory& s) { s.start()->scene()->getCollisionEnv()->distanceSelf(request, result, s.start()->scene()->getCurrentState()); + const auto& links = result.minimum_distance.link_names; + if (result.minimum_distance.distance <= 0) { - // TODO: this should be a comment in the solution - ROS_ERROR_STREAM_NAMED("ClearanceCost", "allegedly valid solution has an unwanted collide between '" - << result.minimum_distance.link_names[0] << "' and '" - << result.minimum_distance.link_names[1] << "'"); + boost::format desc("ClearCost: allegedly valid solution has an unwanted collide between '%1%' and '%2%'"); + desc % links[0] % links[1]; + comment = desc.str(); return std::numeric_limits::infinity(); } else { - // ROS_INFO_STREAM_NAMED("ClearanceCost", result.minimum_distance.link_names[0] << " has distance " << - // result.minimum_distance.distance << " to " << result.minimum_distance.link_names[1]); + 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); } } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 37396f8f..f5ced345 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -225,13 +225,22 @@ bool StagePrivate::addCost(SolutionBase& solution) { double cost{ 0.0 }; auto* trajectory{ dynamic_cast(&solution) }; - if (trajectory && cost_term_) - // CostTerm only applies to SubTrajectory - cost = cost_term_(*trajectory); - else - // everything else is just transformed + if (trajectory && cost_term_) { + // compute specific cost + std::string comment; + cost = cost_term_(*trajectory, comment); + // If a comment was specified, add it to the solution + if (!comment.empty() && !solution.comment().empty()) { + solution.setComment(solution.comment() + " (" + comment + ")"); + } else if (!comment.empty()) { + solution.setComment(comment); + } + } else { + // otherwise forward current cost cost = solution.cost(); + } + // all costs are transformed solution.setCost(cost_transform_(cost)); return !solution.isFailure(); @@ -337,6 +346,10 @@ void Stage::setCostTerm(const CostTerm& term) { pimpl()->cost_term_ = term; } +void Stage::setCostTerm(const CostTermShort& term) { + setCostTerm([=](auto&& solution, auto&&) { return term(solution); }); +} + void Stage::setCostTransform(const CostTransform& transform) { pimpl()->cost_transform_ = transform; }