mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Connecting::compatible() to check compatibility of states
This commit is contained in:
parent
77442c0eed
commit
85e88eabd1
@ -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);
|
||||
|
||||
@ -235,6 +235,10 @@ class ConnectingPrivate : public ComputeBasePrivate {
|
||||
return x.first->priority() + x.second->priority() < y.first->priority() + y.second->priority();
|
||||
}
|
||||
};
|
||||
|
||||
template<Interface::Direction other>
|
||||
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<Interface::Direction other>
|
||||
void newState(Interface::iterator it, bool updated);
|
||||
|
||||
// ordered list of pending state pairs
|
||||
ordered<StatePair, StatePairLess> pending;
|
||||
|
||||
@ -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<std::pair<std::string, solvers::PlannerInterfacePtr>> GroupPlannerVector;
|
||||
Connect(std::string name, const GroupPlannerVector& planners);
|
||||
|
||||
@ -38,11 +38,11 @@
|
||||
#include <moveit/task_constructor/stage_p.h>
|
||||
#include <moveit/task_constructor/container_p.h>
|
||||
#include <moveit/task_constructor/introspection.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
#include <ros/console.h>
|
||||
|
||||
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<Interface::BACKWARD>, this, std::placeholders::_1, std::placeholders::_2)));
|
||||
ends_.reset(new Interface(std::bind(&ConnectingPrivate::newState<Interface::FORWARD>, 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::BACKWARD>
|
||||
(Interface::const_iterator first, Interface::const_iterator second) {
|
||||
return std::make_pair(first, second);
|
||||
}
|
||||
template <>
|
||||
ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>
|
||||
(Interface::const_iterator first, Interface::const_iterator second) {
|
||||
return std::make_pair(second, first);
|
||||
}
|
||||
|
||||
template <Interface::Direction other>
|
||||
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<Connecting*>(me_)->compatible(*it, *oit))
|
||||
pending.insert(make_pair<other>(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)));
|
||||
}
|
||||
|
||||
@ -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<std::string> 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<const Eigen::VectorXd> positions_from (from.getJointPositions(jm), num);
|
||||
Eigen::Map<const Eigen::VectorXd> 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<double>("timeout");
|
||||
|
||||
Loading…
Reference in New Issue
Block a user