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;
|
||||
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;
|
||||
|
||||
@ -184,8 +184,6 @@ 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)
|
||||
|
||||
|
||||
@ -318,18 +318,15 @@ class SolutionSequence : public SolutionBase
|
||||
public:
|
||||
using container_type = std::vector<const SolutionBase*>;
|
||||
|
||||
explicit SolutionSequence();
|
||||
|
||||
SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr);
|
||||
explicit SolutionSequence() : SolutionBase() {}
|
||||
SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr)
|
||||
: SolutionBase(creator, cost), subsolutions_(std::move(subsolutions)) {}
|
||||
|
||||
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;
|
||||
|
||||
@ -339,7 +336,6 @@ public:
|
||||
inline const InterfaceState* internalEnd() const { return subsolutions_.back()->end(); }
|
||||
|
||||
private:
|
||||
CostAggregator aggregator_;
|
||||
/// series of sub solutions
|
||||
container_type subsolutions_;
|
||||
};
|
||||
|
||||
@ -360,9 +360,8 @@ 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()),
|
||||
pimpl()->aggregator_(pimpl()->aggregator_(in.second, current.cost()), out.second));
|
||||
InterfaceState::Priority prio(static_cast<unsigned int>(in.first.size() + 1 + out.first.size()),
|
||||
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()))
|
||||
@ -375,9 +374,7 @@ 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
|
||||
auto sequence{ std::make_shared<SolutionSequence>(std::move(solution), prio.cost(), this) };
|
||||
sequence->setCostAggregator(pimpl()->aggregator_);
|
||||
sorted.insert(std::move(sequence));
|
||||
sorted.insert(std::make_shared<SolutionSequence>(std::move(solution), prio.cost(), this));
|
||||
} else if (prio.depth() > 1) {
|
||||
// update state priorities along the whole partial solution path
|
||||
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)) {}
|
||||
|
||||
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) {
|
||||
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 {
|
||||
for (const auto& stage : pimpl()->children()) {
|
||||
if (stage->pimpl()->canCompute())
|
||||
@ -556,14 +549,11 @@ void SerialContainer::traverse(const SolutionBase& start, const SolutionProcesso
|
||||
else
|
||||
for (SolutionBase* successor : solutions) {
|
||||
trace.push_back(successor);
|
||||
|
||||
double previous_trace_cost { trace_cost };
|
||||
|
||||
trace_cost = pimpl()->aggregator_(trace_cost, successor->cost());
|
||||
trace_cost += successor->cost();
|
||||
|
||||
traverse<dir>(*successor, cb, trace, trace_cost);
|
||||
|
||||
trace_cost = previous_trace_cost;
|
||||
trace_cost -= successor->cost();
|
||||
trace.pop_back();
|
||||
}
|
||||
}
|
||||
|
||||
@ -182,19 +182,10 @@ 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);
|
||||
@ -234,8 +225,7 @@ double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) co
|
||||
double cost{ 0.0 };
|
||||
std::string subcomment;
|
||||
for (auto& solution : subsolutions_) {
|
||||
auto subcost{ solution->computeCost(f, subcomment) };
|
||||
cost = aggregator_(cost, subcost);
|
||||
cost += solution->computeCost(f, subcomment);
|
||||
if (!subcomment.empty()) {
|
||||
if (!comment.empty())
|
||||
comment.append(", ");
|
||||
|
||||
@ -11,8 +11,6 @@
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
using namespace planning_scene;
|
||||
|
||||
@ -351,47 +349,4 @@ 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";
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user