mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
7a00ac646e
commit
d2bd0a0037
@ -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;
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
@ -163,6 +163,7 @@ public:
|
||||
|
||||
enum Direction
|
||||
{
|
||||
NONE = 0,
|
||||
FORWARD,
|
||||
BACKWARD,
|
||||
START = FORWARD,
|
||||
|
||||
@ -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();
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
double distance{ 0.0 };
|
||||
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;
|
||||
}
|
||||
} else {
|
||||
distance = result.minimum_distance.distance;
|
||||
result.minimum_distance.distance = distance;
|
||||
}
|
||||
|
||||
const auto& links = result.minimum_distance.link_names;
|
||||
return result.minimum_distance;
|
||||
} };
|
||||
|
||||
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();
|
||||
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 (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();
|
||||
}
|
||||
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 {
|
||||
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 = PREFIX + "cumulative distance " + std::to_string(distance);
|
||||
}
|
||||
} 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user