enable costs to (optionally) return a comment

With more complicated costs (such as the ClearanceCost), it's useful to get comments.
This commit is contained in:
v4hn 2020-06-30 09:53:58 +02:00
parent a15204b40b
commit f9ba302728
4 changed files with 34 additions and 14 deletions

View File

@ -23,7 +23,7 @@ private:
double PathLengthCost(const SubTrajectory& s); double PathLengthCost(const SubTrajectory& s);
/// distance to self-collision /// distance to self-collision
double ClearanceCost(const SubTrajectory& s); double ClearanceCost(const SubTrajectory& s, std::string& comment);
} }
} }
} }

View File

@ -213,7 +213,9 @@ public:
/// remove function callback /// remove function callback
void removeSolutionCallback(SolutionCallbackList::const_iterator which); void removeSolutionCallback(SolutionCallbackList::const_iterator which);
using CostTerm = std::function<double(const SubTrajectory&)>; using CostTerm = std::function<double(const SubTrajectory&, std::string&)>;
using CostTermShort = std::function<double(const SubTrajectory&)>;
using CostTransform = std::function<double(const double)>; using CostTransform = std::function<double(const double)>;
/* \brief set method to determine costs for solutions of this stage /* \brief set method to determine costs for solutions of this stage
@ -227,6 +229,7 @@ public:
* See setCostTransform * See setCostTransform
*/ */
void setCostTerm(const CostTerm& term); void setCostTerm(const CostTerm& term);
void setCostTerm(const CostTermShort& term);
/* \brief set CostTransform to be applied to costs of this stage's solution costs /* \brief set CostTransform to be applied to costs of this stage's solution costs
* *

View File

@ -42,6 +42,8 @@
#include <moveit/collision_detection/collision_common.h> #include <moveit/collision_detection/collision_common.h>
#include <boost/format.hpp>
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {
namespace cost { namespace cost {
@ -50,7 +52,7 @@ double PathLengthCost(const SubTrajectory& s) {
return s.trajectory() ? s.trajectory()->getDuration() : 0.0; 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; collision_detection::DistanceRequest request;
request.type = collision_detection::DistanceRequestType::GLOBAL; 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()); 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) { if (result.minimum_distance.distance <= 0) {
// TODO: this should be a comment in the solution boost::format desc("ClearCost: allegedly valid solution has an unwanted collide between '%1%' and '%2%'");
ROS_ERROR_STREAM_NAMED("ClearanceCost", "allegedly valid solution has an unwanted collide between '" desc % links[0] % links[1];
<< result.minimum_distance.link_names[0] << "' and '" comment = desc.str();
<< result.minimum_distance.link_names[1] << "'");
return std::numeric_limits<double>::infinity(); return std::numeric_limits<double>::infinity();
} else { } else {
// ROS_INFO_STREAM_NAMED("ClearanceCost", result.minimum_distance.link_names[0] << " has distance " << boost::format desc("ClearCost: distance %1% between'%2%' and '%3%'");
// result.minimum_distance.distance << " to " << result.minimum_distance.link_names[1]); desc % result.minimum_distance.distance % links[0] % links[1];
comment = desc.str();
return 1.0 / (result.minimum_distance.distance + 1e-5); return 1.0 / (result.minimum_distance.distance + 1e-5);
} }
} }

View File

@ -225,13 +225,22 @@ bool StagePrivate::addCost(SolutionBase& solution) {
double cost{ 0.0 }; double cost{ 0.0 };
auto* trajectory{ dynamic_cast<const SubTrajectory*>(&solution) }; auto* trajectory{ dynamic_cast<const SubTrajectory*>(&solution) };
if (trajectory && cost_term_) if (trajectory && cost_term_) {
// CostTerm only applies to SubTrajectory // compute specific cost
cost = cost_term_(*trajectory); std::string comment;
else cost = cost_term_(*trajectory, comment);
// everything else is just transformed // 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(); cost = solution.cost();
}
// all costs are transformed
solution.setCost(cost_transform_(cost)); solution.setCost(cost_transform_(cost));
return !solution.isFailure(); return !solution.isFailure();
@ -337,6 +346,10 @@ void Stage::setCostTerm(const CostTerm& term) {
pimpl()->cost_term_ = term; pimpl()->cost_term_ = term;
} }
void Stage::setCostTerm(const CostTermShort& term) {
setCostTerm([=](auto&& solution, auto&&) { return term(solution); });
}
void Stage::setCostTransform(const CostTransform& transform) { void Stage::setCostTransform(const CostTransform& transform) {
pimpl()->cost_transform_ = transform; pimpl()->cost_transform_ = transform;
} }