mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
turn Clearance cost into parameterized struct
There is a lot of variations for how to compute clearance.
This commit is contained in:
parent
cf0e4e3b01
commit
18ed51d7f3
@ -21,8 +21,24 @@ public:
|
|||||||
/// execution duration of the whole trajectory
|
/// execution duration of the whole trajectory
|
||||||
double PathLengthCost(const SubTrajectory& s);
|
double PathLengthCost(const SubTrajectory& s);
|
||||||
|
|
||||||
/// distance to self-collision
|
/** inverse distance to collision
|
||||||
double ClearanceCost(const SubTrajectory& s, std::string& comment);
|
*
|
||||||
|
* \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 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,
|
||||||
|
std::string group_property = "group")
|
||||||
|
: interface(interface), with_world(with_world), group_property(group_property) {}
|
||||||
|
|
||||||
|
double operator()(const SubTrajectory& s, std::string& comment);
|
||||||
|
|
||||||
|
Interface::Direction interface;
|
||||||
|
bool with_world;
|
||||||
|
std::string group_property;
|
||||||
|
};
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -52,23 +52,27 @@ double PathLengthCost(const SubTrajectory& s) {
|
|||||||
return s.trajectory() ? s.trajectory()->getDuration() : 0.0;
|
return s.trajectory() ? s.trajectory()->getDuration() : 0.0;
|
||||||
}
|
}
|
||||||
|
|
||||||
double ClearanceCost(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 = collision_detection::DistanceRequestType::GLOBAL;
|
||||||
|
|
||||||
// TODO: possibly parameterize hardcoded property name?
|
const auto& state = (interface == Interface::START) ? s.start() : s.end();
|
||||||
const std::string group{ "group" };
|
|
||||||
auto& state_properties{ s.start()->properties() };
|
|
||||||
auto& stage_properties{ s.creator()->properties() };
|
|
||||||
request.group_name = state_properties.hasProperty(group) ? state_properties.get<std::string>(group) :
|
|
||||||
stage_properties.get<std::string>(group);
|
|
||||||
|
|
||||||
request.enableGroup(s.start()->scene()->getRobotModel());
|
// prefer interface state property over stage property to find group_name
|
||||||
request.acm = &s.start()->scene()->getAllowedCollisionMatrix();
|
// TODO: This pattern is general enough to justify its own interface (in the properties?).
|
||||||
|
auto& state_properties{ state->properties() };
|
||||||
|
auto& stage_properties{ s.creator()->properties() };
|
||||||
|
request.group_name = state_properties.hasProperty(group_property) ?
|
||||||
|
state_properties.get<std::string>(group_property) :
|
||||||
|
stage_properties.get<std::string>(group_property);
|
||||||
|
|
||||||
|
// look at all forbidden collisions involving group_name
|
||||||
|
request.enableGroup(state->scene()->getRobotModel());
|
||||||
|
request.acm = &state->scene()->getAllowedCollisionMatrix();
|
||||||
|
|
||||||
collision_detection::DistanceResult result;
|
collision_detection::DistanceResult result;
|
||||||
|
|
||||||
s.start()->scene()->getCollisionEnv()->distanceSelf(request, result, s.start()->scene()->getCurrentState());
|
state->scene()->getCollisionEnv()->distanceSelf(request, result, state->scene()->getCurrentState());
|
||||||
|
|
||||||
const auto& links = result.minimum_distance.link_names;
|
const auto& links = result.minimum_distance.link_names;
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user