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*.
This commit is contained in:
v4hn 2020-08-21 20:39:48 +02:00
parent 28b98e989c
commit 8ae1060b4d
6 changed files with 87 additions and 10 deletions

View File

@ -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;

View File

@ -184,6 +184,8 @@ protected:
// validate that child's interface matches mine (considering start or end only as determined by mask)
template <unsigned int mask>
void validateInterface(const StagePrivate& child, InterfaceFlags required) const;
SolutionSequence::CostAggregator aggregator_;
};
PIMPL_FUNCTIONS(SerialContainer)

View File

@ -325,15 +325,18 @@ class SolutionSequence : public SolutionBase
public:
using container_type = std::vector<const SolutionBase*>;
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<double(double, double)>;
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_;
};

View File

@ -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<unsigned int>(in.first.size() + 1 + out.first.size()),
in.second + current.cost() + out.second);
InterfaceState::Priority prio(
static_cast<unsigned int>(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<SolutionSequence>(std::move(solution), prio.cost(), this));
auto sequence{ std::make_shared<SolutionSequence>(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<double>{}) {}
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<dir>(*successor, cb, trace, trace_cost);
trace_cost -= successor->cost();
trace_cost = previous_trace_cost;
trace.pop_back();
}
}

View File

@ -182,10 +182,19 @@ double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const
return f(*this, comment);
}
SolutionSequence::SolutionSequence() : SolutionBase(), aggregator_(std::plus<double>{}) {}
SolutionSequence::SolutionSequence(container_type&& subsolutions, double cost, Stage* creator)
: SolutionBase(creator, cost), aggregator_(std::plus<double>{}), 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(", ");

View File

@ -11,6 +11,8 @@
#include <gtest/gtest.h>
#include <algorithm>
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<SerialContainer> container{ getModel() };
cost::Constant constant{ 17.0 };
{
auto s1{ std::make_unique<ForwardMockup>() };
auto s2{ std::make_unique<ForwardMockup>() };
auto s3{ std::make_unique<ForwardMockup>() };
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<ForwardMockup>(),
std::make_unique<ForwardMockup>(),
std::make_unique<ForwardMockup>(),
},
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<ForwardMockup>(),
std::make_unique<ForwardMockup>(),
std::make_unique<ForwardMockup>(),
},
constant);
EXPECT_EQ(container.solutions().front()->cost(), constant.cost)
<< "custom aggregator works without container cost term";
}
}