Merge #156: Rework interface resolution

This commit is contained in:
Robert Haschke 2020-04-10 22:34:25 +02:00
commit 1f8dae86dd
14 changed files with 699 additions and 654 deletions

View File

@ -91,8 +91,6 @@ public:
PRIVATE_CLASS(SerialContainer)
SerialContainer(const std::string& name = "serial container");
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool canCompute() const override;
void compute() override;
@ -130,8 +128,6 @@ public:
PRIVATE_CLASS(ParallelContainerBase)
ParallelContainerBase(const std::string& name = "parallel container");
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
protected:
ParallelContainerBase(ParallelContainerBasePrivate* impl);

View File

@ -105,6 +105,10 @@ public:
void validateConnectivity() const override;
// Containers derive their required interface from their children
// UNKNOWN until resolveInterface was called
InterfaceFlags requiredInterface() const override { return required_interface_; }
// forward these methods to the public interface for containers
bool canCompute() const override;
void compute() override;
@ -115,21 +119,17 @@ public:
protected:
ContainerBasePrivate(ContainerBase* me, const std::string& name);
// Set child's push interfaces: allow pushing if child requires it or
// if the interface is unknown: in this case greedily assume a push interface.
// If, during pruneInterface() later, we notice that it's not needed, prune there.
inline void setChildsPushBackwardInterface(Stage& child) {
InterfaceFlags required = child.pimpl()->requiredInterface();
bool allowed = (required & WRITES_PREV_END) || (required == UNKNOWN);
child.pimpl()->setPrevEnds(allowed ? pending_backward_ : InterfacePtr());
// Set child's push interfaces: allow pushing if child requires it.
inline void setChildsPushBackwardInterface(StagePrivate* child) {
InterfaceFlags required = child->requiredInterface();
bool allowed = (required & WRITES_PREV_END);
child->setPrevEnds(allowed ? pending_backward_ : InterfacePtr());
}
inline void setChildsPushForwardInterface(Stage& child) {
InterfaceFlags required = child.pimpl()->requiredInterface();
bool allowed = (required & WRITES_NEXT_START) || (required == UNKNOWN);
child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
inline void setChildsPushForwardInterface(StagePrivate* child) {
InterfaceFlags required = child->requiredInterface();
bool allowed = (required & WRITES_NEXT_START);
child->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
}
// report error about mismatching interface (start or end as determined by mask)
void mismatchingInterface(InitStageException& errors, const StagePrivate& child, const InterfaceFlags mask) const;
/// copy external_state to a child's interface and remember the link in internal_to map
void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
@ -139,6 +139,9 @@ protected:
auto& internalToExternalMap() { return internal_to_external_; }
const auto& internalToExternalMap() const { return internal_to_external_; }
// set in resolveInterface()
InterfaceFlags required_interface_;
private:
container_type children_;
@ -166,28 +169,20 @@ class SerialContainerPrivate : public ContainerBasePrivate
public:
SerialContainerPrivate(SerialContainer* me, const std::string& name);
// containers derive their required interface from their children
InterfaceFlags requiredInterface() const override;
// called by parent asking for pruning of this' interface
void pruneInterface(InterfaceFlags accepted) override;
void resolveInterface(InterfaceFlags expected) override;
// validate connectivity of chain
void validateConnectivity() const override;
private:
// connect cur stage to its predecessor and successor
bool connect(container_type::const_iterator cur);
void reset();
// called by init() to prune interfaces for children in range [first, last)
void pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end);
// prune interface for children in range [first, last) to given direction
void pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end,
InterfaceFlags accepted);
// store first/last child's interface as required for this container
void storeRequiredInterface(container_type::const_iterator first, container_type::const_iterator end);
protected:
// connect two neighbors
void connect(StagePrivate& stage1, StagePrivate& stage2);
private:
InterfaceFlags required_interface_;
// 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;
};
PIMPL_FUNCTIONS(SerialContainer)
@ -219,14 +214,14 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate
public:
ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name);
// containers derive their required interface from their children
InterfaceFlags requiredInterface() const override;
// called by parent asking for pruning of this' interface
void pruneInterface(InterfaceFlags accepted) override;
void resolveInterface(InterfaceFlags expected) override;
void validateConnectivity() const override;
protected:
void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const;
private:
/// callback for new externally received states
void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated);
@ -256,7 +251,7 @@ public:
typedef std::function<void(SubTrajectory&&)> Spawner;
MergerPrivate(Merger* me, const std::string& name);
InterfaceFlags requiredInterface() const override;
void resolveInterface(InterfaceFlags expected) override;
void onNewPropagateSolution(const SolutionBase& s);
void onNewGeneratorSolution(const SolutionBase& s);

View File

@ -83,13 +83,30 @@ enum InterfaceFlag
PROPAGATE_BACKWARDS = READS_END | WRITES_PREV_END,
GENERATE = WRITES_PREV_END | WRITES_NEXT_START,
};
typedef Flags<InterfaceFlag> InterfaceFlags;
/** invert interface such that
* - new end can connect to old start
* - new start can connect to old end
*/
constexpr InterfaceFlags invert(InterfaceFlags f) {
InterfaceFlags inv;
if (f & READS_START)
inv |= WRITES_NEXT_START;
if (f & WRITES_PREV_END)
inv |= READS_END;
if (f & READS_END)
inv |= WRITES_PREV_END;
if (f & WRITES_NEXT_START)
inv |= READS_START;
return inv;
};
// some useful constants
constexpr InterfaceFlags UNKNOWN;
constexpr InterfaceFlags START_IF_MASK({ READS_START, WRITES_PREV_END });
constexpr InterfaceFlags END_IF_MASK({ READS_END, WRITES_NEXT_START });
constexpr InterfaceFlags PROPAGATE_BOTHWAYS({ PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS });
MOVEIT_CLASS_FORWARD(Interface)
MOVEIT_CLASS_FORWARD(Stage)
@ -253,12 +270,9 @@ public:
AUTO = 0x00, // auto-derive direction from context
FORWARD = 0x01, // propagate forward only
BACKWARD = 0x02, // propagate backward only
BOTHWAY = FORWARD | BACKWARD, // propagate both ways
};
void restrictDirection(Direction dir);
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
virtual void computeForward(const InterfaceState& from) = 0;
virtual void computeBackward(const InterfaceState& to) = 0;
@ -368,6 +382,8 @@ protected:
}
};
const char* flowSymbol(moveit::task_constructor::InterfaceFlags f);
/** Return (horizontal) flow symbol for start or end interface (specified by mask) */
template <unsigned int mask>
const char* flowSymbol(InterfaceFlags f);
}
}

View File

