turn Clearance cost into parameterized struct

There is a lot of variations for how to compute clearance.
This commit is contained in:
v4hn 2020-06-30 11:13:58 +02:00
parent cf0e4e3b01
commit 18ed51d7f3
2 changed files with 32 additions and 12 deletions

View File

@ -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;
};
} }
} }
} }

View File

@ -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;