basic merger functionality

This commit is contained in:
Robert Haschke 2018-04-07 21:16:14 +02:00
parent 19fa7349d3
commit b7b7a916df
3 changed files with 347 additions and 9 deletions

View File

@ -132,9 +132,9 @@ class ParallelContainerBasePrivate;
class ParallelContainerBase; class ParallelContainerBase;
/** Parallel containers allow for alternative planning stages /** Parallel containers allow for alternative planning stages
* Parallel containers can come in different flavours: * Parallel containers can come in different flavours:
* - alternatives: each child stage can contribute a solution * - Alternatives: each child stage can contribute a solution
* - fallbacks: the children are considered in series * - Fallbacks: the children are considered in series
* - merged: solutions of all children (actuating disjoint groups) * - Merger: solutions of all children (actuating disjoint groups)
* are merged into a single solution for parallel execution * are merged into a single solution for parallel execution
*/ */
class ParallelContainerBase : public ContainerBase class ParallelContainerBase : public ContainerBase
@ -175,6 +175,10 @@ protected:
trajectory.setCost(cost); trajectory.setCost(cost);
spawn(std::move(state), std::move(trajectory)); spawn(std::move(state), std::move(trajectory));
} }
/// propagate a solution forwards
void sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& trajectory);
/// propagate a solution backwards
void sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& trajectory);
}; };
@ -212,6 +216,25 @@ public:
}; };
class MergerPrivate;
/** Plan for different sub tasks in parallel and finally merge all sub solutions into a single trajectory */
class Merger : public ParallelContainerBase
{
public:
PRIVATE_CLASS(Merger)
Merger(const std::string &name = "merger");
void reset() override;
void init(const core::RobotModelConstPtr &robot_model) override;
bool canCompute() const override;
bool compute() override;
protected:
Merger(MergerPrivate* impl);
void onNewSolution(const SolutionBase& s) override;
};
class WrapperBasePrivate; class WrapperBasePrivate;
/** A wrapper wraps a single child stage, which can be accessed via wrapped(). /** A wrapper wraps a single child stage, which can be accessed via wrapped().
* *

View File

@ -39,11 +39,17 @@
#pragma once #pragma once
#include <moveit/task_constructor/container.h> #include <moveit/task_constructor/container.h>
#include <moveit/macros/class_forward.h>
#include "stage_p.h" #include "stage_p.h"
#include <map> #include <map>
#include <climits> #include <climits>
namespace moveit { namespace core {
MOVEIT_CLASS_FORWARD(JointModelGroup)
MOVEIT_CLASS_FORWARD(RobotState)
} }
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
/* A container needs to decouple its own (external) interfaces /* A container needs to decouple its own (external) interfaces
@ -121,7 +127,10 @@ protected:
void liftSolution(SolutionBase& solution, void liftSolution(SolutionBase& solution,
const InterfaceState *internal_from, const InterfaceState *internal_to); const InterfaceState *internal_from, const InterfaceState *internal_to);
protected: auto& internalToExternalMap() { return internal_to_external_; }
const auto& internalToExternalMap() const { return internal_to_external_; }
private:
container_type children_; container_type children_;
// map start/end states of children (internal) to corresponding states in our external interfaces // map start/end states of children (internal) to corresponding states in our external interfaces
@ -205,6 +214,17 @@ public:
// called by parent asking for pruning of this' interface // called by parent asking for pruning of this' interface
void pruneInterface(InterfaceFlags accepted) override; void pruneInterface(InterfaceFlags accepted) override;
// grant access to protected methods in ParallelContainerBase
inline void spawn(InterfaceState &&state, SubTrajectory&& t) {
static_cast<ParallelContainerBase*>(me_)->spawn(std::move(state), std::move(t));
}
inline void sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) {
static_cast<ParallelContainerBase*>(me_)->sendForward(from, std::move(to), std::move(t));
}
inline void sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& t) {
static_cast<ParallelContainerBase*>(me_)->sendBackward(std::move(from), to, std::move(t));
}
private: private:
/// callback for new externally received states /// callback for new externally received states
void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated); void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated);
@ -232,4 +252,32 @@ public:
}; };
PIMPL_FUNCTIONS(WrapperBase) PIMPL_FUNCTIONS(WrapperBase)
class MergerPrivate : public ParallelContainerBasePrivate {
friend class Merger;
moveit::core::JointModelGroupPtr jmg_merged_;
typedef std::vector<const SubTrajectory*> ChildSolutionList;
typedef std::map<const StagePrivate*, ChildSolutionList> ChildSolutionMap;
// map from external source state (iterator) to all corresponding children's solutions
std::map<InterfaceState*, ChildSolutionMap> source_state_to_solutions_;
public:
typedef std::function<void(SubTrajectory&&)> Spawner;
MergerPrivate(Merger* me, const std::string& name);
InterfaceFlags requiredInterface() const override;
void onNewPropagateSolution(const SolutionBase& s);
void onNewGeneratorSolution(const SolutionBase& s);
void mergeAnyCombination(const ChildSolutionMap& all_solutions, const SolutionBase& current,
const moveit::core::RobotState& state, const Spawner& spawner);
void merge(const ChildSolutionList& sub_solutions, const moveit::core::RobotState& state, const Spawner& spawner);
void sendForward(SubTrajectory&& t, const InterfaceState* from);
void sendBackward(SubTrajectory&& t, const InterfaceState* to);
void spawn(SubTrajectory&& t);
};
PIMPL_FUNCTIONS(Merger)
} } } }

View File

@ -35,8 +35,9 @@
/* Authors: Robert Haschke */ /* Authors: Robert Haschke */
#include <moveit/task_constructor/container_p.h> #include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/merge.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/task_constructor/introspection.h>
#include <ros/console.h> #include <ros/console.h>
#include <memory> #include <memory>
@ -479,9 +480,13 @@ void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model)
// initialize this' pull interfaces if first/last child pulls // initialize this' pull interfaces if first/last child pulls
if (const InterfacePtr& target = (*start)->pimpl()->starts()) if (const InterfacePtr& target = (*start)->pimpl()->starts())
impl->starts_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, _1, std::cref(target), _2))); impl->starts_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl,
std::placeholders::_1, std::cref(target),
std::placeholders::_2)));
if (const InterfacePtr& target = (*last)->pimpl()->ends()) if (const InterfacePtr& target = (*last)->pimpl()->ends())
impl->ends_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, _1, std::cref(target), _2))); impl->ends_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl,
std::placeholders::_1, std::cref(target),
std::placeholders::_2)));
} }
// called by parent asking for pruning of this' interface // called by parent asking for pruning of this' interface
@ -767,11 +772,11 @@ void ParallelContainerBase::init(const moveit::core::RobotModelConstPtr& robot_m
// initialize this' pull connections // initialize this' pull connections
impl->starts().reset(required & READS_START impl->starts().reset(required & READS_START
? new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState, ? new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState,
impl, Interface::FORWARD, _1, _2)) impl, Interface::FORWARD, std::placeholders::_1, std::placeholders::_2))
: nullptr); : nullptr);
impl->ends().reset(required & READS_END impl->ends().reset(required & READS_END
? new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState, ? new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState,
impl, Interface::BACKWARD, _1, _2)) impl, Interface::BACKWARD, std::placeholders::_1, std::placeholders::_2))
: nullptr); : nullptr);
// initialize push connections of children according to their demands // initialize push connections of children according to their demands
@ -875,6 +880,46 @@ void ParallelContainerBase::spawn(InterfaceState &&state, SubTrajectory&& t)
impl->newSolution(*it); impl->newSolution(*it);
} }
void ParallelContainerBase::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t)
{
auto impl = pimpl();
assert(impl->nextStarts());
// store newly created solution (otherwise it's lost)
auto it = impl->created_solutions_.insert(impl->created_solutions_.end(), std::move(t));
it->setStartState(from);
if (it->isFailure()) {
auto state_it = impl->states_.insert(impl->states_.end(), std::move(to));
it->setEndState(*state_it);
impl->failures_.push_back(&*it);
} else {
impl->nextStarts()->add(std::move(to), &*it, NULL);
impl->solutions_.insert(&*it);
}
impl->newSolution(*it);
}
void ParallelContainerBase::sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& t)
{
auto impl = pimpl();
assert(impl->prevEnds());
// store newly created solution (otherwise it's lost)
auto it = impl->created_solutions_.insert(impl->created_solutions_.end(), std::move(t));
it->setEndState(to);
if (it->isFailure()) {
auto state_it = impl->states_.insert(impl->states_.end(), std::move(from));
it->setStartState(*state_it);
impl->failures_.push_back(&*it);
} else {
impl->prevEnds()->add(std::move(from), NULL, &*it);
impl->solutions_.insert(&*it);
}
impl->newSolution(*it);
}
WrapperBasePrivate::WrapperBasePrivate(WrapperBase *me, const std::string &name) WrapperBasePrivate::WrapperBasePrivate(WrapperBase *me, const std::string &name)
: ParallelContainerBasePrivate(me, name) : ParallelContainerBasePrivate(me, name)
@ -982,4 +1027,226 @@ bool Fallbacks::compute()
return false; return false;
} }
MergerPrivate::MergerPrivate(Merger *me, const std::string &name)
: ParallelContainerBasePrivate(me, name)
{}
InterfaceFlags MergerPrivate::requiredInterface() const
{
if (children().size() < 2)
throw InitStageException(*me_, "Need 2 children at least.");
InterfaceFlags required = ParallelContainerBasePrivate::requiredInterface();
// all children need to share a common interface
for (const Stage::pointer& stage : children()) {
InterfaceFlags current = stage->pimpl()->requiredInterface();
if (current != required)
throw InitStageException(*stage, "Interface doesn't match the common one.");
}
switch (required) {
case PROPAGATE_FORWARDS:
case PROPAGATE_BACKWARDS:
case UNKNOWN:
break; // these are supported
case GENERATE:
throw InitStageException(*me_, "Generator stages not yet supported.");
case CONNECT:
throw InitStageException(*me_, "Cannot merge connecting stages. Use Connect.");
default:
throw InitStageException(*me_, "Children's interface not supported.");
}
return required;
}
Merger::Merger(const std::string &name)
: Merger(new MergerPrivate(this, name))
{}
void Merger::reset()
{
ParallelContainerBase::reset();
auto impl = pimpl();
impl->jmg_merged_.reset();
impl->source_state_to_solutions_.clear();
}
void Merger::init(const core::RobotModelConstPtr& robot_model)
{
ParallelContainerBase::init(robot_model);
}
Merger::Merger(MergerPrivate *impl)
: ParallelContainerBase(impl)
{
}
bool Merger::canCompute() const
{
for (const auto& stage : pimpl()->children())
if (stage->pimpl()->canCompute())
return true;
return false;
}
bool Merger::compute()
{
bool success = false;
for (const auto& stage : pimpl()->children()) {
try {
success |= stage->pimpl()->compute();
} catch (const Property::error &e) {
stage->reportPropertyError(e);
}
}
return success;
}
void Merger::onNewSolution(const SolutionBase& s)
{
auto impl = pimpl();
switch (impl->interfaceFlags()) {
case PROPAGATE_FORWARDS:
case PROPAGATE_BACKWARDS:
impl->onNewPropagateSolution(s);
break;
case GENERATE:
impl->onNewGeneratorSolution(s);
break;
default:
assert(false);
}
}
void MergerPrivate::onNewPropagateSolution(const SolutionBase& s)
{
const SubTrajectory* trajectory = dynamic_cast<const SubTrajectory*>(&s);
if (!trajectory) {
ROS_ERROR_NAMED("Merger", "Only simple trajectories are supported");
return;
}
InterfaceFlags dir = interfaceFlags();
assert(dir == PROPAGATE_FORWARDS || dir == PROPAGATE_BACKWARDS);
// internal source state
const InterfaceState* source_state = (dir == PROPAGATE_FORWARDS) ? s.start() : s.end();
// map to external source state that is shared by all children
auto source_it = internalToExternalMap().find(source_state);
// internal->external mapping for source state should have been created
assert(source_it != internalToExternalMap().end());
InterfaceState* external_source_state = &*source_it->second;
// retrieve (or create if necessary) the ChildSolutionMap for the given external source state
ChildSolutionMap& all_solutions = source_state_to_solutions_.insert(std::make_pair(external_source_state, ChildSolutionMap())).first->second;
// retrieve (or create if necessary) the ChildSolutionList corresponding to the child
ChildSolutionList& child_solutions = all_solutions.insert(std::make_pair(s.creator(), ChildSolutionList())).first->second;
// insert the new child solution into the list
child_solutions.push_back(trajectory);
// do we have solutions for all children?
if (all_solutions.size() < children().size()) return;
assert(all_solutions.size() == children().size());
// combine the new solution with all solutions from other children
auto spawner = dir == PROPAGATE_FORWARDS ? &MergerPrivate::sendForward : &MergerPrivate::sendBackward;
mergeAnyCombination(all_solutions, s, external_source_state->scene()->getCurrentState(),
std::bind(spawner, this, std::placeholders::_1, external_source_state));
}
void MergerPrivate::sendForward(SubTrajectory&& t, const InterfaceState* from)
{
// generate target state
planning_scene::PlanningScenePtr to = from->scene()->diff();
to->setCurrentState(t.trajectory()->getLastWayPoint());
ParallelContainerBasePrivate::sendForward(*from, InterfaceState(to), std::move(t));
}
void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to)
{
// generate target state
planning_scene::PlanningScenePtr from = to->scene()->diff();
from->setCurrentState(t.trajectory()->getFirstWayPoint());
ParallelContainerBasePrivate::sendBackward(InterfaceState(from), *to, std::move(t));
}
void MergerPrivate::onNewGeneratorSolution(const SolutionBase& s)
{
// TODO
}
void MergerPrivate::spawn(SubTrajectory&& t)
{
}
void MergerPrivate::mergeAnyCombination(const ChildSolutionMap& all_solutions, const SolutionBase& current,
const moveit::core::RobotState& state, const Spawner& spawner)
{
std::vector<size_t> indeces; // which solution index was considered last for i-th child?
indeces.reserve(children().size());
ChildSolutionList sub_solutions;
sub_solutions.reserve(children().size());
// initialize vector of sub solutions
for (const auto &pair : all_solutions) {
// all children, except current solution's creator, start with zero index
indeces.push_back(pair.first != current.creator() ? 0 : pair.second.size()-1);
sub_solutions.push_back(pair.second[indeces.back()]);
}
while (true) {
merge(sub_solutions, state, spawner);
// compose next combination
size_t child = 0;
for (auto it = all_solutions.cbegin(), end = all_solutions.cend(); it != end; ++it, ++child) {
if (it->first == current.creator()) continue; // skip current solution's child
if (++indeces[child] >= it->second.size()) {
indeces[child] = 0; // start over with zero
sub_solutions[child] = it->second[indeces[child]];
continue; // and continue with next child
}
// otherwise, a new solution combination is available
sub_solutions[child] = it->second[indeces[child]];
break;
}
if (child == children().size()) // all combinations exhausted?
break;
}
}
void MergerPrivate::merge(const ChildSolutionList& sub_solutions, const moveit::core::RobotState& state, const Spawner& spawner)
{
// transform vector of SubTrajectories into vector of RobotTrajectories
std::vector<robot_trajectory::RobotTrajectoryConstPtr> sub_trajectories;
sub_trajectories.reserve(sub_solutions.size());
for (const auto& sub : sub_solutions) {
// TODO: directly skip failures in mergeAnyCombination() or even earlier
if (sub->isFailure())
return;
sub_trajectories.push_back(sub->trajectory());
}
moveit::core::JointModelGroup *jmg = jmg_merged_.get();
robot_trajectory::RobotTrajectoryPtr merged = task_constructor::merge(sub_trajectories, state, jmg);
if (jmg_merged_.get() != jmg)
jmg_merged_.reset(jmg);
if (!merged) return;
SubTrajectory t(merged);
// accumulate costs and markers
double costs = 0.0;
for (const auto& sub : sub_solutions) {
costs += sub->cost();
t.markers().insert(t.markers().end(), sub->markers().begin(), sub->markers().end());
}
t.setCost(costs);
spawner(std::move(t));
}
} } } }