mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
first attempts on ClearanceCost
This commit is contained in:
parent
9060404037
commit
56268cb6cc
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user