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
|
/** inverse distance to collision
|
||||||
*
|
*
|
||||||
* \arg interface compute distances using START or END interface of solution
|
* \arg with_world check distances to world objects or look at self-collisions
|
||||||
* \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 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 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
|
struct Clearance
|
||||||
{
|
{
|
||||||
Clearance(Interface::Direction interface = Interface::START, bool with_world = true, bool cumulative = false,
|
Clearance(bool with_world = true, bool cumulative = false, std::string group_property = "group",
|
||||||
std::string group_property = "group")
|
Interface::Direction interface = Interface::NONE)
|
||||||
: interface(interface), with_world(with_world), group_property(group_property) {}
|
: with_world(with_world), cumulative(cumulative), group_property(group_property), interface(interface) {}
|
||||||
|
|
||||||
double operator()(const SubTrajectory& s, std::string& comment);
|
double operator()(const SubTrajectory& s, std::string& comment);
|
||||||
|
|
||||||
Interface::Direction interface;
|
|
||||||
bool with_world;
|
bool with_world;
|
||||||
bool cumulative;
|
bool cumulative;
|
||||||
std::string group_property;
|
std::string group_property;
|
||||||
|
Interface::Direction interface;
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -163,6 +163,7 @@ public:
|
|||||||
|
|
||||||
enum Direction
|
enum Direction
|
||||||
{
|
{
|
||||||
|
NONE = 0,
|
||||||
FORWARD,
|
FORWARD,
|
||||||
BACKWARD,
|
BACKWARD,
|
||||||
START = FORWARD,
|
START = FORWARD,
|
||||||
|
|||||||
@ -88,11 +88,13 @@ double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
double Clearance::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;
|
collision_detection::DistanceRequest request;
|
||||||
request.type =
|
request.type =
|
||||||
cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL;
|
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
|
// 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?).
|
// 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.enableGroup(state->scene()->getRobotModel());
|
||||||
request.acm = &state->scene()->getAllowedCollisionMatrix();
|
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 };
|
double distance{ 0.0 };
|
||||||
if (cumulative) {
|
|
||||||
for (const auto& distance_of_pair : result.distances) {
|
if (interface == Interface::START || interface == Interface::END ||
|
||||||
assert(distance_of_pair.second.size() == 1);
|
(interface == Interface::NONE && s.trajectory() == nullptr)) {
|
||||||
distance += distance_of_pair.second[0].distance;
|
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 = distance_data.distance;
|
||||||
distance = result.minimum_distance.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];
|
||||||
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];
|
|
||||||
comment = desc.str();
|
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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user