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);
/// 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
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)>;
/* \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
*

View File

@ -42,6 +42,8 @@
#include <moveit/collision_detection/collision_common.h>
#include <boost/format.hpp>
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<double>::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);
}
}

View File

@ -225,13 +225,22 @@ bool StagePrivate::addCost(SolutionBase& solution) {
double cost{ 0.0 };
auto* trajectory{ dynamic_cast<const SubTrajectory*>(&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;
}