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 {
/// These structures all implement the Stage::CostTerm API and can be configured via Stage::setCostTerm()
/// add a constant cost to each solution
class ConstantCost
{
public:
ConstantCost(double cost) : cost_(cost) {}
double operator()(const SolutionBase&) { return cost_; }
double operator()(const SubTrajectory&) { return cost_; }
private:
double cost_;
};
/// 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
void removeSolutionCallback(SolutionCallbackList::const_iterator which);
using CostTerm = std::function<double(const SolutionBase&)>;
void setCostTerm(const CostTerm&);
using CostTerm = std::function<double(const SubTrajectory&)>;
const ordered<SolutionBaseConstPtr>& solutions() const;
const std::list<SolutionBaseConstPtr>& failures() const;

View File

@ -41,20 +41,8 @@ namespace moveit {
namespace task_constructor {
namespace cost {
double PathLengthCost(const SolutionBase& s) {
auto* trajectory{ dynamic_cast<const SubTrajectory*>(&s) };
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;
}
double PathLengthCost(const SubTrajectory& s) {
return s.trajectory() ? s.trajectory()->getDuration() : 0.0;
}
}
}

View File

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