fully restrict CostTerms to primitive solutions

simplify implementations.
This commit is contained in:
v4hn 2020-05-20 14:26:33 +02:00
parent ffc62c2d11
commit f79625d1bd
4 changed files with 10 additions and 23 deletions

View File

@ -7,21 +7,20 @@ namespace task_constructor {
namespace cost { namespace cost {
/// These structures all implement the Stage::CostTerm API and can be configured via Stage::setCostTerm() /// 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
class ConstantCost class ConstantCost
{ {
public: public:
ConstantCost(double cost) : cost_(cost) {} ConstantCost(double cost) : cost_(cost) {}
double operator()(const SolutionBase&) { return cost_; } double operator()(const SubTrajectory&) { return cost_; }
private: private:
double cost_; double cost_;
}; };
/// define cost as execution duration of the whole trajectory /// define cost as execution duration of the whole trajectory
double PathLengthCost(const SolutionBase& s); double PathLengthCost(const SubTrajectory& s);
} }
} }
} }

View File

@ -213,8 +213,7 @@ public:
/// remove function callback /// remove function callback
void removeSolutionCallback(SolutionCallbackList::const_iterator which); void removeSolutionCallback(SolutionCallbackList::const_iterator which);
using CostTerm = std::function<double(const SolutionBase&)>; using CostTerm = std::function<double(const SubTrajectory&)>;
void setCostTerm(const CostTerm&);
const ordered<SolutionBaseConstPtr>& solutions() const; const ordered<SolutionBaseConstPtr>& solutions() const;
const std::list<SolutionBaseConstPtr>& failures() const; const std::list<SolutionBaseConstPtr>& failures() const;

View File

@ -41,20 +41,8 @@ namespace moveit {
namespace task_constructor { namespace task_constructor {
namespace cost { namespace cost {
double PathLengthCost(const SolutionBase& s) { double PathLengthCost(const SubTrajectory& s) {
auto* trajectory{ dynamic_cast<const SubTrajectory*>(&s) }; return s.trajectory() ? s.trajectory()->getDuration() : 0.0;
if (trajectory) {
return trajectory->trajectory() ? trajectory->trajectory()->getDuration() : 0.0;
} else {
// there is no other alternative implementation of SolutionBase so static_cast is safe
auto& seq{ *static_cast<const SolutionSequence*>(&s) };
double cost{ 0.0 };
for (auto& sub : seq.solutions())
cost += PathLengthCost(*sub);
return cost;
}
} }
} }
} }

View File

@ -217,8 +217,9 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) {
} }
bool StagePrivate::addCost(SolutionBase& solution) { bool StagePrivate::addCost(SolutionBase& solution) {
if (cost_term_) auto* trajectory{ dynamic_cast<const SubTrajectory*>(&solution) };
solution.setCost(cost_term_(solution)); if (trajectory && cost_term_)
solution.setCost(cost_term_(*trajectory));
return !solution.isFailure(); return !solution.isFailure();
} }
@ -319,8 +320,8 @@ void Stage::removeSolutionCallback(SolutionCallbackList::const_iterator which) {
pimpl()->solution_cbs_.erase(which); pimpl()->solution_cbs_.erase(which);
} }
void Stage::setCostTerm(const CostTerm& cost_term) { void Stage::setCostTerm(const CostTerm& term) {
pimpl()->cost_term_ = cost_term; pimpl()->cost_term_ = term;
} }
const ordered<SolutionBaseConstPtr>& Stage::solutions() const { const ordered<SolutionBaseConstPtr>& Stage::solutions() const {