diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index f1dd680c..f25a27c5 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -146,6 +146,8 @@ public: std::string group_property; Interface::Direction interface; + std::function distance_to_cost; + protected: double compute(const SubTrajectory& s, std::string& comment) const; }; diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index d22ec5d4..72c99750 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -127,10 +127,11 @@ double LinkMotion::compute(const SubTrajectory& s, std::string& comment) const { Clearance::Clearance(bool with_world, bool cumulative, std::string group_property, Interface::Direction interface) : CostTerm{ [this](auto&& s, auto&& c) { return this->compute(s, c); } } - , with_world(with_world) - , cumulative(cumulative) - , group_property(group_property) - , interface(interface) {} + , with_world{ with_world } + , cumulative{ cumulative } + , group_property{ group_property } + , interface{ interface } + , distance_to_cost{ [](double d) { return 1.0 / (d + 1e-5); } } {} double Clearance::compute(const SubTrajectory& s, std::string& comment) const { const std::string PREFIX{ "Clearance: " }; @@ -216,7 +217,7 @@ double Clearance::compute(const SubTrajectory& s, std::string& comment) const { comment = desc.str(); } - return 1.0 / (distance + 1e-5); + return distance_to_cost(distance); } } // namespace cost } // namespace task_constructor