add cumulative distance to ClearanceCost

This commit is contained in:
v4hn 2020-06-30 11:31:17 +02:00
parent 18ed51d7f3
commit a92753dafc
2 changed files with 23 additions and 6 deletions

View File

@ -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;
};
}

View File

@ -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) {
@ -82,10 +93,14 @@ double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) {
comment = desc.str();
return std::numeric_limits<double>::infinity();
} 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);
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 / (distance + 1e-5);
}
}
}