diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index d16b6722..8f551664 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -4,14 +4,14 @@ namespace moveit { namespace task_constructor { +/// These structures all implement the Stage::CostTerm API and can be configured via Stage::setCostTerm() namespace cost { -/// These structures all implement the Stage::CostTerm API and can be configured via Stage::setCostTerm() /// add a constant cost to each solution -struct ConstantCost +struct Constant { public: - ConstantCost(double c) : cost(c) {} + Constant(double c) : cost(c) {} double operator()(const SubTrajectory&, std::string& /* unused */) const { return cost; } @@ -19,14 +19,14 @@ public: }; /// trajectory length (interpolated between waypoints) -double PathLengthCost(const SubTrajectory& s); +double PathLength(const SubTrajectory& s); /// 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&); @@ -40,10 +40,10 @@ struct LinkMotionCost * \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 * */ -struct ClearanceCost +struct Clearance { - ClearanceCost(Interface::Direction interface = Interface::START, bool with_world = true, bool cumulative = false, - std::string group_property = "group") + Clearance(Interface::Direction interface = Interface::START, bool with_world = true, bool cumulative = false, + std::string group_property = "group") : interface(interface), with_world(with_world), group_property(group_property) {} double operator()(const SubTrajectory& s, std::string& comment); diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index bbdad413..5dc139d3 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -48,7 +48,7 @@ namespace moveit { namespace task_constructor { namespace cost { -double PathLengthCost(const SubTrajectory& s) { +double PathLength(const SubTrajectory& s) { const auto& traj = s.trajectory(); if (traj == nullptr) @@ -60,11 +60,11 @@ double PathLengthCost(const SubTrajectory& s) { return path_length; } -double TrajectoryDurationCost(const SubTrajectory& s) { +double TrajectoryDuration(const SubTrajectory& s) { 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() }; if (traj == nullptr || traj->getWayPointCount() == 0) @@ -87,7 +87,7 @@ double LinkMotionCost::operator()(const SubTrajectory& s, std::string& comment) return distance; } -double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) { +double Clearance::operator()(const SubTrajectory& s, std::string& comment) { collision_detection::DistanceRequest request; request.type = cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL; diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 8d38da55..cb66a78d 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -48,7 +48,7 @@ namespace stages { Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : Connecting(name), planner_(planners) { setTimeout(1.0); - setCostTerm(cost::PathLengthCost); + setCostTerm(cost::PathLength); auto& p = properties(); p.declare("merge_mode", WAYPOINTS, "merge mode"); diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp index 0d0f9cc8..05b820e8 100644 --- a/core/src/stages/fix_collision_objects.cpp +++ b/core/src/stages/fix_collision_objects.cpp @@ -56,7 +56,7 @@ namespace stages { FixCollisionObjects::FixCollisionObjects(const std::string& name) : PropagatingEitherWay(name) { // TODO: possibly weight solutions based on the required displacement? - setCostTerm(cost::ConstantCost(0.0)); + setCostTerm(cost::Constant(0.0)); auto& p = properties(); p.declare("max_penetration", "maximally corrected penetration depth"); diff --git a/core/src/stages/fixed_cartesian_poses.cpp b/core/src/stages/fixed_cartesian_poses.cpp index a78c2414..d6f0d96a 100644 --- a/core/src/stages/fixed_cartesian_poses.cpp +++ b/core/src/stages/fixed_cartesian_poses.cpp @@ -49,7 +49,7 @@ namespace stages { using PosesList = std::vector; FixedCartesianPoses::FixedCartesianPoses(const std::string& name) : MonitoringGenerator(name) { - setCostTerm(cost::ConstantCost(0.0)); + setCostTerm(cost::Constant(0.0)); auto& p = properties(); p.declare("poses", PosesList(), "target poses to spawn"); diff --git a/core/src/stages/fixed_state.cpp b/core/src/stages/fixed_state.cpp index d76d37ac..971b6da6 100644 --- a/core/src/stages/fixed_state.cpp +++ b/core/src/stages/fixed_state.cpp @@ -44,7 +44,7 @@ namespace task_constructor { namespace stages { 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) { diff --git a/core/src/stages/generate_pose.cpp b/core/src/stages/generate_pose.cpp index 85efbc44..ebdb73ba 100644 --- a/core/src/stages/generate_pose.cpp +++ b/core/src/stages/generate_pose.cpp @@ -46,7 +46,7 @@ namespace task_constructor { namespace stages { GeneratePose::GeneratePose(const std::string& name) : MonitoringGenerator(name) { - setCostTerm(cost::ConstantCost(0.0)); + setCostTerm(cost::Constant(0.0)); auto& p = properties(); p.declare("pose", "target pose to pass on in spawned states"); diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index 36a45753..8343151e 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -47,7 +47,7 @@ namespace task_constructor { namespace stages { 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) { diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index da398dcd..218cd721 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -49,7 +49,7 @@ namespace stages { MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterfacePtr& planner) : PropagatingEitherWay(name), planner_(planner) { - setCostTerm(cost::PathLengthCost); + setCostTerm(cost::PathLength); auto& p = properties(); p.property("timeout").setDefaultValue(1.0); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 21db9815..c151f580 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -50,7 +50,7 @@ namespace stages { MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& planner) : PropagatingEitherWay(name), planner_(planner) { - setCostTerm(cost::PathLengthCost); + setCostTerm(cost::PathLength); auto& p = properties(); p.property("timeout").setDefaultValue(1.0);