mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
28b98e989c
commit
8ae1060b4d
@ -94,6 +94,12 @@ public:
|
|||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
void compute() 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:
|
protected:
|
||||||
/// called by a (direct) child when a new solution becomes available
|
/// called by a (direct) child when a new solution becomes available
|
||||||
void onNewSolution(const SolutionBase& s) override;
|
void onNewSolution(const SolutionBase& s) override;
|
||||||
|
|||||||
@ -184,6 +184,8 @@ protected:
|
|||||||
// validate that child's interface matches mine (considering start or end only as determined by mask)
|
// validate that child's interface matches mine (considering start or end only as determined by mask)
|
||||||
template <unsigned int mask>
|
template <unsigned int mask>
|
||||||
void validateInterface(const StagePrivate& child, InterfaceFlags required) const;
|
void validateInterface(const StagePrivate& child, InterfaceFlags required) const;
|
||||||
|
|
||||||
|
SolutionSequence::CostAggregator aggregator_;
|
||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(SerialContainer)
|
PIMPL_FUNCTIONS(SerialContainer)
|
||||||
|
|
||||||
|
|||||||
@ -325,15 +325,18 @@ class SolutionSequence : public SolutionBase
|
|||||||
public:
|
public:
|
||||||
using container_type = std::vector<const SolutionBase*>;
|
using container_type = std::vector<const SolutionBase*>;
|
||||||
|
|
||||||
explicit SolutionSequence() : SolutionBase() {}
|
explicit SolutionSequence();
|
||||||
SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr)
|
|
||||||
: SolutionBase(creator, cost), subsolutions_(std::move(subsolutions)) {}
|
SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr);
|
||||||
|
|
||||||
void push_back(const SolutionBase& solution);
|
void push_back(const SolutionBase& solution);
|
||||||
|
|
||||||
/// append all subsolutions to solution
|
/// append all subsolutions to solution
|
||||||
void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override;
|
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
|
/// aggregate costs along the sequence
|
||||||
double computeCost(const CostTerm& cost, std::string& comment) const override;
|
double computeCost(const CostTerm& cost, std::string& comment) const override;
|
||||||
|
|
||||||
@ -343,6 +346,7 @@ public:
|
|||||||
inline const InterfaceState* internalEnd() const { return subsolutions_.back()->end(); }
|
inline const InterfaceState* internalEnd() const { return subsolutions_.back()->end(); }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
CostAggregator aggregator_;
|
||||||
/// series of sub solutions
|
/// series of sub solutions
|
||||||
container_type subsolutions_;
|
container_type subsolutions_;
|
||||||
};
|
};
|
||||||
|
|||||||
@ -360,8 +360,9 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
|
|||||||
solution.reserve(children.size());
|
solution.reserve(children.size());
|
||||||
for (auto& in : incoming.solutions) {
|
for (auto& in : incoming.solutions) {
|
||||||
for (auto& out : outgoing.solutions) {
|
for (auto& out : outgoing.solutions) {
|
||||||
InterfaceState::Priority prio(static_cast<unsigned int>(in.first.size() + 1 + out.first.size()),
|
InterfaceState::Priority prio(
|
||||||
in.second + current.cost() + out.second);
|
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?
|
// found a complete solution path connecting start to end?
|
||||||
if (prio.depth() == children.size()) {
|
if (prio.depth() == children.size()) {
|
||||||
if (std::isinf(prio.cost()))
|
if (std::isinf(prio.cost()))
|
||||||
@ -374,7 +375,9 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
|
|||||||
// insert outgoing solutions in normal order
|
// insert outgoing solutions in normal order
|
||||||
solution.insert(solution.end(), out.first.begin(), out.first.end());
|
solution.insert(solution.end(), out.first.begin(), out.first.end());
|
||||||
// store solution in sorted list
|
// 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) {
|
} else if (prio.depth() > 1) {
|
||||||
// update state priorities along the whole partial solution path
|
// update state priorities along the whole partial solution path
|
||||||
updateStateCosts(in.first, prio);
|
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)) {}
|
SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new SerialContainerPrivate(this, name)) {}
|
||||||
|
|
||||||
SerialContainerPrivate::SerialContainerPrivate(SerialContainer* me, const std::string& 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) {
|
void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) {
|
||||||
InterfaceFlags flags1 = stage1.requiredInterface();
|
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 {
|
bool SerialContainer::canCompute() const {
|
||||||
for (const auto& stage : pimpl()->children()) {
|
for (const auto& stage : pimpl()->children()) {
|
||||||
if (stage->pimpl()->canCompute())
|
if (stage->pimpl()->canCompute())
|
||||||
@ -549,11 +556,14 @@ void SerialContainer::traverse(const SolutionBase& start, const SolutionProcesso
|
|||||||
else
|
else
|
||||||
for (SolutionBase* successor : solutions) {
|
for (SolutionBase* successor : solutions) {
|
||||||
trace.push_back(successor);
|
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);
|
traverse<dir>(*successor, cb, trace, trace_cost);
|
||||||
|
|
||||||
trace_cost -= successor->cost();
|
trace_cost = previous_trace_cost;
|
||||||
trace.pop_back();
|
trace.pop_back();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -182,10 +182,19 @@ double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const
|
|||||||
return f(*this, comment);
|
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) {
|
void SolutionSequence::push_back(const SolutionBase& solution) {
|
||||||
subsolutions_.push_back(&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 {
|
void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const {
|
||||||
moveit_task_constructor_msgs::SubSolution sub_msg;
|
moveit_task_constructor_msgs::SubSolution sub_msg;
|
||||||
SolutionBase::fillInfo(sub_msg.info, introspection);
|
SolutionBase::fillInfo(sub_msg.info, introspection);
|
||||||
@ -225,7 +234,8 @@ double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) co
|
|||||||
double cost{ 0.0 };
|
double cost{ 0.0 };
|
||||||
std::string subcomment;
|
std::string subcomment;
|
||||||
for (auto& solution : subsolutions_) {
|
for (auto& solution : subsolutions_) {
|
||||||
cost += solution->computeCost(f, subcomment);
|
auto subcost{ solution->computeCost(f, subcomment) };
|
||||||
|
cost = aggregator_(cost, subcost);
|
||||||
if (!subcomment.empty()) {
|
if (!subcomment.empty()) {
|
||||||
if (!comment.empty())
|
if (!comment.empty())
|
||||||
comment.append(", ");
|
comment.append(", ");
|
||||||
|
|||||||
@ -11,6 +11,8 @@
|
|||||||
|
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
|
||||||
using namespace moveit::task_constructor;
|
using namespace moveit::task_constructor;
|
||||||
using namespace planning_scene;
|
using namespace planning_scene;
|
||||||
|
|
||||||
@ -323,4 +325,47 @@ TEST(CostTerm, CompositeSolutionsContainerCost) {
|
|||||||
<< "container cost term overwrites stage costs";
|
<< "container cost term overwrites stage costs";
|
||||||
EXPECT_EQ(s1_ptr->solutions().front()->cost(), STAGE_COST) << "child cost is not affected";
|
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";
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user