generalize Clearance cost

- can now be used to estimate cost for either interface state or the trajectory
- Introduced Interface::Direction NONE as a way of pointing to the trajectory in contrast to START or END
This commit is contained in:
v4hn 2020-07-16 16:07:43 +02:00
parent 7a00ac646e
commit d2bd0a0037
3 changed files with 68 additions and 31 deletions

View File

@ -73,23 +73,24 @@ struct LinkMotion
/** inverse distance to collision
*
* \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 with_world check distances to world objects or 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
* \arg interface compute distances using START or END interface of solution *only*, instead of averaging over
* trajectory
* */
struct Clearance
{
Clearance(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) {}
Clearance(bool with_world = true, bool cumulative = false, std::string group_property = "group",
Interface::Direction interface = Interface::NONE)
: with_world(with_world), cumulative(cumulative), group_property(group_property), interface(interface) {}
double operator()(const SubTrajectory& s, std::string& comment);
Interface::Direction interface;
bool with_world;
bool cumulative;
std::string group_property;
Interface::Direction interface;
};
}
}

View File

@ -163,6 +163,7 @@ public:
enum Direction
{
NONE = 0,
FORWARD,
BACKWARD,
START = FORWARD,

View File

@ -88,11 +88,13 @@ double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) {
}
double Clearance::operator()(const SubTrajectory& s, std::string& comment) {
const std::string PREFIX{ "Clearance: " };
collision_detection::DistanceRequest request;
request.type =
cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL;
const auto& state = (interface == Interface::START) ? s.start() : s.end();
const auto& state{ (interface == Interface::END) ? s.end() : s.start() };
// prefer interface state property over stage property to find group_name
// TODO: This pattern is general enough to justify its own interface (in the properties?).
@ -106,37 +108,70 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) {
request.enableGroup(state->scene()->getRobotModel());
request.acm = &state->scene()->getAllowedCollisionMatrix();
collision_detection::DistanceResult result;
// compute relevant distance data for state & robot
auto check_distance{ [=](const InterfaceState* state, const moveit::core::RobotState& robot) {
collision_detection::DistanceResult result;
if (with_world)
state->scene()->getCollisionEnv()->distanceRobot(request, result, robot);
else
state->scene()->getCollisionEnv()->distanceSelf(request, result, robot);
state->scene()->getCollisionEnv()->distanceSelf(request, result, state->scene()->getCurrentState());
if (result.minimum_distance.distance <= 0) {
return result.minimum_distance;
}
if (cumulative) {
double distance{ 0.0 };
for (const auto& distance_of_pair : result.distances) {
assert(distance_of_pair.second.size() == 1);
distance += distance_of_pair.second[0].distance;
}
result.minimum_distance.distance = distance;
}
return result.minimum_distance;
} };
auto collision_comment{ [=](const auto& distance) {
boost::format desc{ PREFIX + "allegedly valid solution collides between '%1%' and '%2%'" };
desc % distance.link_names[0] % distance.link_names[1];
return desc.str();
} };
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;
if (interface == Interface::START || interface == Interface::END ||
(interface == Interface::NONE && s.trajectory() == nullptr)) {
auto distance_data{ check_distance(state, state->scene()->getCurrentState()) };
if (distance_data.distance < 0) {
comment = collision_comment(distance_data);
return std::numeric_limits<double>::infinity();
}
} else {
distance = result.minimum_distance.distance;
}
const auto& links = result.minimum_distance.link_names;
if (result.minimum_distance.distance <= 0) {
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 {
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];
distance = distance_data.distance;
if (!cumulative) {
boost::format desc{ PREFIX + "distance %1% between '%2%' and '%3%'" };
desc % distance % distance_data.link_names[0] % distance_data.link_names[1];
comment = desc.str();
} else {
comment = PREFIX + "cumulative distance " + std::to_string(distance);
}
return 1.0 / (distance + 1e-5);
} else { // check trajectory
for (size_t i = 0; i < s.trajectory()->getWayPointCount(); ++i) {
auto distance_data = check_distance(state, s.trajectory()->getWayPoint(i));
if (distance_data.distance < 0) {
comment = collision_comment(distance_data);
return std::numeric_limits<double>::infinity();
}
distance += distance_data.distance;
}
distance /= s.trajectory()->getWayPointCount();
boost::format desc(PREFIX + "average%1% distance: %2%");
desc % (cumulative ? " cumulative" : "") % distance;
comment = desc.str();
}
return 1.0 / (distance + 1e-5);
}
}
}