mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
add cumulative distance to ClearanceCost
This commit is contained in:
parent
18ed51d7f3
commit
a92753dafc
@ -25,11 +25,12 @@ double PathLengthCost(const SubTrajectory& s);
|
||||
*
|
||||
* \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 cumulative if true, compute clearance as aggregated distance of all bodies
|
||||
* \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,
|
||||
ClearanceCost(Interface::Direction interface = Interface::START, bool with_world = true, bool cumulative = false,
|
||||
std::string group_property = "group")
|
||||
: interface(interface), with_world(with_world), group_property(group_property) {}
|
||||
|
||||
@ -37,6 +38,7 @@ struct ClearanceCost
|
||||
|
||||
Interface::Direction interface;
|
||||
bool with_world;
|
||||
bool cumulative;
|
||||
std::string group_property;
|
||||
};
|
||||
}
|
||||
|
||||
@ -54,7 +54,8 @@ double PathLengthCost(const SubTrajectory& s) {
|
||||
|
||||
double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) {
|
||||
collision_detection::DistanceRequest request;
|
||||
request.type = collision_detection::DistanceRequestType::GLOBAL;
|
||||
request.type =
|
||||
cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL;
|
||||
|
||||
const auto& state = (interface == Interface::START) ? s.start() : s.end();
|
||||
|
||||
@ -74,6 +75,16 @@ double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) {
|
||||
|
||||
state->scene()->getCollisionEnv()->distanceSelf(request, result, state->scene()->getCurrentState());
|
||||
|
||||
double distance{ 0.0 };
|
||||
if (cumulative) {
|
||||
for (const auto& distance_of_pair : result.distances) {
|
||||
assert(distance_of_pair.second.size() == 1);
|
||||
distance += distance_of_pair.second[0].distance;
|
||||
}
|
||||
} else {
|
||||
distance = result.minimum_distance.distance;
|
||||
}
|
||||
|
||||
const auto& links = result.minimum_distance.link_names;
|
||||
|
||||
if (result.minimum_distance.distance <= 0) {
|
||||
@ -81,11 +92,15 @@ double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) {
|
||||
desc % links[0] % links[1];
|
||||
comment = desc.str();
|
||||
return std::numeric_limits<double>::infinity();
|
||||
} else {
|
||||
if (cumulative) {
|
||||
comment = "ClearCost: cumulative distance " + std::to_string(distance);
|
||||
} else {
|
||||
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);
|
||||
}
|
||||
return 1.0 / (distance + 1e-5);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user