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 {
|
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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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 {
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user