remove Cost suffix for elements in mtc::cost::

This commit is contained in:
v4hn 2020-06-30 21:02:53 +02:00
parent 9a96a7e4f6
commit 14c885621f
10 changed files with 22 additions and 22 deletions

View File

@ -4,14 +4,14 @@
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {
/// These structures all implement the Stage::CostTerm API and can be configured via Stage::setCostTerm()
namespace cost { namespace cost {
/// These structures all implement the Stage::CostTerm API and can be configured via Stage::setCostTerm()
/// add a constant cost to each solution /// add a constant cost to each solution
struct ConstantCost struct Constant
{ {
public: public:
ConstantCost(double c) : cost(c) {} Constant(double c) : cost(c) {}
double operator()(const SubTrajectory&, std::string& /* unused */) const { return cost; } double operator()(const SubTrajectory&, std::string& /* unused */) const { return cost; }
@ -19,14 +19,14 @@ public:
}; };
/// trajectory length (interpolated between waypoints) /// trajectory length (interpolated between waypoints)
double PathLengthCost(const SubTrajectory& s); double PathLength(const SubTrajectory& s);
/// execution duration of the whole trajectory /// execution duration of the whole trajectory
double TrajectoryDurationCost(const SubTrajectory& s); double TrajectoryDuration(const SubTrajectory& s);
struct LinkMotionCost struct LinkMotion
{ {
LinkMotionCost(std::string link_name) : link_name(link_name) {} LinkMotion(std::string link_name) : link_name(link_name) {}
double operator()(const SubTrajectory&, std::string&); double operator()(const SubTrajectory&, std::string&);
@ -40,9 +40,9 @@ struct LinkMotionCost
* \arg cumulative if true, compute clearance as aggregated distance of all bodies * \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 Clearance
{ {
ClearanceCost(Interface::Direction interface = Interface::START, bool with_world = true, bool cumulative = false, Clearance(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) {}

View File

@ -48,7 +48,7 @@ namespace moveit {
namespace task_constructor { namespace task_constructor {
namespace cost { namespace cost {
double PathLengthCost(const SubTrajectory& s) { double PathLength(const SubTrajectory& s) {
const auto& traj = s.trajectory(); const auto& traj = s.trajectory();
if (traj == nullptr) if (traj == nullptr)
@ -60,11 +60,11 @@ double PathLengthCost(const SubTrajectory& s) {
return path_length; return path_length;
} }
double TrajectoryDurationCost(const SubTrajectory& s) { double TrajectoryDuration(const SubTrajectory& s) {
return s.trajectory() ? s.trajectory()->getDuration() : 0.0; return s.trajectory() ? s.trajectory()->getDuration() : 0.0;
} }
double LinkMotionCost::operator()(const SubTrajectory& s, std::string& comment) { double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) {
const auto& traj{ s.trajectory() }; const auto& traj{ s.trajectory() };
if (traj == nullptr || traj->getWayPointCount() == 0) if (traj == nullptr || traj->getWayPointCount() == 0)
@ -87,7 +87,7 @@ double LinkMotionCost::operator()(const SubTrajectory& s, std::string& comment)
return distance; return distance;
} }
double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) { double Clearance::operator()(const SubTrajectory& s, std::string& comment) {
collision_detection::DistanceRequest request; collision_detection::DistanceRequest request;
request.type = request.type =
cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL; cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL;

View File

@ -48,7 +48,7 @@ namespace stages {
Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : Connecting(name), planner_(planners) { Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : Connecting(name), planner_(planners) {
setTimeout(1.0); setTimeout(1.0);
setCostTerm(cost::PathLengthCost); setCostTerm(cost::PathLength);
auto& p = properties(); auto& p = properties();
p.declare<MergeMode>("merge_mode", WAYPOINTS, "merge mode"); p.declare<MergeMode>("merge_mode", WAYPOINTS, "merge mode");

View File

@ -56,7 +56,7 @@ namespace stages {
FixCollisionObjects::FixCollisionObjects(const std::string& name) : PropagatingEitherWay(name) { FixCollisionObjects::FixCollisionObjects(const std::string& name) : PropagatingEitherWay(name) {
// TODO: possibly weight solutions based on the required displacement? // TODO: possibly weight solutions based on the required displacement?
setCostTerm(cost::ConstantCost(0.0)); setCostTerm(cost::Constant(0.0));
auto& p = properties(); auto& p = properties();
p.declare<double>("max_penetration", "maximally corrected penetration depth"); p.declare<double>("max_penetration", "maximally corrected penetration depth");

View File

@ -49,7 +49,7 @@ namespace stages {
using PosesList = std::vector<geometry_msgs::PoseStamped>; using PosesList = std::vector<geometry_msgs::PoseStamped>;
FixedCartesianPoses::FixedCartesianPoses(const std::string& name) : MonitoringGenerator(name) { FixedCartesianPoses::FixedCartesianPoses(const std::string& name) : MonitoringGenerator(name) {
setCostTerm(cost::ConstantCost(0.0)); setCostTerm(cost::Constant(0.0));
auto& p = properties(); auto& p = properties();
p.declare<PosesList>("poses", PosesList(), "target poses to spawn"); p.declare<PosesList>("poses", PosesList(), "target poses to spawn");

View File

@ -44,7 +44,7 @@ namespace task_constructor {
namespace stages { namespace stages {
FixedState::FixedState(const std::string& name) : Generator(name) { FixedState::FixedState(const std::string& name) : Generator(name) {
setCostTerm(cost::ConstantCost(0.0)); setCostTerm(cost::Constant(0.0));
} }
void FixedState::setState(const planning_scene::PlanningScenePtr& scene) { void FixedState::setState(const planning_scene::PlanningScenePtr& scene) {

View File

@ -46,7 +46,7 @@ namespace task_constructor {
namespace stages { namespace stages {
GeneratePose::GeneratePose(const std::string& name) : MonitoringGenerator(name) { GeneratePose::GeneratePose(const std::string& name) : MonitoringGenerator(name) {
setCostTerm(cost::ConstantCost(0.0)); setCostTerm(cost::Constant(0.0));
auto& p = properties(); auto& p = properties();
p.declare<geometry_msgs::PoseStamped>("pose", "target pose to pass on in spawned states"); p.declare<geometry_msgs::PoseStamped>("pose", "target pose to pass on in spawned states");

View File

@ -47,7 +47,7 @@ namespace task_constructor {
namespace stages { namespace stages {
ModifyPlanningScene::ModifyPlanningScene(const std::string& name) : PropagatingEitherWay(name) { ModifyPlanningScene::ModifyPlanningScene(const std::string& name) : PropagatingEitherWay(name) {
setCostTerm(cost::ConstantCost(0.0)); setCostTerm(cost::Constant(0.0));
} }
void ModifyPlanningScene::attachObjects(const Names& objects, const std::string& attach_link, bool attach) { void ModifyPlanningScene::attachObjects(const Names& objects, const std::string& attach_link, bool attach) {

View File

@ -49,7 +49,7 @@ namespace stages {
MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterfacePtr& planner) MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterfacePtr& planner)
: PropagatingEitherWay(name), planner_(planner) { : PropagatingEitherWay(name), planner_(planner) {
setCostTerm(cost::PathLengthCost); setCostTerm(cost::PathLength);
auto& p = properties(); auto& p = properties();
p.property("timeout").setDefaultValue(1.0); p.property("timeout").setDefaultValue(1.0);

View File

@ -50,7 +50,7 @@ namespace stages {
MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& planner) MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& planner)
: PropagatingEitherWay(name), planner_(planner) { : PropagatingEitherWay(name), planner_(planner) {
setCostTerm(cost::PathLengthCost); setCostTerm(cost::PathLength);
auto& p = properties(); auto& p = properties();
p.property("timeout").setDefaultValue(1.0); p.property("timeout").setDefaultValue(1.0);