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 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 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
|
* \arg group_property the name of the property which defines the group to look at
|
||||||
* */
|
* */
|
||||||
struct ClearanceCost
|
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")
|
std::string group_property = "group")
|
||||||
: interface(interface), with_world(with_world), group_property(group_property) {}
|
: interface(interface), with_world(with_world), group_property(group_property) {}
|
||||||
|
|
||||||
@ -37,6 +38,7 @@ struct ClearanceCost
|
|||||||
|
|
||||||
Interface::Direction interface;
|
Interface::Direction interface;
|
||||||
bool with_world;
|
bool with_world;
|
||||||
|
bool cumulative;
|
||||||
std::string group_property;
|
std::string group_property;
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
|||||||
@ -54,7 +54,8 @@ double PathLengthCost(const SubTrajectory& s) {
|
|||||||
|
|
||||||
double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) {
|
double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) {
|
||||||
collision_detection::DistanceRequest request;
|
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();
|
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());
|
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;
|
const auto& links = result.minimum_distance.link_names;
|
||||||
|
|
||||||
if (result.minimum_distance.distance <= 0) {
|
if (result.minimum_distance.distance <= 0) {
|
||||||
@ -81,11 +92,15 @@ double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) {
|
|||||||
desc % links[0] % links[1];
|
desc % links[0] % links[1];
|
||||||
comment = desc.str();
|
comment = desc.str();
|
||||||
return std::numeric_limits<double>::infinity();
|
return std::numeric_limits<double>::infinity();
|
||||||
|
} else {
|
||||||
|
if (cumulative) {
|
||||||
|
comment = "ClearCost: cumulative distance " + std::to_string(distance);
|
||||||
} else {
|
} else {
|
||||||
boost::format desc("ClearCost: distance %1% between '%2%' and '%3%'");
|
boost::format desc("ClearCost: distance %1% between '%2%' and '%3%'");
|
||||||
desc % result.minimum_distance.distance % links[0] % links[1];
|
desc % result.minimum_distance.distance % links[0] % links[1];
|
||||||
comment = desc.str();
|
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