@ -71,15 +71,11 @@ public:
/// actually configured interface of this stage (only valid after init())
InterfaceFlags interfaceFlags() const;
/** Interface required by this stage
*
* If the interface is unknown (because it is auto-detected from context), return 0 */
/// Retrieve interface required by this stage, UNKNOWN if auto-detected from context
virtual InterfaceFlags requiredInterface() const = 0;
/** Prune interface to comply with the given propagation direction
*
* PropagatingEitherWay uses this in restrictDirection() */
virtual void pruneInterface(InterfaceFlags accepted) {}
/// Resolve interface/propagation direction to comply with the given external interface
virtual void resolveInterface(InterfaceFlags /* expected */) {}
/// validate connectivity of children (after init() was done)
virtual void validateConnectivity() const;
@ -163,6 +159,7 @@ protected:
std::string name_;
PropertyMap properties_;
// pull interfaces, created by the stage as required
InterfacePtr starts_;
InterfacePtr ends_;
@ -182,6 +179,8 @@ private:
ContainerBase* parent_; // owning parent
container_type::iterator it_; // iterator into parent's children_ list referring to this
// push interfaces, assigned by the parent container
// linking to previous/next sibling's pull interfaces
InterfaceWeakPtr prev_ends_; // interface to be used for sendBackward()
InterfaceWeakPtr next_starts_; // interface to be used for sendForward()
@ -208,19 +207,17 @@ class PropagatingEitherWayPrivate : public ComputeBasePrivate
friend class PropagatingEitherWay;
public:
PropagatingEitherWay::Direction required_interface_dirs_;
PropagatingEitherWay::Direction configured_dir_;
InterfaceFlags required_interface_;
inline PropagatingEitherWayPrivate(PropagatingEitherWay* me,
PropagatingEitherWay::Direction required_interface_dirs_,
inline PropagatingEitherWayPrivate(PropagatingEitherWay* me, PropagatingEitherWay::Direction configured_dir_,
const std::string& name);
InterfaceFlags requiredInterface() const override;
// initialize pull interfaces for given propagation directions
void initInterface(PropagatingEitherWay::Direction dir);
// prune interface to the given propagation direction
void pruneInterface(InterfaceFlags accepted) override;
// validate that we can propagate in one direction at least
void validateConnectivity() const override;
// resolve interface to the given propagation direction
void resolveInterface(InterfaceFlags expected) override;
bool canCompute() const override;
void compute() override;

View File

@ -53,10 +53,11 @@ using namespace std::placeholders;
namespace moveit {
namespace task_constructor {
ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name) : StagePrivate(me, name) {
pending_backward_.reset(new Interface);
pending_forward_.reset(new Interface);
}
ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name)
: StagePrivate(me, name)
, required_interface_(UNKNOWN)
, pending_backward_(new Interface)
, pending_forward_(new Interface) {}
ContainerBasePrivate::const_iterator ContainerBasePrivate::childByIndex(int index, bool for_insert) const {
if (!for_insert && index < 0)
@ -90,25 +91,9 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback& pr
}
void ContainerBasePrivate::validateConnectivity() const {
InitStageException errors;
// recursively validate all children and accumulate errors
for (const auto& child : children()) {
try {
child->pimpl()->validateConnectivity();
} catch (InitStageException& e) {
errors.append(e);
}
}
if (errors)
throw errors;
}
void ContainerBasePrivate::mismatchingInterface(InitStageException& errors, const StagePrivate& child,
const InterfaceFlags mask) const {
boost::format desc("%1% interface of '%2%' (%3%) doesn't match mine (%4%)");
errors.push_back(*me(), (desc % (mask == START_IF_MASK ? "start" : "end") % child.name() %
flowSymbol(child.interfaceFlags() & mask) % flowSymbol(interfaceFlags() & mask))
.str());
for (const auto& child : children())
child->pimpl()->validateConnectivity();
}
bool ContainerBasePrivate::canCompute() const {
@ -252,6 +237,11 @@ void ContainerBase::reset() {
// ... and state mapping
impl->internal_to_external_.clear();
// interfaces depend on children which might change
impl->required_interface_ = UNKNOWN;
impl->starts_.reset();
impl->ends_.reset();
Stage::reset();
}
@ -401,238 +391,103 @@ SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new
SerialContainerPrivate::SerialContainerPrivate(SerialContainer* me, const std::string& name)
: ContainerBasePrivate(me, name) {}
// a serial container's required interface is derived from the required input interfaces
// of the first and last children. After resolving, it is remembered in required_interface_.
InterfaceFlags SerialContainerPrivate::requiredInterface() const {
if ((required_interface_ & START_IF_MASK) && (required_interface_ & END_IF_MASK))
return required_interface_;
void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) {
InterfaceFlags flags1 = stage1.requiredInterface();
InterfaceFlags flags2 = stage2.requiredInterface();
if (children().empty())
return UNKNOWN;
return (children().front()->pimpl()->requiredInterface() & START_IF_MASK) |
(children().back()->pimpl()->requiredInterface() & END_IF_MASK);
}
// connect cur stage to its predecessor and successor by setting the push interface pointers
// return true if cur stage should be scheduled for a second sweep
bool SerialContainerPrivate::connect(container_type::const_iterator cur) {
StagePrivate* const cur_impl = **cur;
InterfaceFlags required = cur_impl->requiredInterface();
// get iterators to prev / next stage in sequence
auto prev = cur;
--prev;
auto next = cur;
++next;
// set push forward connection using next's starts
if ((required == UNKNOWN || required & WRITES_NEXT_START) &&
next != children().end()) // last child has not a next one
cur_impl->setNextStarts((*next)->pimpl()->starts());
// set push backward connection using prev's ends
if ((required == UNKNOWN || required & WRITES_PREV_END) &&
cur != children().begin()) // first child has not a previous one
cur_impl->setPrevEnds((*prev)->pimpl()->ends());
// schedule stage with unknown interface for 2nd sweep
return required == UNKNOWN || required == PROPAGATE_BOTHWAYS;
}
/* Establishing the interface connections, we face a chicken-egg-problem:
* To establish a connection, a predecessors/successors pull interface is
* assigned to the current's stage push interface.
* However, propagating stages (in auto-detection mode) can only create
* their pull interfaces if the corresponding, opposite-side push interface
* is present already (because that's the mechanism to determine the supported
* propagation directions).
*
* Hence, we need to resolve this by performing two sweeps:
* - initialization, assuming both propagation directions should be supported,
* thus generating both pull interfaces, i.e. providing the egg
* - stripping down the interfaces to the actual context
* This context is provided by two stages pushing from both ends
* into a (potentially long) sequence of propagating stages (tbd).
*/
void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model) {
// reset pull interfaces
auto impl = pimpl();
impl->starts_.reset();
impl->ends_.reset();
impl->required_interface_ = UNKNOWN;
// recursively init all children, throws if there are no children
ContainerBase::init(robot_model);
auto start = impl->children().begin();
auto last = --impl->children().end();
// connect first / last child's push interfaces to our pending_* buffers
// if they require pushing
impl->setChildsPushBackwardInterface(**start);
impl->setChildsPushForwardInterface(**last);
// initialize and connect remaining children in two sweeps
// to allow interface auto-detection for propagating stages
auto first_unknown = start; // pointer to first stage with unknown interface
for (auto cur = start, end = impl->children().end(); cur != end; ++cur) {
// 1st sweep: connect everything potentially possible,
// remembering start of unknown sub sequence
if (impl->connect(cur))
;
else { // reached a stage with known interface
// 2nd sweep: prune interfaces from [first_unknown, cur)
impl->pruneInterfaces(first_unknown, cur);
// restart with first_unknown = ++cur
first_unknown = cur;
++first_unknown;
}
if ((flags1 & WRITES_NEXT_START) && (flags2 & READS_START))
stage1.setNextStarts(stage2.starts());
else if ((flags1 & READS_END) && (flags2 & WRITES_PREV_END))
stage2.setPrevEnds(stage1.ends());
else {
boost::format desc("cannot connect end interface of '%1%' (%2%) to start interface of '%3%' (%4%)");
desc % stage1.name() % flowSymbol<END_IF_MASK>(flags1);
desc % stage2.name() % flowSymbol<START_IF_MASK>(flags2);
throw InitStageException(*me(), desc.str());
}
// prune stages [first_unknown, end())
impl->pruneInterfaces(first_unknown, impl->children().end());
// initialize this' pull interfaces if first/last child pulls
if (const InterfacePtr& target = (*start)->pimpl()->starts())
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())
impl->ends_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, std::placeholders::_1,
std::cref(target), std::placeholders::_2)));
}
// prune interface for children in range [first, last) to given direction
void SerialContainerPrivate::storeRequiredInterface(container_type::const_iterator first,
container_type::const_iterator end) {
if (first == children().begin())
required_interface_ |= children().front()->pimpl()->interfaceFlags() & START_IF_MASK;
if (end == children().end() && !children().empty())
required_interface_ |= children().back()->pimpl()->interfaceFlags() & END_IF_MASK;
template <unsigned int mask>
void SerialContainerPrivate::validateInterface(const StagePrivate& child, InterfaceFlags required) const {
required = required & mask;
if (required == UNKNOWN)
return; // cannot yet validate
InterfaceFlags child_interface = child.interfaceFlags() & mask;
if (required != child_interface) {
boost::format desc("%1% interface (%3%) of '%2%' does not match mine (%4%)");
desc % (mask == START_IF_MASK ? "start" : "end") % child.name();
desc % flowSymbol<mask>(child_interface) % flowSymbol<mask>(required);
throw InitStageException(*me_, desc.str());
}
}
// called by parent asking for pruning of this' interface
void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
// we need to have some children to do the actual work
if (children().empty())
return;
throw InitStageException(*me(), "no children");
// reading is always allowed if current interface flags do so
accepted |= (interfaceFlags() & InterfaceFlags({ READS_START, READS_END }));
if (!(expected & START_IF_MASK))
throw InitStageException(*me(), "unknown start interface");
if (accepted == PROPAGATE_BOTHWAYS)
return; // There is nothing to prune
Stage& first = *children().front();
Stage& last = *children().back();
// If whole chain is still undecided, prune all children
if (children().front()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS &&
children().back()->pimpl()->interfaceFlags() == PROPAGATE_BOTHWAYS) {
pruneInterfaces(children().begin(), children().end(), accepted);
} else { // otherwise only prune the first / last child's input / output interface
StagePrivate* child_impl;
child_impl = children().front()->pimpl();
child_impl->pruneInterface((accepted & START_IF_MASK) | (child_impl->interfaceFlags() & END_IF_MASK));
child_impl = children().back()->pimpl();
child_impl->pruneInterface((accepted & END_IF_MASK) | (child_impl->interfaceFlags() & START_IF_MASK));
InitStageException exceptions;
try { // FIRST child
first.pimpl()->resolveInterface(expected & START_IF_MASK);
// connect first child's (start) push interface
setChildsPushBackwardInterface(first.pimpl());
// validate that first child's and this container's start interfaces match
validateInterface<START_IF_MASK>(*first.pimpl(), expected);
// connect first child's (start) pull interface
if (const InterfacePtr& target = first.pimpl()->starts())
starts_.reset(new Interface(
[this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); }));
} catch (InitStageException& e) {
exceptions.append(e);
}
// reset my pull interfaces, if first/last child don't pull anymore
if (!children().front()->pimpl()->starts())
starts_.reset();
if (!children().back()->pimpl()->ends())
ends_.reset();
if (interfaceFlags() == UNKNOWN)
throw InitStageException(*me(), "failed to derive propagation direction");
}
// called by init() to prune interfaces for children in range [first, last)
// this function determines the feasible propagation directions
void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end) {
if (first == end) {
storeRequiredInterface(first, end);
return; // nothing to do in this case
// process all children and connect them
for (auto it = ++children().begin(), previous_it = children().begin(); it != children().end(); ++it, ++previous_it) {
try {
StagePrivate* child_impl = (**it).pimpl();
StagePrivate* previous_impl = (**previous_it).pimpl();
child_impl->resolveInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK);
connect(*previous_impl, *child_impl);
} catch (InitStageException& e) {
exceptions.append(e);
}
}
// determine accepted interface from available push interfaces
InterfaceFlags accepted;
// if first stage ...
if (first != children().begin()) {
auto prev = first;
--prev; // pointer to previous stage
// ... pushes forward, we accept forward propagation
if ((*prev)->pimpl()->requiredInterface() & WRITES_NEXT_START)
accepted |= PROPAGATE_FORWARDS;
// ... pulls backward, we accept backward propagation
if ((*prev)->pimpl()->requiredInterface() & READS_END)
accepted |= PROPAGATE_BACKWARDS;
} // else: for first child we cannot determine the interface yet
// if end stage ...
if (end != children().end()) {
// ... pushes backward, we accept backward propagation
if ((*end)->pimpl()->requiredInterface() & WRITES_PREV_END)
accepted |= PROPAGATE_BACKWARDS;
// ... pulls forward, we accept forward propagation
if ((*end)->pimpl()->requiredInterface() & READS_START)
accepted |= PROPAGATE_FORWARDS;
} // else: for last child we cannot determine the interface yet
// nothing to do if:
// - accepted == UNKNOWN: interface still unknown
// - accepted == PROPAGATE_BOTHWAYS: no change
if (accepted != UNKNOWN && accepted != PROPAGATE_BOTHWAYS)
pruneInterfaces(first, end, accepted);
}
// prune interface for children in range [first, last) to given direction
void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end,
InterfaceFlags accepted) {
// 1st sweep: remove push interfaces
for (auto it = first; it != end; ++it) {
StagePrivate* impl = (*it)->pimpl();
// the required interface should be a subset of the accepted one
if ((impl->requiredInterface() & accepted) != impl->requiredInterface())
throw InitStageException(*impl->me(), "Required interface not satisfied after pruning");
// remove push interfaces if not accepted
if (!(accepted & WRITES_PREV_END))
impl->setPrevEnds(InterfacePtr());
if (!(accepted & WRITES_NEXT_START))
impl->setNextStarts(InterfacePtr());
}
// 2nd sweep: recursively prune children
for (auto it = first; it != end; ++it) {
StagePrivate* impl = (*it)->pimpl();
impl->pruneInterface(accepted);
try { // connect last child's (end) push interface
setChildsPushForwardInterface(last.pimpl());
// validate that last child's and this container's end interfaces match
validateInterface<END_IF_MASK>(*last.pimpl(), expected);
// connect last child's (end) pull interface
if (const InterfacePtr& target = last.pimpl()->ends())
ends_.reset(new Interface(
[this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); }));
} catch (InitStageException& e) {
exceptions.append(e);
}
storeRequiredInterface(first, end);
required_interface_ = first.pimpl()->interfaceFlags() & START_IF_MASK | last.pimpl()->interfaceFlags() & END_IF_MASK;
if (exceptions)
throw exceptions;
}
void SerialContainerPrivate::validateConnectivity() const {
InitStageException errors;
// recursively validate children
try {
ContainerBasePrivate::validateConnectivity();
} catch (InitStageException& e) {
errors.append(e);
}
ContainerBasePrivate::validateConnectivity();
InterfaceFlags mine = interfaceFlags();
// check that input / output interface of first / last child matches this' resp. interface
if (!children().empty()) {
const StagePrivate* start = children().front()->pimpl();
const auto my_flags = this->interfaceFlags();
auto child_flags = start->interfaceFlags() & START_IF_MASK;
if (child_flags != (my_flags & START_IF_MASK))
mismatchingInterface(errors, *start, START_IF_MASK);
validateInterface<START_IF_MASK>(*children().front()->pimpl(), mine);
validateInterface<END_IF_MASK>(*children().back()->pimpl(), mine);
const StagePrivate* last = children().back()->pimpl();
child_flags = last->interfaceFlags() & END_IF_MASK;
if (child_flags != (my_flags & END_IF_MASK))
mismatchingInterface(errors, *last, END_IF_MASK);
}
// validate connectivity of children amongst each other
// validate connectivity of children between each other
// ContainerBasePrivate::validateConnectivity() ensures that required push interfaces are present,
// that is, neighbouring stages have a corresponding pull interface.
// Here, it remains to check that - if a child has a pull interface - it's indeed feeded.
@ -649,16 +504,13 @@ void SerialContainerPrivate::validateConnectivity() const {
// start pull interface fed?
if (cur != children().begin() && // first child has not a previous one
(required & READS_START) && !(*prev)->pimpl()->nextStarts())
errors.push_back(**cur, "start interface is not fed");
throw InitStageException(**cur, "start interface is not fed");
// end pull interface fed?
if (next != end && // last child has not a next one
(required & READS_END) && !(*next)->pimpl()->prevEnds())
errors.push_back(**cur, "end interface is not fed");
throw InitStageException(**cur, "end interface is not fed");
}
if (errors)
throw errors;
}
bool SerialContainer::canCompute() const {
@ -715,74 +567,74 @@ void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& soluti
ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name)
: ContainerBasePrivate(me, name) {}
// A parallel container's required interface is derived from the required interfaces of all of its children.
// They must not conflict to each other. Otherwise an InitStageException is thrown.
InterfaceFlags ParallelContainerBasePrivate::requiredInterface() const {
void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) {
// we need to have some children to do the actual work
if (children().empty())
return UNKNOWN;
/* The interfaces of all children need to be consistent with each other. Allowed combinations are:
* = (connecting stages)
* = (backward propagating)
* = (forward propagating)
* = = = (propagating in both directions)
* = (generating)
*/
throw InitStageException(*me(), "no children");
InterfaceFlags accumulated = children().front()->pimpl()->requiredInterface();
for (const Stage::pointer& stage : children()) {
InterfaceFlags current = stage->pimpl()->requiredInterface();
if (accumulated != PROPAGATE_BOTHWAYS &&
(accumulated & current) == current) // all flags of current are already available in accumulated
InitStageException exceptions;
bool first = true;
for (const Stage::pointer& child : children()) {
try {
auto child_impl = child->pimpl();
child_impl->resolveInterface(expected);
validateInterfaces(*child_impl, expected, first);
// initialize push connections of children according to their demands
setChildsPushForwardInterface(child_impl);
setChildsPushBackwardInterface(child_impl);
first = false;
} catch (InitStageException& e) {
exceptions.append(e);
continue;
bool current_is_propagating =
(current == PROPAGATE_BOTHWAYS || current == PROPAGATE_FORWARDS || current == PROPAGATE_BACKWARDS);
if (current_is_propagating && accumulated != CONNECT && accumulated != GENERATE)
accumulated |= current; // propagating is compatible to all except CONNECT and GENERATE
else
throw InitStageException(*me(),
"child '" + stage->name() + "' has conflicting interface to previous children");
}
}
return accumulated;
if (exceptions)
throw exceptions;
// States received by the container need to be copied to all children's pull interfaces.
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);
}));
required_interface_ = expected;
}
void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) {
// forward pruning to all children with UNKNOWN required interface
for (const Stage::pointer& stage : children()) {
if (stage->pimpl()->requiredInterface() == UNKNOWN)
stage->pimpl()->pruneInterface(accepted);
void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, InterfaceFlags& external,
bool first) const {
const InterfaceFlags child_interface = child.requiredInterface();
bool valid = true;
for (InterfaceFlags mask : { START_IF_MASK, END_IF_MASK }) {
if ((external & mask) == UNKNOWN)
external |= child_interface & mask;
valid = valid & ((external & mask) == (child_interface & mask));
}
if (!valid) {
boost::format desc("interface of '%1%' (%3% %4%) does not match %2% (%5% %6%).");
desc % child.name();
desc % (first ? "external one" : "other children's");
desc % flowSymbol<START_IF_MASK>(child_interface) % flowSymbol<END_IF_MASK>(child_interface);
desc % flowSymbol<START_IF_MASK>(external) % flowSymbol<END_IF_MASK>(external);
throw InitStageException(*me_, desc.str());
}
}
void ParallelContainerBasePrivate::validateConnectivity() const {
InitStageException errors;
InterfaceFlags my_interface = interfaceFlags();
InterfaceFlags children_interfaces;
// check that input / output interfaces of all children are handled by my interface
for (const auto& child : children()) {
InterfaceFlags current = child->pimpl()->interfaceFlags();
children_interfaces |= current; // compute union of all children interfaces
for (const auto& child : children())
validateInterfaces(*child->pimpl(), my_interface);
if ((current & my_interface & START_IF_MASK) != (current & START_IF_MASK))
mismatchingInterface(errors, *child->pimpl(), START_IF_MASK);
if ((current & my_interface & END_IF_MASK) != (current & END_IF_MASK))
mismatchingInterface(errors, *child->pimpl(), END_IF_MASK);
}
// check that there is a child matching the expected push interfaces
if ((my_interface & GENERATE) != (children_interfaces & GENERATE))
errors.push_back(*me(), "no child provides expected push interface");
// recursively validate children
try {
ContainerBasePrivate::validateConnectivity();
} catch (InitStageException& e) {
errors.append(e);
}
if (errors)
throw errors;
ContainerBasePrivate::validateConnectivity();
}
void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external,
@ -795,35 +647,6 @@ ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl)
ParallelContainerBase::ParallelContainerBase(const std::string& name)
: ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {}
/* States received by the container need to be copied to all children's pull interfaces.
* States generated by children can be directly forwarded into the container's push interfaces.
*/
void ParallelContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model) {
// recursively init children
ContainerBase::init(robot_model);
auto impl = pimpl();
// determine the union of interfaces required by children
// TODO: should we better use the least common interface?
InterfaceFlags required = impl->requiredInterface();
// initialize this' pull connections
impl->starts().reset(required & READS_START ?
new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState, impl,
Interface::FORWARD, std::placeholders::_1, std::placeholders::_2)) :
nullptr);
impl->ends().reset(required & READS_END ?
new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState, impl,
Interface::BACKWARD, std::placeholders::_1, std::placeholders::_2)) :
nullptr);
// initialize push connections of children according to their demands
for (const Stage::pointer& stage : impl->children()) {
impl->setChildsPushForwardInterface(*stage);
impl->setChildsPushBackwardInterface(*stage);
}
}
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) {
auto impl = pimpl();
impl->liftSolution(std::make_shared<WrappedSolution>(impl, &solution, cost, std::move(comment)), solution.start(),
@ -938,24 +761,14 @@ void Fallbacks::onNewSolution(const SolutionBase& s) {
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) {
void MergerPrivate::resolveInterface(InterfaceFlags expected) {
ContainerBasePrivate::resolveInterface(expected);
switch (interfaceFlags()) {
case PROPAGATE_FORWARDS:
case PROPAGATE_BACKWARDS:
case UNKNOWN:
break; // these are supported
case GENERATE:
throw InitStageException(*me_, "Generator stages not yet supported.");
case CONNECT:
@ -963,7 +776,6 @@ InterfaceFlags MergerPrivate::requiredInterface() const {
default:
throw InitStageException(*me_, "Children's interface not supported.");
}
return required;
}
Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) {}

View File

@ -39,15 +39,43 @@
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/introspection.h>
#include <moveit/planning_scene/planning_scene.h>
#include <ros/console.h>
#include <boost/format.hpp>
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <ros/console.h>
#include <utility>
namespace moveit {
namespace task_constructor {
template <>
const char* flowSymbol<START_IF_MASK>(InterfaceFlags f) {
f = f & START_IF_MASK;
if (f == READS_START)
return "";
if (f == WRITES_PREV_END)
return "";
if (f == UNKNOWN)
return "?";
return "";
}
template <>
const char* flowSymbol<END_IF_MASK>(InterfaceFlags f) {
f = f & END_IF_MASK;
if (f == READS_END)
return "";
if (f == WRITES_NEXT_START)
return "";
if (f == UNKNOWN)
return "?";
return "";
}
void InitStageException::push_back(const Stage& stage, const std::string& msg) {
errors_.emplace_back(std::make_pair(&stage, msg));
}
@ -57,7 +85,7 @@ void InitStageException::append(InitStageException& other) {
}
const char* InitStageException::what() const noexcept {
static const char* msg = "Error initializing stage(s)";
static const char* msg = "Error initializing stage(s). ROS_ERROR_STREAM(e) for details.";
return msg;
}
@ -88,8 +116,12 @@ void StagePrivate::validateConnectivity() const {
// check that the required interface is provided
InterfaceFlags required = requiredInterface();
InterfaceFlags actual = interfaceFlags();
if ((required & actual) != required)
throw InitStageException(*me(), "required interface is not satisfied");
if ((required & actual) != required) {
boost::format desc("actual interface %1% %2% does not match required interface %3% %4%");
desc % flowSymbol<START_IF_MASK>(actual) % flowSymbol<END_IF_MASK>(actual);
desc % flowSymbol<START_IF_MASK>(required) % flowSymbol<END_IF_MASK>(required);
throw InitStageException(*me(), desc.str());
}
}
bool StagePrivate::storeSolution(const SolutionBasePtr& solution) {
@ -332,43 +364,6 @@ void Stage::reportPropertyError(const Property::error& e) {
throw std::runtime_error(oss.str());
}
template <InterfaceFlag own, InterfaceFlag other>
const char* direction(const StagePrivate& stage) {
InterfaceFlags f = stage.interfaceFlags();
bool own_if = f & own;
bool other_if = f & other;
bool reverse = own & START_IF_MASK;
if (own_if && other_if)
return "<>";
if (!own_if && !other_if)
return "--";
if (other_if ^ reverse)
return "->";
return "<-";
}
const char* flowSymbol(InterfaceFlags f) {
if (f == UNKNOWN)
return "?"; // unknown interface
// f should have either INPUT or OUTPUT flags set (not both)
assert(static_cast<bool>(f & START_IF_MASK) ^ static_cast<bool>(f & END_IF_MASK));
if (f & START_IF_MASK) {
if (f == READS_START)
return "";
if (f == WRITES_PREV_END)
return "";
} else if (f & END_IF_MASK) {
if (f == READS_END)
return "";
if (f == WRITES_NEXT_START)
return "";
}
return "";
}
std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) {
// starts
for (const InterfaceConstPtr& i : { impl.prevEnds(), impl.starts() }) {
@ -379,8 +374,9 @@ std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) {
os << "-";
}
// trajectories
os << std::setw(5) << direction<READS_START, WRITES_PREV_END>(impl) << std::setw(3) << impl.solutions_.size()
<< std::setw(5) << direction<READS_END, WRITES_NEXT_START>(impl);
os << " " << flowSymbol<START_IF_MASK>(impl.interfaceFlags() | impl.requiredInterface()) << " ";
os << std::setw(3) << impl.solutions_.size();
os << " " << flowSymbol<END_IF_MASK>(impl.interfaceFlags() | impl.requiredInterface()) << " ";
// ends
for (const InterfaceConstPtr& i : { impl.ends(), impl.nextStarts() }) {
os << std::setw(3);
@ -398,67 +394,57 @@ ComputeBase::ComputeBase(ComputeBasePrivate* impl) : Stage(impl) {}
PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay* me, PropagatingEitherWay::Direction dir,
const std::string& name)
: ComputeBasePrivate(me, name), required_interface_dirs_(dir) {
initInterface(required_interface_dirs_);
: ComputeBasePrivate(me, name), configured_dir_(dir) {
initInterface(dir);
}
// initialize pull interfaces to match requested propagation directions
void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction dir) {
if (dir & PropagatingEitherWay::FORWARD) {
if (!starts_) // keep existing interface if possible
starts_.reset(
new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedStarts, this, std::placeholders::_1)));
} else {
starts_.reset();
}
if (dir & PropagatingEitherWay::BACKWARD) {
if (!ends_) // keep existing interface if possible
ends_.reset(
new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedEnds, this, std::placeholders::_1)));
} else {
ends_.reset();
switch (dir) {
case PropagatingEitherWay::FORWARD:
required_interface_ = PROPAGATE_FORWARDS;
if (!starts_) // keep existing interface if possible
starts_.reset(
new Interface([this](Interface::iterator it, bool /*updated*/) { this->dropFailedStarts(it); }));
ends_.reset();
return;
case PropagatingEitherWay::BACKWARD:
required_interface_ = PROPAGATE_BACKWARDS;
starts_.reset();
if (!ends_) // keep existing interface if possible
ends_.reset(new Interface([this](Interface::iterator it, bool /*updated*/) { this->dropFailedEnds(it); }));
return;
case PropagatingEitherWay::AUTO:
required_interface_ = UNKNOWN;
return;
}
}
void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) {
int dir = 0;
if ((accepted & PROPAGATE_FORWARDS) == PROPAGATE_FORWARDS)
dir |= PropagatingEitherWay::FORWARD;
if ((accepted & PROPAGATE_BACKWARDS) == PROPAGATE_BACKWARDS)
dir |= PropagatingEitherWay::BACKWARD;
initInterface(PropagatingEitherWay::Direction(dir));
}
void PropagatingEitherWayPrivate::resolveInterface(InterfaceFlags expected) {
if (expected == UNKNOWN)
throw InitStageException(*me(), "cannot initialize to unknown interface");
void PropagatingEitherWayPrivate::validateConnectivity() const {
InterfaceFlags required = requiredInterface();
InterfaceFlags actual = interfaceFlags();
if (actual == UNKNOWN)
throw InitStageException(*me(), "not connected in any direction");
InitStageException errors;
if ((actual & READS_START) && !(actual & WRITES_NEXT_START))
errors.push_back(*me(), "Cannot push forwards");
if ((actual & READS_END) && !(actual & WRITES_PREV_END))
errors.push_back(*me(), "Cannot push backwards");
if (required_interface_dirs_ == PropagatingEitherWay::BOTHWAY && (required & actual) != required)
ROS_WARN_STREAM_NAMED("PropagatingEitherWay", "Cannot propagate "
<< (actual & PROPAGATE_FORWARDS ? "backwards" : "forwards"));
if (errors)
throw errors;
auto dir = PropagatingEitherWay::AUTO;
if ((expected & START_IF_MASK) == READS_START || (expected & END_IF_MASK) == WRITES_NEXT_START)
dir = PropagatingEitherWay::FORWARD;
else if ((expected & START_IF_MASK) == WRITES_PREV_END || (expected & END_IF_MASK) == READS_END)
dir = PropagatingEitherWay::BACKWARD;
else {
boost::format desc("propagator cannot satisfy expected interface %1% %2%");
desc % flowSymbol<START_IF_MASK>(expected) % flowSymbol<END_IF_MASK>(expected);
throw InitStageException(*me(), desc.str());
}
if (configured_dir_ != PropagatingEitherWay::AUTO && dir != configured_dir_) {
boost::format desc("configured interface (%1% %2%) does not match expected one (%3% %4%)");
desc % flowSymbol<START_IF_MASK>(required_interface_) % flowSymbol<END_IF_MASK>(required_interface_);
desc % flowSymbol<START_IF_MASK>(expected) % flowSymbol<END_IF_MASK>(expected);
throw InitStageException(*me(), desc.str());
}
initInterface(dir);
}
InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const {
InterfaceFlags f;
if (required_interface_dirs_ & PropagatingEitherWay::FORWARD)
f |= PROPAGATE_FORWARDS;
if (required_interface_dirs_ & PropagatingEitherWay::BACKWARD)
f |= PROPAGATE_BACKWARDS;
// if required_interface_dirs_ == AUTO, we don't require an interface
// but the parent container auto-derives the propagation direction
return f;
return required_interface_;
}
void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) {
@ -516,26 +502,12 @@ PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate* impl) :
void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir) {
auto impl = pimpl();
if (impl->required_interface_dirs_ == dir)
if (impl->configured_dir_ == dir)
return;
if (impl->prevEnds() || impl->nextStarts())
if (impl->configured_dir_ != AUTO)
throw std::runtime_error("Cannot change direction after being connected");
impl->required_interface_dirs_ = dir;
impl->initInterface(impl->required_interface_dirs_);
}
void PropagatingEitherWay::init(const moveit::core::RobotModelConstPtr& robot_model) {
Stage::init(robot_model);
auto impl = pimpl();
// In AUTO-mode, i.e. when auto-detecting direction of propagation from context,
// pretend that we offer both interface directions during init().
// This is needed due to a chicken-egg-problem: interface auto-detection requires
// the context (external pushing interfaces prevEnds, nextStarts) to be set,
// while they are ony set if we detected the correct interface...
if (impl->required_interface_dirs_ == AUTO)
impl->initInterface(BOTHWAY);
// otherwise the interface is already fixed and well-defined
impl->configured_dir_ = dir;
impl->initInterface(dir);
}
void PropagatingEitherWay::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) {
@ -555,7 +527,7 @@ PropagatingForwardPrivate::PropagatingForwardPrivate(PropagatingForward* me, con
PropagatingForward::PropagatingForward(const std::string& name)
: PropagatingEitherWay(new PropagatingForwardPrivate(this, name)) {}
void PropagatingForward::computeBackward(const InterfaceState& to) {
void PropagatingForward::computeBackward(const InterfaceState& /* to */) {
assert(false); // This should never be called
}
@ -568,14 +540,14 @@ PropagatingBackwardPrivate::PropagatingBackwardPrivate(PropagatingBackward* me,
PropagatingBackward::PropagatingBackward(const std::string& name)
: PropagatingEitherWay(new PropagatingBackwardPrivate(this, name)) {}
void PropagatingBackward::computeForward(const InterfaceState& from) {
void PropagatingBackward::computeForward(const InterfaceState& /* from */) {
assert(false); // This should never be called
}
GeneratorPrivate::GeneratorPrivate(Generator* me, const std::string& name) : ComputeBasePrivate(me, name) {}
InterfaceFlags GeneratorPrivate::requiredInterface() const {
return InterfaceFlags({ WRITES_NEXT_START, WRITES_PREV_END });
return InterfaceFlags(GENERATE);
}
bool GeneratorPrivate::canCompute() const {

View File

@ -261,9 +261,7 @@ void Task::init() {
// and *afterwards* initialize all children recursively
stages()->init(impl->robot_model_);
// task expects its wrapped child to push to both ends, this triggers interface resolution
stages()->pimpl()->pruneInterface(InterfaceFlags({ GENERATE }));
// and *finally* validate connectivity
stages()->pimpl()->validateConnectivity();
stages()->pimpl()->resolveInterface(InterfaceFlags({ GENERATE }));
// provide introspection instance to all stages
impl->setIntrospection(impl->introspection_.get());

View File

@ -11,12 +11,14 @@
using namespace moveit::task_constructor;
static unsigned int mock_id = 0;
class GeneratorMockup : public Generator
{
int runs = 0;
public:
GeneratorMockup(int runs = 0) : Generator("generator"), runs(runs) {}
GeneratorMockup(int runs = 0) : Generator("generator " + std::to_string(++mock_id)), runs(runs) {}
bool canCompute() const override { return runs > 0; }
void compute() override {
if (runs > 0)
@ -27,10 +29,11 @@ public:
class MonitoringGeneratorMockup : public MonitoringGenerator
{
public:
MonitoringGeneratorMockup(Stage* monitored) : MonitoringGenerator("monitoring generator", monitored) {}
MonitoringGeneratorMockup(Stage* monitored)
: MonitoringGenerator("monitoring generator " + std::to_string(++mock_id), monitored) {}
bool canCompute() const override { return false; }
void compute() override {}
void onNewSolution(const SolutionBase& s) override {}
void onNewSolution(const SolutionBase&) override {}
};
class PropagatorMockup : public PropagatingEitherWay
@ -39,12 +42,13 @@ class PropagatorMockup : public PropagatingEitherWay
int bw_runs = 0;
public:
PropagatorMockup(int fw = 0, int bw = 0) : PropagatingEitherWay("either way"), fw_runs(fw), bw_runs(bw) {}
void computeForward(const InterfaceState& from) override {
PropagatorMockup(int fw = 0, int bw = 0)
: PropagatingEitherWay("propagate " + std::to_string(++mock_id)), fw_runs(fw), bw_runs(bw) {}
void computeForward(const InterfaceState& /* from */) override {
if (fw_runs > 0)
--fw_runs;
}
void computeBackward(const InterfaceState& from) override {
void computeBackward(const InterfaceState& /* from */) override {
if (bw_runs > 0)
--bw_runs;
}
@ -54,7 +58,7 @@ class ForwardMockup : public PropagatorMockup
public:
ForwardMockup(int runs = 0) : PropagatorMockup(runs, 0) {
restrictDirection(FORWARD);
setName("forward");
setName("forward " + std::to_string(mock_id));
}
};
class BackwardMockup : public PropagatorMockup
@ -62,7 +66,7 @@ class BackwardMockup : public PropagatorMockup
public:
BackwardMockup(int runs = 0) : PropagatorMockup(0, runs) {
restrictDirection(BACKWARD);
setName("backward");
setName("backward " + std::to_string(mock_id));
}
};
@ -71,8 +75,8 @@ class ConnectMockup : public Connecting
int runs = 0;
public:
ConnectMockup(int runs = 0) : Connecting("connect"), runs(runs) {}
void compute(const InterfaceState& from, const InterfaceState& to) override {
ConnectMockup(int runs = 0) : Connecting("connect " + std::to_string(++mock_id)), runs(runs) {}
void compute(const InterfaceState& /* from */, const InterfaceState& /* to */) override {
if (runs > 0)
--runs;
}
@ -84,32 +88,25 @@ enum StageType
FW,
BW,
ANY,
BOTH,
CONN
};
void append(ContainerBase& c, const std::initializer_list<StageType>& types, int runs = 0) {
for (StageType t : types) {
switch (t) {
case GEN:
c.insert(std::make_unique<GeneratorMockup>(runs));
c.add(std::make_unique<GeneratorMockup>(runs));
break;
case FW:
c.insert(std::make_unique<ForwardMockup>(runs));
c.add(std::make_unique<ForwardMockup>(runs));
break;
case BW:
c.insert(std::make_unique<BackwardMockup>(runs));
c.add(std::make_unique<BackwardMockup>(runs));
break;
case ANY:
c.insert(std::make_unique<PropagatorMockup>(runs, runs));
c.add(std::make_unique<PropagatorMockup>(runs, runs));
break;
case BOTH: {
auto s = std::make_unique<PropagatorMockup>(runs, runs);
s->restrictDirection(PropagatingEitherWay::BOTHWAY);
c.insert(std::move(s));
break;
}
case CONN:
c.insert(std::make_unique<ConnectMockup>(runs));
c.add(std::make_unique<ConnectMockup>(runs));
break;
}
}
@ -130,13 +127,13 @@ TEST(ContainerBase, positionForInsert) {
EXPECT_EQ(impl->childByIndex(-1, true), impl->children().end());
EXPECT_EQ(impl->childByIndex(-2, true), impl->children().end());
s.insert(std::make_unique<NamedStage>("0"));
s.add(std::make_unique<NamedStage>("0"));
EXPECT_STREQ((*impl->childByIndex(0, true))->name().c_str(), "0");
EXPECT_EQ(impl->childByIndex(-1, true), impl->children().end());
EXPECT_STREQ((*impl->childByIndex(-2, true))->name().c_str(), "0");
EXPECT_EQ(impl->childByIndex(-3, true), impl->children().end());
s.insert(std::make_unique<NamedStage>("1"));
s.add(std::make_unique<NamedStage>("1"));
EXPECT_STREQ((*impl->childByIndex(0, true))->name().c_str(), "0");
EXPECT_STREQ((*impl->childByIndex(1, true))->name().c_str(), "1");
EXPECT_EQ(impl->childByIndex(2, true), impl->children().end());
@ -147,15 +144,16 @@ TEST(ContainerBase, positionForInsert) {
EXPECT_EQ(impl->childByIndex(-4, true), impl->children().end());
}
/* TODO: remove interface as it returns raw pointers */
TEST(ContainerBase, findChild) {
SerialContainer s, *c2;
Stage *a, *b, *c1, *d;
s.insert(Stage::pointer(a = new NamedStage("a")));
s.insert(Stage::pointer(b = new NamedStage("b")));
s.insert(Stage::pointer(c1 = new NamedStage("c")));
auto sub = Stage::pointer(c2 = new SerialContainer("c"));
c2->insert(Stage::pointer(d = new NamedStage("d")));
s.insert(std::move(sub));
s.add(Stage::pointer(a = new NamedStage("a")));
s.add(Stage::pointer(b = new NamedStage("b")));
s.add(Stage::pointer(c1 = new NamedStage("c")));
auto sub = ContainerBase::pointer(c2 = new SerialContainer("c"));
sub->add(Stage::pointer(d = new NamedStage("d")));
s.add(std::move(sub));
EXPECT_EQ(s.findChild("a"), a);
EXPECT_EQ(s.findChild("b"), b);
@ -189,27 +187,42 @@ protected:
impl->setPrevEnds(InterfacePtr());
pushInterface(start, end);
}
void initContainer(const std::initializer_list<StageType>& types = {}) {
container.clear();
mock_id = 0;
append(container, types);
}
void validateInit(bool start, bool end, bool expect_failure) { validateInit(start, end, {}, expect_failure); }
void validateInit(bool start, bool end, const std::initializer_list<StageType>& types, bool expect_failure) {
reset(start, end);
append(container, types);
try {
container.init(robot_model);
// prune pull interfaces based on provided external interface (start, end)
InterfaceFlags accepted;
// resolve interfaces based on provided external interface (start, end)
InterfaceFlags expected;
if (start)
accepted |= WRITES_PREV_END;
expected |= WRITES_PREV_END;
else
expected |= READS_START;
if (end)
accepted |= WRITES_NEXT_START;
container.pimpl()->pruneInterface(accepted);
expected |= WRITES_NEXT_START;
else
expected |= READS_END;
container.pimpl()->resolveInterface(expected);
container.pimpl()->validateConnectivity();
if (!expect_failure)
return; // as expected
ADD_FAILURE() << "init() didn't recognize a failure condition as expected\n" << container;
} catch (const InitStageException& e) {
if (expect_failure)
if (expect_failure) {
std::cout << "Expected " << e << "\n" << container;
return; // as expected
ADD_FAILURE() << "unexpected init failure: \n" << e << "\n" << container;
}
ADD_FAILURE() << "Unexpected " << e << "\n" << container;
} catch (const std::exception& e) {
ADD_FAILURE() << "unexpected exception thrown:\n" << e.what();
} catch (...) {
@ -287,14 +300,14 @@ TEST_F(SerialTest, insertion_order) {
#define EXPECT_INIT_FAILURE(start, end, ...) \
{ \
SCOPED_TRACE("validateInit({" #__VA_ARGS__ "})"); \
container.clear(); \
initContainer(); \
validateInit(start, end, { __VA_ARGS__ }, true); \
}
#define EXPECT_INIT_SUCCESS(start, end, ...) \
{ \
SCOPED_TRACE("validateInit({" #__VA_ARGS__ "})"); \
container.clear(); \
initContainer(); \
validateInit(start, end, { __VA_ARGS__ }, false); \
}
@ -310,8 +323,10 @@ TEST_F(SerialTest, init_connecting) {
EXPECT_INIT_SUCCESS(false, false, CONN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
// external interface does not match CONNECT
EXPECT_INIT_FAILURE(false, true, CONN);
EXPECT_INIT_FAILURE(true, false, CONN);
EXPECT_INIT_FAILURE(true, true, CONN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags({ CONNECT, GENERATE }));
EXPECT_INIT_FAILURE(false, false, CONN, CONN); // two connecting stages cannot be connected
}
@ -320,11 +335,38 @@ TEST_F(SerialTest, init_generator) {
EXPECT_INIT_SUCCESS(true, true, GEN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
EXPECT_INIT_FAILURE(false, false, GEN); // generator wants to push, but container cannot propagate pushes
// external interface does not match GENERATE
EXPECT_INIT_FAILURE(false, false, GEN);
EXPECT_INIT_FAILURE(false, true, GEN);
EXPECT_INIT_FAILURE(true, false, GEN);
EXPECT_INIT_FAILURE(true, true, GEN, GEN); // two generator stages cannot be connected
}
TEST_F(SerialTest, keep_configured_propagator_interface) {
// configuring an ANY propagator should work
EXPECT_INIT_SUCCESS(false, true, ANY);
EXPECT_EQ(container.pimpl()->children().front()->pimpl()->requiredInterface(), InterfaceFlags(PROPAGATE_FORWARDS));
// multiple times
validateInit(false, true, false);
EXPECT_EQ(container.pimpl()->children().front()->pimpl()->requiredInterface(), InterfaceFlags(PROPAGATE_FORWARDS));
// and with another interface direction as well
validateInit(true, false, false);
EXPECT_EQ(container.pimpl()->children().front()->pimpl()->requiredInterface(), InterfaceFlags(PROPAGATE_BACKWARDS));
// configuring a FORWARD propagator should work in forward direction
EXPECT_INIT_SUCCESS(false, true, FW);
// but fail when initialized in backward direction
validateInit(true, false, true);
EXPECT_EQ(container.pimpl()->children().front()->pimpl()->requiredInterface(), InterfaceFlags(PROPAGATE_FORWARDS));
// same with BACKWARD propagator
EXPECT_INIT_SUCCESS(true, false, BW);
validateInit(false, true, true);
EXPECT_EQ(container.pimpl()->children().front()->pimpl()->requiredInterface(), InterfaceFlags(PROPAGATE_BACKWARDS));
}
TEST_F(SerialTest, init_forward) {
// container interface doesn't match children
EXPECT_INIT_FAILURE(false, false, FW);
@ -359,12 +401,6 @@ TEST_F(SerialTest, init_forward) {
EXPECT_INIT_SUCCESS(true, false, BW, ANY);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
// BOTH requires propagation in both directions, while FW/BW can only match one dir
EXPECT_INIT_FAILURE(true, true, BOTH, FW);
EXPECT_INIT_FAILURE(true, true, FW, BOTH);
EXPECT_INIT_FAILURE(true, true, BOTH, BW);
EXPECT_INIT_FAILURE(true, true, BW, BOTH);
}
TEST_F(SerialTest, init_backward) {
@ -385,6 +421,18 @@ TEST_F(SerialTest, init_backward) {
}
TEST_F(SerialTest, interface_detection) {
// ANY resolves in either direction
EXPECT_INIT_SUCCESS(false, true, ANY);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_INIT_SUCCESS(true, false, ANY);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_INIT_SUCCESS(true, true, GEN, ANY);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
EXPECT_INIT_SUCCESS(true, true, ANY, GEN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
// derive propagation direction from inner generator
EXPECT_INIT_SUCCESS(true, true, ANY, GEN, ANY); // <- <-> ->
auto it = container.pimpl()->children().begin();
@ -393,12 +441,6 @@ TEST_F(SerialTest, interface_detection) {
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
EXPECT_INIT_SUCCESS(true, true, GEN, ANY);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
EXPECT_INIT_SUCCESS(true, true, ANY, GEN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
// derive propagation direction from inner connector
EXPECT_INIT_SUCCESS(false, false, ANY, CONN, ANY); // -> -- <-
it = container.pimpl()->children().begin();
@ -414,9 +456,9 @@ TEST_F(SerialTest, interface_detection) {
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
// derive propagation direction from outer interface
EXPECT_INIT_SUCCESS(false, true, ANY); // should be pruned to FW
EXPECT_INIT_SUCCESS(false, true, ANY); // should be resolved to FW
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_INIT_SUCCESS(true, false, ANY); // should be pruned to BW
EXPECT_INIT_SUCCESS(true, false, ANY); // should be resolved to BW
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_INIT_SUCCESS(false, true, ANY, ANY); // -> ->
@ -431,109 +473,74 @@ TEST_F(SerialTest, interface_detection) {
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_INIT_SUCCESS(true, true, ANY, ANY); // <> <>
it = container.pimpl()->children().begin();
EXPECT_EQ((*it)->pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_INIT_FAILURE(true, true, ANY, ANY); // no generator
EXPECT_INIT_FAILURE(false, false, ANY, ANY); // -- --
it = container.pimpl()->children().begin();
EXPECT_EQ((*it)->pimpl()->interfaceFlags(), UNKNOWN);
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), UNKNOWN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), UNKNOWN);
EXPECT_INIT_FAILURE(false, false, ANY, ANY); // no connect
}
template <typename T>
Stage::pointer make(const std::string& name, const std::initializer_list<StageType>& children, int runs = 0) {
auto container = new T(name);
append(*container, children);
return Stage::pointer(container);
}
TEST_F(SerialTest, nested_interface_detection) {
SerialContainer* inner;
// direction imposed from outer generator
container.clear();
inner = new SerialContainer("inner serial");
append(*inner, { ANY, ANY });
append(container, { GEN, ANY, ANY });
container.insert(Stage::pointer(inner), -2);
initContainer({ GEN, ANY });
container.add(make<SerialContainer>("inner serial", { ANY, ANY }));
append(container, { ANY });
{
SCOPED_TRACE("GEN - ANY - inner - ANY");
validateInit(true, true, {}, false);
SCOPED_TRACE("GEN - ANY - Serial( ANY - ANY ) - ANY");
validateInit(true, true, false);
}
// direction imposed from inner generator
container.clear();
inner = new SerialContainer("inner serial");
append(*inner, { ANY, GEN, ANY });
append(container, { ANY, ANY });
container.insert(Stage::pointer(inner), -2);
initContainer({ ANY });
container.add(make<SerialContainer>("inner serial", { ANY, GEN, ANY }));
append(container, { ANY });
{
SCOPED_TRACE("ANY - inner - ANY");
validateInit(true, true, {}, false);
SCOPED_TRACE("ANY - Serial( ANY - GEN - ANY ) - ANY");
validateInit(true, true, false);
}
container.clear();
inner = new SerialContainer("inner serial");
append(*inner, { GEN, ANY });
container.insert(Stage::pointer(inner));
initContainer();
container.add(make<SerialContainer>("inner serial", { GEN, ANY }));
append(container, { ANY });
{
SCOPED_TRACE("inner - ANY");
validateInit(true, true, { ANY }, false);
SCOPED_TRACE("Serial( GEN - ANY ) - ANY");
validateInit(true, true, false);
}
// outer and inner generators conflict with each other
container.clear();
inner = new SerialContainer("inner serial");
append(*inner, { ANY, GEN, ANY });
append(container, { GEN, ANY, ANY });
container.insert(Stage::pointer(inner), -2);
initContainer({ GEN, ANY });
container.add(make<SerialContainer>("inner serial", { ANY, GEN, ANY }));
append(container, { ANY });
{
SCOPED_TRACE("GEN - ANY - inner - ANY");
validateInit(true, true, {}, true);
SCOPED_TRACE("GEN - ANY - Serial( ANY - GEN - ANY ) - ANY");
validateInit(true, true, true); // expected to fail
}
}
TEST_F(SerialTest, nested_parallel) {
initContainer({ GEN });
container.add(make<Alternatives>("inner alternatives", { ANY, ANY }));
{
SCOPED_TRACE("GEN - Alternatives( ANY - ANY )");
validateInit(true, true, false);
}
initContainer({ GEN });
container.add(make<Fallbacks>("inner alternatives", { ANY, FW }));
{
SCOPED_TRACE("GEN - Fallback( ANY - FW )");
validateInit(true, true, false);
}
}
class ParallelTest : public InitTest<Alternatives>
{};
TEST_F(ParallelTest, init_propagating) {
EXPECT_INIT_SUCCESS(true, true, BW, FW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_INIT_SUCCESS(true, true, BOTH, BOTH);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_INIT_SUCCESS(true, true, BW, BOTH);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_INIT_SUCCESS(true, true, FW, BOTH);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_INIT_SUCCESS(true, true, FW, BOTH, FW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_INIT_SUCCESS(true, true, FW, BW, FW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_INIT_SUCCESS(true, true, BW, FW, BW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_INIT_SUCCESS(false, true, FW, FW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_FORWARDS);
EXPECT_INIT_SUCCESS(true, false, BW, BW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BACKWARDS);
// external interface doesn't match derived
EXPECT_INIT_FAILURE(false, false, BOTH);
EXPECT_INIT_FAILURE(false, true, BOTH);
EXPECT_INIT_FAILURE(true, false, BOTH);
EXPECT_INIT_FAILURE(false, false, BW, BW);
EXPECT_INIT_FAILURE(false, true, BW, BW);
EXPECT_INIT_FAILURE(true, true, BW, BW);
EXPECT_INIT_FAILURE(false, false, FW, FW);
EXPECT_INIT_FAILURE(true, false, FW, FW);
EXPECT_INIT_FAILURE(true, true, FW, FW);
}
TEST_F(ParallelTest, init_matching) {
EXPECT_INIT_SUCCESS(true, true, GEN, GEN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), GENERATE);
@ -562,6 +569,11 @@ TEST_F(ParallelTest, init_mismatching) {
EXPECT_INIT_FAILURE(false, true, FW, CONN);
EXPECT_INIT_FAILURE(true, true, FW, CONN);
EXPECT_INIT_FAILURE(false, false, ANY, CONN);
EXPECT_INIT_FAILURE(true, false, ANY, CONN);
EXPECT_INIT_FAILURE(false, true, ANY, CONN);
EXPECT_INIT_FAILURE(true, true, ANY, CONN);
EXPECT_INIT_FAILURE(false, false, BW, GEN);
EXPECT_INIT_FAILURE(true, false, BW, GEN);
EXPECT_INIT_FAILURE(false, true, BW, GEN);
@ -572,20 +584,62 @@ TEST_F(ParallelTest, init_mismatching) {
EXPECT_INIT_FAILURE(false, true, FW, GEN);
EXPECT_INIT_FAILURE(true, true, FW, GEN);
EXPECT_INIT_FAILURE(false, false, ANY, GEN);
EXPECT_INIT_FAILURE(true, false, ANY, GEN);
EXPECT_INIT_FAILURE(false, true, ANY, GEN);
EXPECT_INIT_FAILURE(true, true, ANY, GEN);
EXPECT_INIT_FAILURE(false, false, CONN, GEN);
EXPECT_INIT_FAILURE(true, false, CONN, GEN);
EXPECT_INIT_FAILURE(false, true, CONN, GEN);
EXPECT_INIT_FAILURE(true, true, CONN, GEN);
EXPECT_INIT_FAILURE(false, false, BOTH, CONN);
EXPECT_INIT_FAILURE(true, false, BOTH, CONN);
EXPECT_INIT_FAILURE(false, true, BOTH, CONN);
EXPECT_INIT_FAILURE(true, true, BOTH, CONN);
EXPECT_INIT_FAILURE(false, false, BW, FW);
EXPECT_INIT_FAILURE(true, false, BW, FW);
EXPECT_INIT_FAILURE(false, true, BW, FW);
EXPECT_INIT_FAILURE(true, true, BW, FW);
}
EXPECT_INIT_FAILURE(false, false, BOTH, GEN);
EXPECT_INIT_FAILURE(true, false, BOTH, GEN);
EXPECT_INIT_FAILURE(false, true, BOTH, GEN);
EXPECT_INIT_FAILURE(true, true, BOTH, GEN);
TEST_F(ParallelTest, init_propagating) {
EXPECT_INIT_SUCCESS(false, true, FW, FW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_FORWARDS);
EXPECT_INIT_FAILURE(false, false, FW, FW);
EXPECT_INIT_FAILURE(true, false, FW, FW);
EXPECT_INIT_FAILURE(true, true, FW, FW);
EXPECT_INIT_SUCCESS(true, false, BW, BW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BACKWARDS);
EXPECT_INIT_FAILURE(false, false, BW, BW);
EXPECT_INIT_FAILURE(false, true, BW, BW);
EXPECT_INIT_FAILURE(true, true, BW, BW);
}
TEST_F(ParallelTest, init_any) {
EXPECT_INIT_FAILURE(true, true, ANY, ANY); // no generator
EXPECT_INIT_FAILURE(false, false, ANY, ANY); // no connector
// infer container (and children) direction from outside
EXPECT_INIT_SUCCESS(false, true, ANY, ANY);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
for (const auto& child : container.pimpl()->children())
EXPECT_EQ(child->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)) << child->name();
EXPECT_INIT_SUCCESS(true, false, ANY, ANY);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
for (const auto& child : container.pimpl()->children())
EXPECT_EQ(child->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)) << child->name();
EXPECT_INIT_SUCCESS(true, false, BW, ANY); // infer ANY as BACKWARDS
for (const auto& child : container.pimpl()->children())
EXPECT_EQ(child->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)) << child->name();
EXPECT_INIT_SUCCESS(false, true, ANY, FW); // infer ANY as FORWARDS
for (const auto& child : container.pimpl()->children())
EXPECT_EQ(child->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)) << child->name();
EXPECT_INIT_SUCCESS(false, true, FW, ANY, FW); // infer ANY as FORWARDS
for (const auto& child : container.pimpl()->children())
EXPECT_EQ(child->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)) << child->name();
}
TEST(Task, move) {

46
demo/scripts/grasp.py Normal file
View File

@ -0,0 +1,46 @@
#! /usr/bin/env python
# -*- coding: utf-8 -*-
from std_msgs.msg import Header
from geometry_msgs.msg import TwistStamped, Twist, Vector3Stamped, Vector3, PoseStamped, Pose, Point, Quaternion
from moveit.task_constructor import core, stages
import moveit_commander
import rospy
import numpy
from moveit.python_tools import roscpp_init
roscpp_init("mtc_tutorial")
rospy.init_node('mtc_tutorial_py', anonymous=False)
def create_object():
scene = moveit_commander.PlanningSceneInterface(synchronous=True)
scene.remove_world_object();
pose = PoseStamped()
pose.header.frame_id = "world"
pose.pose.position.x = 0.5
pose.pose.position.y = numpy.random.uniform(-0.1, 0.1)
pose.pose.position.z = 0.16
pose.pose.orientation.w = 1.0
scene.add_box('object', pose, size=(0.05, 0.05, 0.2))
group = "panda_arm"
eef_frame = "panda_link8"
# Cartesian interpolation planner
cartesian = core.CartesianPath()
task = core.Task()
# start from current robot state
task.add(stages.CurrentState("current state"))
move = stages.MoveTo("to object", cartesian)
move.group = group
header = Header(frame_id = "object")
move.setGoal(PoseStamped(header=header, pose=Pose(position=Point(0,0,0.18),orientation=Quaternion(0.92388, -0.38268, 0, 0))))
task.add(move)
create_object()
if task.plan():
task.publish(task.solutions[0])
rospy.spin()

65
demo/scripts/pick.py Normal file
View File

@ -0,0 +1,65 @@
#! /usr/bin/env python
# -*- coding: utf-8 -*-
from std_msgs.msg import Header
from geometry_msgs.msg import TwistStamped, Twist, Vector3Stamped, Vector3, PoseStamped, Pose, Point, Quaternion
from moveit.task_constructor import core, stages
import moveit_commander
import rospy
import numpy
from moveit.python_tools import roscpp_init
roscpp_init("mtc_tutorial")
rospy.init_node('mtc_tutorial_py', anonymous=False)
group = 'panda_arm'
eef = 'panda_hand'
eef_frame = "panda_link8"
sampling_planner = core.JointInterpolationPlanner()
cartesian_planner = core.CartesianPath()
task = core.Task()
task.properties.update({'group': group, 'eef': eef, 'hand': eef, 'hand_grasping_frame': eef, 'ik_frame': eef_frame})
currstate = stages.CurrentState('current state')
#task.add(currstate) # Adding it to the task results in error for argument types in setMonitoredStage in GenerateGraspPose
open_hand = stages.MoveTo("open hand", sampling_planner)
open_hand.group = eef
open_hand.setGoal('open')
task.add(open_hand)
connect = stages.Connect('move to pick', [(group, sampling_planner)])
connect.timeout = 5
connect.properties.configureInitFrom(core.PARENT)
task.add(connect)
grasp = core.SerialContainer('pick object')
task.properties.exposeTo(grasp.properties, ['eef', 'hand', 'group', 'ik_frame'])
grasp.properties.configureInitFrom(core.PARENT, ['eef', 'hand', 'group', 'ik_frame'])
approach_object = stages.MoveRelative("approach_object", cartesian_planner)
approach_object.properties.update({'marker_ns': 'approach_object', 'link': eef_frame})
approach_object.properties.configureInitFrom(core.PARENT, ['group'])
approach_object.min_distance = 0.01
approach_object.max_distance = 0.1
print(approach_object.properties.__getitem__('group')) # Why is this None? how to get properties from within SerialContainer?
approach_object.setDirection(TwistStamped(header=Header(frame_id = eef_frame), twist=Twist(linear=Vector3(0,0,0.1))))
grasp.insert(approach_object)
generatepose = stages.GenerateGraspPose('generate grasp pose')
generatepose.properties.configureInitFrom(core.PARENT)
generatepose.properties.update({'marker_ns': 'grasp_pose'})
generatepose.pregrasp = 'open'
generatepose.object = 'base'
generatepose.angle_delta = numpy.pi/12
generatepose.setMonitoredStage(currstate)
# To be continued
task.add(grasp)
if task.plan():
task.publish(task.solutions[0])
rospy.spin()

53
notes.org Normal file
View File

@ -0,0 +1,53 @@
* scheduling
There are 2 levels of scheduling:
- choosing which state or state pair should be scheduled for execution
- choosing which stage should be scheduled for execution
Main goal is to find small-cost solutions fast.
To this end, we need to find a solution at all, i.e. connect start to end.
Second, we prefer small-cost solutions.
First sort by length of trajectory, second by sum of trajectory costs.
** approach
- scheduling priority for stage (default / assigned by user)
- fallback: sort stages by priority
- evaluate success rate
- expected costs of solutions
- expected computing time
- Container::schedule() -> return ordered list of stages to execute
* containers / interfaces
Basic stages have unique solutions connecting start-end.
Containers allow for several solutions connecting start-end:
- serial: several pathes might exist
- parallel: inherent
* notation
push vs pull connections
* TODO
** modify-ps
- std::function API
- replace templates with sfniae
** eef collision
- requires modification in MoveIt
- disable warnings vs. only consider relevant links (separate function?)
** MoveTo / MoveRelative
- interface: pass goal constraints
- fill goal constraints during setup?
requires transforms to be specified relative to current robot pose
** properties
- fix plan_pick_pa10: forward grasp property
- global type registry for serialization/deserialization functions
- use bits as enums to allow for configureInitFrom(PARENT | INTERFACE)
** incremental GraspPoseGenerator
in pending_ list, store the current angular value
** TaskModels
- move scene_, display_context_ to BaseTaskModel
- use registered property creators for RemoteTaskModel too
- deserialize property
- call creator function

28
pick+place.org Normal file
View File

@ -0,0 +1,28 @@
* capability params
- overall planning timeout
- max number of solutions
** Pick
- possible_grasps -> fixed list
- instantiate GraspProvider+Task on each call (eventually cache)
- object
- key-value parameter map (passed on to Action Server)
* GraspProvider params
- eef (hand_group, arm_group, parent link)
- hand_group: default from arm_group
- arm_group: default from hand_group
- grasp frame (PoseStamped rel. zu Link fest zu Tip-Link im Arm)
- key-value parameter map (passed on to Action Server)
- grasp config
- action server name (with default)
* PlaceProvider params
- PlaceLocation[]
- surface_name: sample PlaceLocations, disable collision with table during IK
* GraspMsg
- grasp frame?
- no-collision links (during closing)
- no-collision

16
test.py Normal file
View File

@ -0,0 +1,16 @@
import moveit.task_constructor as mtc
o=mtc.MyObject(42)
a=o
#print o
#print a
mtc.access(o)
mtc.access(o)
mtc.consume(o)
mtc.access(o)
mtc.access(o)
#print o
#print a
del o
del a
print "done"

View File

@ -124,7 +124,6 @@ QVariant BaseTaskModel::flowIcon(moveit::task_constructor::InterfaceFlags f) {
static const QIcon CONNECT_ICON = icons::CONNECT.icon();
static const QIcon FORWARD_ICON = icons::FORWARD.icon();
static const QIcon BACKWARD_ICON = icons::BACKWARD.icon();
static const QIcon BOTHWAY_ICON = icons::BOTHWAY.icon();
static const QIcon GENERATE_ICON = icons::GENERATE.icon();
if (f == InterfaceFlags(CONNECT))
@ -133,8 +132,6 @@ QVariant BaseTaskModel::flowIcon(moveit::task_constructor::InterfaceFlags f) {
return FORWARD_ICON;
if (f == InterfaceFlags(PROPAGATE_BACKWARDS))
return BACKWARD_ICON;
if (f == PROPAGATE_BOTHWAYS)
return BOTHWAY_ICON;
if (f == InterfaceFlags(GENERATE))
return GENERATE_ICON;