From f3fec26f5a3281996a037d23a259a916faa260a9 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 20 May 2020 14:27:33 +0200 Subject: [PATCH] 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. --- core/include/moveit/task_constructor/stage.h | 21 +++++++++++++++++++ .../include/moveit/task_constructor/stage_p.h | 1 + core/src/stage.cpp | 21 +++++++++++++++++-- 3 files changed, 41 insertions(+), 2 deletions(-) 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_; }