first attempts on ClearanceCost

This commit is contained in:
v4hn 2020-06-19 23:08:12 +02:00
parent 9060404037
commit 56268cb6cc
2 changed files with 39 additions and 0 deletions

View File

@ -21,6 +21,9 @@ private:
/// execution duration of the whole trajectory
double PathLengthCost(const SubTrajectory& s);
/// distance to self-collision
double ClearanceCost(const SubTrajectory& s);
}
}
}

View File

@ -35,7 +35,12 @@
/* Authors: Michael Goerner */
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/stage.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/collision_detection/collision_common.h>
namespace moveit {
namespace task_constructor {
@ -44,6 +49,37 @@ namespace cost {
double PathLengthCost(const SubTrajectory& s) {
return s.trajectory() ? s.trajectory()->getDuration() : 0.0;
}
double ClearanceCost(const SubTrajectory& s) {
collision_detection::DistanceRequest request;
request.type = collision_detection::DistanceRequestType::GLOBAL;
// TODO: possibly parameterize hardcoded property name?
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());
request.acm = &s.start()->scene()->getAllowedCollisionMatrix();
collision_detection::DistanceResult result;
s.start()->scene()->getCollisionEnv()->distanceSelf(request, result, s.start()->scene()->getCurrentState());
if (result.minimum_distance.distance <= 0) {
// TODO: this should be a comment in the solution
ROS_ERROR_STREAM_NAMED("ClearanceCost", "allegedly valid solution has an unwanted collide between '"
<< result.minimum_distance.link_names[0] << "' and '"
<< result.minimum_distance.link_names[1] << "'");
return std::numeric_limits<double>::infinity();
} else {
// ROS_INFO_STREAM_NAMED("ClearanceCost", result.minimum_distance.link_names[0] << " has distance " <<
// result.minimum_distance.distance << " to " << result.minimum_distance.link_names[1]);
return 1.0 / (result.minimum_distance.distance + 1e-5);
}
}
}
}
}