mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Implement state-wise Fallbacks
Keep the previous logic around for Generator stages. Note that this only makes sense for *pure* Generators and not for MonitoringGenerator, because for the latter we would expect monitored solutions to be passed individually (similar to pruning).
This commit is contained in:
parent
f32e747645
commit
8719b1c3d6
@ -150,6 +150,7 @@ public:
|
|||||||
void onNewSolution(const SolutionBase& s) override;
|
void onNewSolution(const SolutionBase& s) override;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
class FallbacksPrivate;
|
||||||
/** Plan for different alternatives in sequence.
|
/** Plan for different alternatives in sequence.
|
||||||
*
|
*
|
||||||
* Try to find feasible solutions using first child. Only if this fails,
|
* Try to find feasible solutions using first child. Only if this fails,
|
||||||
@ -158,16 +159,17 @@ public:
|
|||||||
*/
|
*/
|
||||||
class Fallbacks : public ParallelContainerBase
|
class Fallbacks : public ParallelContainerBase
|
||||||
{
|
{
|
||||||
mutable Stage* active_child_ = nullptr;
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
Fallbacks(const std::string& name = "fallbacks") : ParallelContainerBase(name) {}
|
PRIVATE_CLASS(Fallbacks);
|
||||||
|
Fallbacks(const std::string& name = "fallbacks");
|
||||||
|
|
||||||
void reset() override;
|
void reset() override;
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
void compute() override;
|
void compute() override;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
Fallbacks(FallbacksPrivate* impl);
|
||||||
void onNewSolution(const SolutionBase& s) override;
|
void onNewSolution(const SolutionBase& s) override;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@ -131,7 +131,7 @@ public:
|
|||||||
inline const auto& externalToInternalMap() const { return internal_external_.by<EXTERNAL>(); }
|
inline const auto& externalToInternalMap() const { return internal_external_.by<EXTERNAL>(); }
|
||||||
|
|
||||||
/// called by a (direct) child when a solution failed
|
/// called by a (direct) child when a solution failed
|
||||||
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to);
|
virtual void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
ContainerBasePrivate(ContainerBase* me, const std::string& name);
|
ContainerBasePrivate(ContainerBase* me, const std::string& name);
|
||||||
@ -155,6 +155,8 @@ protected:
|
|||||||
/// copy external_state to a child's interface and remember the link in internal_external map
|
/// copy external_state to a child's interface and remember the link in internal_external map
|
||||||
template <Interface::Direction>
|
template <Interface::Direction>
|
||||||
void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
|
void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
|
||||||
|
/// non-template version
|
||||||
|
void copyState(Interface::Direction dir, Interface::iterator external, const InterfacePtr& target, bool updated);
|
||||||
/// lift solution from internal to external level
|
/// lift solution from internal to external level
|
||||||
void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
|
void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
|
||||||
const InterfaceState* internal_to);
|
const InterfaceState* internal_to);
|
||||||
@ -237,6 +239,37 @@ private:
|
|||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(ParallelContainerBase)
|
PIMPL_FUNCTIONS(ParallelContainerBase)
|
||||||
|
|
||||||
|
class FallbacksPrivate : public ParallelContainerBasePrivate
|
||||||
|
{
|
||||||
|
friend class Fallbacks;
|
||||||
|
|
||||||
|
public:
|
||||||
|
FallbacksPrivate(Fallbacks* me, const std::string& name);
|
||||||
|
|
||||||
|
protected:
|
||||||
|
void computeFromExternal();
|
||||||
|
void computeGenerate();
|
||||||
|
|
||||||
|
struct ExternalState
|
||||||
|
{
|
||||||
|
ExternalState(Interface::iterator e, Interface::Direction d, container_type::const_iterator c)
|
||||||
|
: external_state(e), dir(d), stage(c) {}
|
||||||
|
|
||||||
|
Interface::iterator external_state;
|
||||||
|
Interface::Direction dir;
|
||||||
|
container_type::const_iterator stage;
|
||||||
|
};
|
||||||
|
std::deque<ExternalState> pending_states_;
|
||||||
|
container_type::const_iterator current_generator_;
|
||||||
|
|
||||||
|
private:
|
||||||
|
void initializeExternalInterfaces(InterfaceFlags expected) override;
|
||||||
|
template <typename Interface::Direction>
|
||||||
|
void onNewExternalState(Interface::iterator external, bool updated);
|
||||||
|
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
|
||||||
|
};
|
||||||
|
PIMPL_FUNCTIONS(Fallbacks)
|
||||||
|
|
||||||
class WrapperBasePrivate : public ParallelContainerBasePrivate
|
class WrapperBasePrivate : public ParallelContainerBasePrivate
|
||||||
{
|
{
|
||||||
friend class WrapperBase;
|
friend class WrapperBase;
|
||||||
|
|||||||
@ -209,6 +209,14 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa
|
|||||||
internalToExternalMap().insert(std::make_pair(&*internal, &*external));
|
internalToExternalMap().insert(std::make_pair(&*internal, &*external));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void ContainerBasePrivate::copyState(Interface::Direction dir, Interface::iterator external, const InterfacePtr& target,
|
||||||
|
bool updated) {
|
||||||
|
if (dir == Interface::FORWARD)
|
||||||
|
copyState<Interface::FORWARD>(external, target, updated);
|
||||||
|
else
|
||||||
|
copyState<Interface::BACKWARD>(external, target, updated);
|
||||||
|
}
|
||||||
|
|
||||||
void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
|
void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
|
||||||
const InterfaceState* internal_to) {
|
const InterfaceState* internal_to) {
|
||||||
computeCost(*internal_from, *internal_to, *solution);
|
computeCost(*internal_from, *internal_to, *solution);
|
||||||
@ -800,44 +808,142 @@ void Alternatives::onNewSolution(const SolutionBase& s) {
|
|||||||
liftSolution(s);
|
liftSolution(s);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Fallbacks::Fallbacks(const std::string& name) : Fallbacks(new FallbacksPrivate(this, name)) {}
|
||||||
|
|
||||||
|
Fallbacks::Fallbacks(FallbacksPrivate* impl) : ParallelContainerBase(impl) {}
|
||||||
|
|
||||||
void Fallbacks::reset() {
|
void Fallbacks::reset() {
|
||||||
active_child_ = nullptr;
|
|
||||||
ParallelContainerBase::reset();
|
ParallelContainerBase::reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) {
|
void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) {
|
||||||
ParallelContainerBase::init(robot_model);
|
ParallelContainerBase::init(robot_model);
|
||||||
active_child_ = pimpl()->children().front().get();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Fallbacks::canCompute() const {
|
bool Fallbacks::canCompute() const {
|
||||||
while (active_child_) {
|
auto impl { pimpl() };
|
||||||
StagePrivate* child = active_child_->pimpl();
|
|
||||||
if (child->canCompute())
|
|
||||||
return true;
|
|
||||||
|
|
||||||
// active child failed, continue with next
|
if (impl->requiredInterface() == GENERATE) {
|
||||||
auto next = child->it();
|
// current_generator_ is fixed if it produced solutions before
|
||||||
++next;
|
if( !solutions().empty() )
|
||||||
if (next == pimpl()->children().end())
|
return (*impl->current_generator_)->pimpl()->canCompute();
|
||||||
active_child_ = nullptr;
|
|
||||||
else
|
else
|
||||||
active_child_ = next->get();
|
// we still have children to try
|
||||||
|
return impl->current_generator_ != impl->children().end();
|
||||||
}
|
}
|
||||||
return false;
|
else
|
||||||
|
return !pimpl()->pending_states_.empty();
|
||||||
}
|
}
|
||||||
|
|
||||||
void Fallbacks::compute() {
|
void Fallbacks::compute() {
|
||||||
if (!active_child_)
|
auto impl { pimpl() };
|
||||||
return;
|
|
||||||
|
|
||||||
active_child_->pimpl()->runCompute();
|
if(impl->requiredInterface() == GENERATE)
|
||||||
|
impl->computeGenerate();
|
||||||
|
else
|
||||||
|
impl->computeFromExternal();
|
||||||
}
|
}
|
||||||
|
|
||||||
void Fallbacks::onNewSolution(const SolutionBase& s) {
|
void Fallbacks::onNewSolution(const SolutionBase& s) {
|
||||||
liftSolution(s);
|
liftSolution(s);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name)
|
||||||
|
: ParallelContainerBasePrivate(me, name) {}
|
||||||
|
|
||||||
|
void FallbacksPrivate::initializeExternalInterfaces(InterfaceFlags expected) {
|
||||||
|
if (expected & READS_START)
|
||||||
|
starts().reset(new Interface([this](Interface::iterator external, bool updated) {
|
||||||
|
this->onNewExternalState<Interface::FORWARD>(external, updated);
|
||||||
|
}));
|
||||||
|
if (expected & READS_END)
|
||||||
|
ends().reset(new Interface([this](Interface::iterator external, bool updated) {
|
||||||
|
this->onNewExternalState<Interface::BACKWARD>(external, updated);
|
||||||
|
}));
|
||||||
|
|
||||||
|
// we've got to set this somewhere once the interface flags are known.
|
||||||
|
// so we might as well do it here
|
||||||
|
if(expected == GENERATE)
|
||||||
|
current_generator_ = children().begin();
|
||||||
|
}
|
||||||
|
|
||||||
|
void FallbacksPrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {
|
||||||
|
// only react to failure if it's the last possible candidate failing
|
||||||
|
// otherwise there might still be a feasible solution
|
||||||
|
if(&child == &*children().back())
|
||||||
|
ContainerBasePrivate::onNewFailure(child, from, to);
|
||||||
|
}
|
||||||
|
|
||||||
|
void FallbacksPrivate::computeGenerate() {
|
||||||
|
if(solutions_.empty())
|
||||||
|
// move to first generator that can run
|
||||||
|
while(current_generator_ != children().end() && !(*current_generator_)->pimpl()->canCompute())
|
||||||
|
++current_generator_;
|
||||||
|
|
||||||
|
if(current_generator_ == children().end())
|
||||||
|
return;
|
||||||
|
|
||||||
|
// run ALL possible computations (on new state)
|
||||||
|
// this is needed to decide whether it should be passed to the next child too
|
||||||
|
while ((*current_generator_)->pimpl()->canCompute()){
|
||||||
|
(*current_generator_)->pimpl()->runCompute();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename Interface::Direction dir>
|
||||||
|
void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool updated) {
|
||||||
|
// TODO(v4hn): updated is not implemented
|
||||||
|
if(updated){
|
||||||
|
ROS_DEBUG_NAMED("Fallback", "updating external states is not supported in Fallbacks");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
pending_states_.push_back(ExternalState(external, dir, children().begin()));
|
||||||
|
}
|
||||||
|
|
||||||
|
void FallbacksPrivate::computeFromExternal(){
|
||||||
|
assert(!pending_states_.empty());
|
||||||
|
auto spec { pending_states_.front() };
|
||||||
|
|
||||||
|
pending_states_.pop_front();
|
||||||
|
|
||||||
|
ROS_DEBUG_STREAM_NAMED("Fallback", "Push external state to '" << (*spec.stage)->name() << "'");
|
||||||
|
// feed a new state
|
||||||
|
copyState(spec.dir,
|
||||||
|
spec.external_state,
|
||||||
|
(*spec.stage)->pimpl()->pullInterface(spec.dir),
|
||||||
|
false);
|
||||||
|
|
||||||
|
const auto& stage { (*spec.stage)->pimpl() };
|
||||||
|
|
||||||
|
try {
|
||||||
|
// run ALL possible computations (on new state)
|
||||||
|
// this is needed to decide whether it should be passed to the next child too
|
||||||
|
while (stage->canCompute()){
|
||||||
|
stage->runCompute();
|
||||||
|
}
|
||||||
|
} catch (const Property::error& e) {
|
||||||
|
stage->me()->reportPropertyError(e);
|
||||||
|
}
|
||||||
|
|
||||||
|
auto has_solutions{ [](const InterfaceState& state, Interface::Direction dir){
|
||||||
|
return dir == Interface::FORWARD
|
||||||
|
? !state.outgoingTrajectories().empty()
|
||||||
|
: !state.incomingTrajectories().empty();
|
||||||
|
} };
|
||||||
|
|
||||||
|
if(!has_solutions(*spec.external_state, spec.dir)){
|
||||||
|
ROS_DEBUG_STREAM_NAMED("Fallback", "Child '" << (*spec.stage)->name() << "' failed to generate a solution, schedule state with next child (if any)");
|
||||||
|
if(++spec.stage != children().cend())
|
||||||
|
pending_states_.push_back(spec);
|
||||||
|
else
|
||||||
|
// prune solution path if there is no way to extend external_state through Fallbacks
|
||||||
|
ContainerBasePrivate::onNewFailure(*children().back(),
|
||||||
|
spec.dir == Interface::FORWARD ? &*spec.external_state : nullptr,
|
||||||
|
spec.dir == Interface::BACKWARD ? nullptr : &*spec.external_state);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}
|
MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}
|
||||||
|
|
||||||
void MergerPrivate::resolveInterface(InterfaceFlags expected) {
|
void MergerPrivate::resolveInterface(InterfaceFlags expected) {
|
||||||
|
|||||||
@ -17,7 +17,7 @@ using namespace moveit::task_constructor;
|
|||||||
|
|
||||||
using FallbacksFixtureGenerator = TaskTestBase;
|
using FallbacksFixtureGenerator = TaskTestBase;
|
||||||
|
|
||||||
TEST_F(FallbacksFixtureGenerator, DISABLED_stayWithFirstSuccessful) {
|
TEST_F(FallbacksFixtureGenerator, stayWithFirstSuccessful) {
|
||||||
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
|
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
|
||||||
fallback->add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(INF)));
|
fallback->add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(INF)));
|
||||||
fallback->add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(1.0)));
|
fallback->add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(1.0)));
|
||||||
@ -55,7 +55,7 @@ TEST_F(FallbacksFixturePropagate, failingWithFailedSolutions) {
|
|||||||
EXPECT_EQ(t.solutions().size(), 0u);
|
EXPECT_EQ(t.solutions().size(), 0u);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStageOnly) {
|
TEST_F(FallbacksFixturePropagate, computeFirstSuccessfulStageOnly) {
|
||||||
t.add(std::make_unique<GeneratorMockup>());
|
t.add(std::make_unique<GeneratorMockup>());
|
||||||
|
|
||||||
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
|
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
|
||||||
@ -117,7 +117,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_MultipleActivePendingStates) {
|
|||||||
// check that first solution is not marked as pruned
|
// check that first solution is not marked as pruned
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions) {
|
TEST_F(FallbacksFixturePropagate, successfulWithMixedSolutions) {
|
||||||
t.add(std::make_unique<GeneratorMockup>());
|
t.add(std::make_unique<GeneratorMockup>());
|
||||||
|
|
||||||
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
|
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
|
||||||
@ -129,7 +129,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions) {
|
|||||||
EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0));
|
EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0));
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions2) {
|
TEST_F(FallbacksFixturePropagate, successfulWithMixedSolutions2) {
|
||||||
t.add(std::make_unique<GeneratorMockup>());
|
t.add(std::make_unique<GeneratorMockup>());
|
||||||
|
|
||||||
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
|
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
|
||||||
@ -141,7 +141,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions2) {
|
|||||||
EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0));
|
EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0));
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(FallbacksFixturePropagate, DISABLED_ActiveChildReset) {
|
TEST_F(FallbacksFixturePropagate, activeChildReset) {
|
||||||
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, INF, 3.0 })));
|
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, INF, 3.0 })));
|
||||||
|
|
||||||
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
|
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user