mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
a15204b40b
commit
f9ba302728
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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
|
||||
*
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user