mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Revert "add CostAggregator for SerialContainer"
This reverts commit dc7ce9bdfac97eb468f5a850adcb27cc118b5fd7. It turns out multiple places in SerialContainer's cost inference expect 0.0 as the neutral element (which is why std::min and multiply did not work). While these additional issues can be fixed, it would make the interface much less elegant. We should consider adding it back if an actual use-case is there to discuss.
This commit is contained in:
parent
cce7380f9d
commit
45e6cdb47d
@ -94,12 +94,6 @@ 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,8 +184,6 @@ 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)
|
||||||
|
|
||||||
|
|||||||
@ -318,18 +318,15 @@ class SolutionSequence : public SolutionBase
|
|||||||
public:
|
public:
|
||||||
using container_type = std::vector<const SolutionBase*>;
|
using container_type = std::vector<const SolutionBase*>;
|
||||||
|
|
||||||
explicit SolutionSequence();
|
explicit SolutionSequence() : SolutionBase() {}
|
||||||
|
SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr)
|
||||||
SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr);
|
: SolutionBase(creator, cost), subsolutions_(std::move(subsolutions)) {}
|
||||||
|
|
||||||
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;
|
||||||
|
|
||||||
@ -339,7 +336,6 @@ 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,9 +360,8 @@ 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(
|
InterfaceState::Priority prio(static_cast<unsigned int>(in.first.size() + 1 + out.first.size()),
|
||||||
static_cast<unsigned int>(in.first.size() + 1 + out.first.size()),
|
in.second + current.cost() + out.second);
|
||||||
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()))
|
||||||
@ -375,9 +374,7 @@ 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
|
||||||
auto sequence{ std::make_shared<SolutionSequence>(std::move(solution), prio.cost(), this) };
|
sorted.insert(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);
|
||||||
@ -396,7 +393,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), aggregator_(std::plus<double>{}) {}
|
: ContainerBasePrivate(me, name) {}
|
||||||
|
|
||||||
void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) {
|
void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) {
|
||||||
InterfaceFlags flags1 = stage1.requiredInterface();
|
InterfaceFlags flags1 = stage1.requiredInterface();
|
||||||
@ -521,10 +518,6 @@ 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())
|
||||||
@ -556,14 +549,11 @@ 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 = previous_trace_cost;
|
trace_cost -= successor->cost();
|
||||||
trace.pop_back();
|
trace.pop_back();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -182,19 +182,10 @@ 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);
|
||||||
@ -234,8 +225,7 @@ 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_) {
|
||||||
auto subcost{ solution->computeCost(f, subcomment) };
|
cost += 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,8 +11,6 @@
|
|||||||
|
|
||||||
#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;
|
||||||
|
|
||||||
@ -351,47 +349,4 @@ 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