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 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) {}
|
||||||
|
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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");
|
||||||
|
|||||||
@ -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");
|
||||||
|
|||||||
@ -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");
|
||||||
|
|||||||
@ -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) {
|
||||||
|
|||||||
@ -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");
|
||||||
|
|||||||
@ -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) {
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user