mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
remove Cost suffix for elements in mtc::cost::
This commit is contained in:
parent
9a96a7e4f6
commit
14c885621f
@ -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,9 +40,9 @@ 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,
|
||||
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) {}
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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<MergeMode>("merge_mode", WAYPOINTS, "merge mode");
|
||||
|
||||
@ -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<double>("max_penetration", "maximally corrected penetration depth");
|
||||
|
||||
@ -49,7 +49,7 @@ namespace stages {
|
||||
using PosesList = std::vector<geometry_msgs::PoseStamped>;
|
||||
|
||||
FixedCartesianPoses::FixedCartesianPoses(const std::string& name) : MonitoringGenerator(name) {
|
||||
setCostTerm(cost::ConstantCost(0.0));
|
||||
setCostTerm(cost::Constant(0.0));
|
||||
|
||||
auto& p = properties();
|
||||
p.declare<PosesList>("poses", PosesList(), "target poses to spawn");
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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<geometry_msgs::PoseStamped>("pose", "target pose to pass on in spawned states");
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user