diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 2336b633..397a56a6 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -214,6 +214,27 @@ public: void removeSolutionCallback(SolutionCallbackList::const_iterator which); using CostTerm = std::function; + using CostTransform = std::function; + + /* \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& solutions() const; const std::list& failures() const; diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 3fbabfa0..ad0f1bfa 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -173,6 +173,7 @@ protected: // user-configurable cost estimator Stage::CostTerm cost_term_; + Stage::CostTransform cost_transform_; // The total compute time std::chrono::duration total_compute_time_; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 0e29e674..37396f8f 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -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(&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& Stage::solutions() const { return pimpl()->solutions_; }