mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
fully restrict CostTerms to primitive solutions
simplify implementations.
This commit is contained in:
parent
ffc62c2d11
commit
f79625d1bd
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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 {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user