Make transform in cost::Clearance configurable

In theory this can be done with a PassThrough with a modifying transform,
but this specific mapping is intrinsic to the definition of Clearance as a cost.
This commit is contained in:
v4hn 2020-09-12 17:41:58 +02:00
parent cdc7cb04fc
commit f63d5b6892
2 changed files with 8 additions and 5 deletions

View File

@ -146,6 +146,8 @@ public:
std::string group_property; std::string group_property;
Interface::Direction interface; Interface::Direction interface;
std::function<double(double)> distance_to_cost;
protected: protected:
double compute(const SubTrajectory& s, std::string& comment) const; double compute(const SubTrajectory& s, std::string& comment) const;
}; };

View File

@ -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) 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); } } : CostTerm{ [this](auto&& s, auto&& c) { return this->compute(s, c); } }
, with_world(with_world) , with_world{ with_world }
, cumulative(cumulative) , cumulative{ cumulative }
, group_property(group_property) , group_property{ group_property }
, interface(interface) {} , interface{ interface }
, distance_to_cost{ [](double d) { return 1.0 / (d + 1e-5); } } {}
double Clearance::compute(const SubTrajectory& s, std::string& comment) const { double Clearance::compute(const SubTrajectory& s, std::string& comment) const {
const std::string PREFIX{ "Clearance: " }; const std::string PREFIX{ "Clearance: " };
@ -216,7 +217,7 @@ double Clearance::compute(const SubTrajectory& s, std::string& comment) const {
comment = desc.str(); comment = desc.str();
} }
return 1.0 / (distance + 1e-5); return distance_to_cost(distance);
} }
} // namespace cost } // namespace cost
} // namespace task_constructor } // namespace task_constructor