mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
add CostTransform
CostTerms only apply to primitive solutions and generalizing them to Containers would make them quite unintuitive (and adds overhead). Instead CostTransform can be used in any container to scale, crop, square the cost of the solutions. I initially thought about adding scaling factors, but then again, other transforms are of interest just as well.
This commit is contained in:
parent
f79625d1bd
commit
f3fec26f5a
@ -214,6 +214,27 @@ public:
|
||||
void removeSolutionCallback(SolutionCallbackList::const_iterator which);
|
||||
|
||||
using CostTerm = std::function<double(const SubTrajectory&)>;
|
||||
using CostTransform = std::function<double(const double)>;
|
||||
|
||||
/* \brief set method to determine costs for solutions of this stage
|
||||
*
|
||||
* This interface supports only primitive solutions, no containers.
|
||||
*
|
||||
* In order to support containers, the associated CostTerms would need to be able
|
||||
* to decide costs for arbitrary partial solutions of the container.
|
||||
* These need to be determined for scheduling, but would make the terms too complex.
|
||||
*
|
||||
* See setCostTransform
|
||||
*/
|
||||
void setCostTerm(const CostTerm& term);
|
||||
|
||||
/* \brief set CostTransform to be applied to costs of this stage's solution costs
|
||||
*
|
||||
* This interface can be used to modify the cost associated with each solution of this stage/container.
|
||||
* The most common case would be linear weighting `[=a](double c){ return a*c; }`
|
||||
* but any non-linear transform is obviously possible as well.
|
||||
*/
|
||||
void setCostTransform(const CostTransform& transform);
|
||||
|
||||
const ordered<SolutionBaseConstPtr>& solutions() const;
|
||||
const std::list<SolutionBaseConstPtr>& failures() const;
|
||||
|
||||
@ -173,6 +173,7 @@ protected:
|
||||
|
||||
// user-configurable cost estimator
|
||||
Stage::CostTerm cost_term_;
|
||||
Stage::CostTransform cost_transform_;
|
||||
|
||||
// The total compute time
|
||||
std::chrono::duration<double> total_compute_time_;
|
||||
|
||||
@ -97,7 +97,12 @@ std::ostream& operator<<(std::ostream& os, const InitStageException& e) {
|
||||
}
|
||||
|
||||
StagePrivate::StagePrivate(Stage* me, const std::string& name)
|
||||
: me_(me), name_(name), total_compute_time_{}, parent_(nullptr), introspection_(nullptr) {}
|
||||
: me_(me)
|
||||
, name_(name)
|
||||
, cost_transform_([](auto x) { return x; })
|
||||
, total_compute_time_{}
|
||||
, parent_(nullptr)
|
||||
, introspection_(nullptr) {}
|
||||
|
||||
InterfaceFlags StagePrivate::interfaceFlags() const {
|
||||
InterfaceFlags f;
|
||||
@ -217,9 +222,17 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) {
|
||||
}
|
||||
|
||||
bool StagePrivate::addCost(SolutionBase& solution) {
|
||||
double cost{ 0.0 };
|
||||
|
||||
auto* trajectory{ dynamic_cast<const SubTrajectory*>(&solution) };
|
||||
if (trajectory && cost_term_)
|
||||
solution.setCost(cost_term_(*trajectory));
|
||||
// CostTerm only applies to SubTrajectory
|
||||
cost = cost_term_(*trajectory);
|
||||
else
|
||||
// everything else is just transformed
|
||||
cost = solution.cost();
|
||||
|
||||
solution.setCost(cost_transform_(cost));
|
||||
|
||||
return !solution.isFailure();
|
||||
}
|
||||
@ -324,6 +337,10 @@ void Stage::setCostTerm(const CostTerm& term) {
|
||||
pimpl()->cost_term_ = term;
|
||||
}
|
||||
|
||||
void Stage::setCostTransform(const CostTransform& transform) {
|
||||
pimpl()->cost_transform_ = transform;
|
||||
}
|
||||
|
||||
const ordered<SolutionBaseConstPtr>& Stage::solutions() const {
|
||||
return pimpl()->solutions_;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user