From 8ae1060b4df13ec4958daa4587669788219eb93b Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 21 Aug 2020 20:39:48 +0200 Subject: [PATCH] add CostAggregator for SerialContainer It can be useful to change the default addition to other operators. The simplest example is applying a Constant cost term to a container. As the tests show, the visitor-based cost computation ends up adding cost::Constant *for each subtrajectory*. --- .../moveit/task_constructor/container.h | 6 +++ .../moveit/task_constructor/container_p.h | 2 + .../include/moveit/task_constructor/storage.h | 10 +++-- core/src/container.cpp | 22 ++++++--- core/src/storage.cpp | 12 ++++- core/test/test_cost_terms.cpp | 45 +++++++++++++++++++ 6 files changed, 87 insertions(+), 10 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 27b2fde0..8ca032e7 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -94,6 +94,12 @@ public: bool canCompute() const override; void compute() override; + /** Specify how to aggregate costs of childrens' solutions + * + * By default costs are added up + */ + void setCostAggregator(const SolutionSequence::CostAggregator& agg); + protected: /// called by a (direct) child when a new solution becomes available void onNewSolution(const SolutionBase& s) override; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 35a903ad..cad54fcd 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -184,6 +184,8 @@ protected: // validate that child's interface matches mine (considering start or end only as determined by mask) template void validateInterface(const StagePrivate& child, InterfaceFlags required) const; + + SolutionSequence::CostAggregator aggregator_; }; PIMPL_FUNCTIONS(SerialContainer) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 58dcc0cb..20d646ec 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -325,15 +325,18 @@ class SolutionSequence : public SolutionBase public: using container_type = std::vector; - explicit SolutionSequence() : SolutionBase() {} - SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr) - : SolutionBase(creator, cost), subsolutions_(std::move(subsolutions)) {} + explicit SolutionSequence(); + + SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr); void push_back(const SolutionBase& solution); /// append all subsolutions to solution void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override; + using CostAggregator = std::function; + void setCostAggregator(const CostAggregator&); + /// aggregate costs along the sequence double computeCost(const CostTerm& cost, std::string& comment) const override; @@ -343,6 +346,7 @@ public: inline const InterfaceState* internalEnd() const { return subsolutions_.back()->end(); } private: + CostAggregator aggregator_; /// series of sub solutions container_type subsolutions_; }; diff --git a/core/src/container.cpp b/core/src/container.cpp index 18892591..1c1b0993 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -360,8 +360,9 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { solution.reserve(children.size()); for (auto& in : incoming.solutions) { for (auto& out : outgoing.solutions) { - InterfaceState::Priority prio(static_cast(in.first.size() + 1 + out.first.size()), - in.second + current.cost() + out.second); + InterfaceState::Priority prio( + static_cast(in.first.size() + 1 + out.first.size()), + pimpl()->aggregator_(pimpl()->aggregator_(in.second, current.cost()), out.second)); // found a complete solution path connecting start to end? if (prio.depth() == children.size()) { if (std::isinf(prio.cost())) @@ -374,7 +375,9 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { // insert outgoing solutions in normal order solution.insert(solution.end(), out.first.begin(), out.first.end()); // store solution in sorted list - sorted.insert(std::make_shared(std::move(solution), prio.cost(), this)); + auto sequence{ std::make_shared(std::move(solution), prio.cost(), this) }; + sequence->setCostAggregator(pimpl()->aggregator_); + sorted.insert(std::move(sequence)); } else if (prio.depth() > 1) { // update state priorities along the whole partial solution path updateStateCosts(in.first, prio); @@ -393,7 +396,7 @@ SerialContainer::SerialContainer(SerialContainerPrivate* impl) : ContainerBase(i SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new SerialContainerPrivate(this, name)) {} SerialContainerPrivate::SerialContainerPrivate(SerialContainer* me, const std::string& name) - : ContainerBasePrivate(me, name) {} + : ContainerBasePrivate(me, name), aggregator_(std::plus{}) {} void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) { InterfaceFlags flags1 = stage1.requiredInterface(); @@ -518,6 +521,10 @@ void SerialContainerPrivate::validateConnectivity() const { } } +void SerialContainer::setCostAggregator(const SolutionSequence::CostAggregator& agg) { + pimpl()->aggregator_ = agg; +} + bool SerialContainer::canCompute() const { for (const auto& stage : pimpl()->children()) { if (stage->pimpl()->canCompute()) @@ -549,11 +556,14 @@ void SerialContainer::traverse(const SolutionBase& start, const SolutionProcesso else for (SolutionBase* successor : solutions) { trace.push_back(successor); - trace_cost += successor->cost(); + + double previous_trace_cost { trace_cost }; + + trace_cost = pimpl()->aggregator_(trace_cost, successor->cost()); traverse(*successor, cb, trace, trace_cost); - trace_cost -= successor->cost(); + trace_cost = previous_trace_cost; trace.pop_back(); } } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index ff096a12..1133257e 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -182,10 +182,19 @@ double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const return f(*this, comment); } +SolutionSequence::SolutionSequence() : SolutionBase(), aggregator_(std::plus{}) {} + +SolutionSequence::SolutionSequence(container_type&& subsolutions, double cost, Stage* creator) + : SolutionBase(creator, cost), aggregator_(std::plus{}), subsolutions_(std::move(subsolutions)) {} + void SolutionSequence::push_back(const SolutionBase& solution) { subsolutions_.push_back(&solution); } +void SolutionSequence::setCostAggregator(const CostAggregator& agg) { + aggregator_ = agg; +} + void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const { moveit_task_constructor_msgs::SubSolution sub_msg; SolutionBase::fillInfo(sub_msg.info, introspection); @@ -225,7 +234,8 @@ double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) co double cost{ 0.0 }; std::string subcomment; for (auto& solution : subsolutions_) { - cost += solution->computeCost(f, subcomment); + auto subcost{ solution->computeCost(f, subcomment) }; + cost = aggregator_(cost, subcost); if (!subcomment.empty()) { if (!comment.empty()) comment.append(", "); diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index 0a3d24af..ca6f382f 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -11,6 +11,8 @@ #include +#include + using namespace moveit::task_constructor; using namespace planning_scene; @@ -323,4 +325,47 @@ TEST(CostTerm, CompositeSolutionsContainerCost) { << "container cost term overwrites stage costs"; EXPECT_EQ(s1_ptr->solutions().front()->cost(), STAGE_COST) << "child cost is not affected"; } + +TEST(CostTerm, CompositeSolutionsContainerAggregator) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + Standalone container{ getModel() }; + + cost::Constant constant{ 17.0 }; + + { + auto s1{ std::make_unique() }; + auto s2{ std::make_unique() }; + auto s3{ std::make_unique() }; + + container.computeWithStageCost({ std::move(s1), std::move(s2), std::move(s3) }, constant); + EXPECT_EQ(container.solutions().front()->cost(), 3 * constant.cost) << "default cost aggregates through stages"; + } + + { + container.setCostAggregator([](double a, double b) { return std::max(a, b); }); + container.computeWithContainerCost( + { + std::make_unique(), + std::make_unique(), + std::make_unique(), + }, + constant); + + EXPECT_EQ(container.solutions().front()->cost(), constant.cost) + << "custom aggregator works with explicit cost term in container"; + } + + { + container.setCostAggregator([](auto a, auto b) { return std::max(a, b); }); + container.computeWithStageCost( + { + std::make_unique(), + std::make_unique(), + std::make_unique(), + }, + constant); + + EXPECT_EQ(container.solutions().front()->cost(), constant.cost) + << "custom aggregator works without container cost term"; + } }