diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 435fbb67..238c9aaa 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -339,6 +339,9 @@ protected: class ConnectingPrivate; class Connecting : public ComputeBase { +protected: + virtual bool compatible(const InterfaceState& from_state, const InterfaceState& to_state) const; + public: PRIVATE_CLASS(Connecting) Connecting(const std::string& name); diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 90320699..9ef2d6ae 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -235,6 +235,10 @@ class ConnectingPrivate : public ComputeBasePrivate { return x.first->priority() + x.second->priority() < y.first->priority() + y.second->priority(); } }; + + template + inline StatePair make_pair(Interface::const_iterator first, Interface::const_iterator second); + public: inline ConnectingPrivate(Connecting *me, const std::string &name); @@ -247,8 +251,8 @@ public: private: // get informed when new start or end state becomes available - void newStartState(Interface::iterator it, bool updated); - void newEndState(Interface::iterator it, bool updated); + template + void newState(Interface::iterator it, bool updated); // ordered list of pending state pairs ordered pending; diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index 98d003f3..60192d21 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -46,6 +46,9 @@ namespace moveit { namespace task_constructor { namespace stages { class Connect : public Connecting { +protected: + bool compatible(const InterfaceState &from_state, const InterfaceState &to_state) const override; + public: typedef std::vector> GroupPlannerVector; Connect(std::string name, const GroupPlannerVector& planners); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 0ddf26a1..fd002cb9 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -38,11 +38,11 @@ #include #include #include +#include #include #include #include -using namespace std::placeholders; namespace moveit { namespace task_constructor { void InitStageException::push_back(const Stage &stage, const std::string &msg) @@ -310,14 +310,14 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction { if (dir & PropagatingEitherWay::FORWARD) { if (!starts_) // keep existing interface if possible - starts_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedStarts, this, _1))); + 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, _1))); + ends_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedEnds, this, std::placeholders::_1))); } else { ends_.reset(); } @@ -573,7 +573,7 @@ void MonitoringGenerator::init(const moveit::core::RobotModelConstPtr& robot_mod if (!impl->monitored_) throw InitStageException(*this, "no monitored stage defined"); if (!impl->registered_) { // register only once - impl->cb_ = impl->monitored_->addSolutionCallback(std::bind(&MonitoringGenerator::onNewSolution, this, _1)); + impl->cb_ = impl->monitored_->addSolutionCallback(std::bind(&MonitoringGenerator::onNewSolution, this, std::placeholders::_1)); impl->registered_ = true; } } @@ -582,15 +582,27 @@ void MonitoringGenerator::init(const moveit::core::RobotModelConstPtr& robot_mod ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name) : ComputeBasePrivate(me, name) { - starts_.reset(new Interface(std::bind(&ConnectingPrivate::newStartState, this, _1, _2))); - ends_.reset(new Interface(std::bind(&ConnectingPrivate::newEndState, this, _1, _2))); + starts_.reset(new Interface(std::bind(&ConnectingPrivate::newState, this, std::placeholders::_1, std::placeholders::_2))); + ends_.reset(new Interface(std::bind(&ConnectingPrivate::newState, this, std::placeholders::_1, std::placeholders::_2))); } InterfaceFlags ConnectingPrivate::requiredInterface() const { return InterfaceFlags(CONNECT); } -void ConnectingPrivate::newStartState(Interface::iterator it, bool updated) +template <> +ConnectingPrivate::StatePair ConnectingPrivate::make_pair +(Interface::const_iterator first, Interface::const_iterator second) { + return std::make_pair(first, second); +} +template <> +ConnectingPrivate::StatePair ConnectingPrivate::make_pair +(Interface::const_iterator first, Interface::const_iterator second) { + return std::make_pair(second, first); +} + +template +void ConnectingPrivate::newState(Interface::iterator it, bool updated) { // TODO: only consider interface states with priority depth > threshold if (!std::isfinite(it->priority().cost())) { @@ -603,30 +615,12 @@ void ConnectingPrivate::newStartState(Interface::iterator it, bool updated) // many pairs might be affected: sort pending.sort(); } else { // new state: insert all pairs with other interface - for (auto oit = ends_->begin(), oend = ends_->end(); oit != oend; ++oit) { + InterfacePtr other_interface = pullInterface(other); + for (auto oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { if (!std::isfinite(oit->priority().cost())) break; - pending.insert(std::make_pair(it, oit)); - } - } -} - -void ConnectingPrivate::newEndState(Interface::iterator it, bool updated) -{ - if (!std::isfinite(it->priority().cost())) { - // remove pending pairs, if cost updated to infinity - if (updated) - pending.remove_if([it](const StatePair& p) { return p.second == it; }); - return; - } - if (updated) { - // many pairs might be affected: sort - pending.sort(); - } else { // new state: insert all pairs with other interface - for (auto oit = starts_->begin(), oend = starts_->end(); oit != oend; ++oit) { - if (!std::isfinite(oit->priority().cost())) - break; - pending.insert(std::make_pair(oit, it)); + if (static_cast(me_)->compatible(*it, *oit)) + pending.insert(make_pair(it, oit)); } } } @@ -654,6 +648,30 @@ void Connecting::reset() ComputeBase::reset(); } +/// compare consistency of planning scenes +bool Connecting::compatible(const InterfaceState& from_state, const InterfaceState& to_state) const +{ + const planning_scene::PlanningSceneConstPtr& from = from_state.scene(); + const planning_scene::PlanningSceneConstPtr& to = to_state.scene(); + + if (from->getWorld()->size() != to->getWorld()->size()) + return false; // different number of collision objects + + // both scenes should have the same set of collision objects, at the same location + for (const auto& from_object_pair : *from->getWorld()) { + const collision_detection::World::ObjectPtr& from_object = from_object_pair.second; + const collision_detection::World::ObjectConstPtr& to_object = to->getWorld()->getObject(from_object_pair.first); + if (!to_object) return false; // object missing + if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) return false; // shapes not matching + + for (auto from_it = from_object->shape_poses_.cbegin(), from_end = from_object->shape_poses_.cend(), + to_it = to_object->shape_poses_.cbegin(); from_it != from_end; ++from_it, ++to_it) + if (!from_it->matrix().array().isApprox(to_it->matrix().array())) + return false; // transforms do not match + } + return true; +} + void Connecting::connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& t) { newSolution(from, to, pimpl()->addTrajectory(std::move(t))); } diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 4a86e049..0a12c5fd 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -81,6 +81,39 @@ void Connect::init(const core::RobotModelConstPtr& robot_model) throw errors; } +bool Connect::compatible(const InterfaceState& from_state, const InterfaceState& to_state) const +{ + if (!Connecting::compatible(from_state, to_state)) + return false; + + const moveit::core::RobotState& from = from_state.scene()->getCurrentState(); + const moveit::core::RobotState& to = to_state.scene()->getCurrentState(); + + // compose set of joint names we plan for + std::set planned_joint_names; + for (const GroupPlannerVector::value_type& pair : planner_) { + const moveit::core::JointModelGroup *jmg = from.getJointModelGroup(pair.first); + const auto &names = jmg->getJointModelNames(); + planned_joint_names.insert(names.begin(), names.end()); + } + // all active joints that we don't plan for should match + for (const moveit::core::JointModel* jm : from.getRobotModel()->getJointModels()) { + if (planned_joint_names.count(jm->getName())) + continue; // ignore joints we plan for + + const unsigned int num = jm->getVariableCount(); + Eigen::Map positions_from (from.getJointPositions(jm), num); + Eigen::Map positions_to (to.getJointPositions(jm), num); + if (!positions_from.array().isApprox(positions_to.array())) { + ROS_INFO_STREAM_ONCE_NAMED("Connect", "Deviation in joint " << jm->getName() + << ": [" << positions_from.transpose() + << "] != [" << positions_to.transpose() << "]"); + return false; + } + } + return true; +} + bool Connect::compute(const InterfaceState &from, const InterfaceState &to) { const auto& props = properties(); double timeout = props.get("timeout");