diff --git a/core/demo/plan_pick_pa10.cpp b/core/demo/plan_pick_pa10.cpp index 3fa104ae..3a570cc3 100644 --- a/core/demo/plan_pick_pa10.cpp +++ b/core/demo/plan_pick_pa10.cpp @@ -73,16 +73,10 @@ int main(int argc, char** argv){ stage->setMaxPenetration(0.1); t.add(std::move(stage)); } - { - auto move = std::make_unique("open gripper", pipeline); - move->restrictDirection(stages::MoveTo::FORWARD); - move->properties().property("group").configureInitFrom(Stage::PARENT, "gripper"); - move->setGoal("open"); - t.add(std::move(move)); - } { - auto move = std::make_unique("move to object", pipeline); + stages::Connect::GroupPlannerVector planners = {{"left_hand", pipeline}, {"left_arm", pipeline}}; + auto move = std::make_unique("connect", planners); move->properties().configureInitFrom(Stage::PARENT); t.add(std::move(move)); } diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 67027150..435fbb67 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -351,6 +351,8 @@ public: trajectory.setCost(cost); connect(from, to, std::move(trajectory)); } + + void newSolution(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution); }; } } diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index 566d3489..98d003f3 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Michael Goerner, Robert Haschke +/* Authors: Robert Haschke, Michael Goerner Desc: Connect arbitrary states by motion planning */ @@ -47,7 +47,8 @@ namespace moveit { namespace task_constructor { namespace stages { class Connect : public Connecting { public: - Connect(std::string name, const solvers::PlannerInterfacePtr &planner); + typedef std::vector> GroupPlannerVector; + Connect(std::string name, const GroupPlannerVector& planners); void setTimeout(const ros::Duration& timeout){ setProperty("timeout", timeout.toSec()); @@ -57,11 +58,20 @@ public: setProperty("path_constraints", std::move(path_constraints)); } - void init(const moveit::core::RobotModelConstPtr& robot_model); - bool compute(const InterfaceState &from, const InterfaceState &to); + void reset() override; + void init(const moveit::core::RobotModelConstPtr& robot_model) override; + bool compute(const InterfaceState &from, const InterfaceState &to) override; + + size_t numSolutions() const override { return solutions_.size(); } + size_t numFailures() const override { return 0; } + void processSolutions(const SolutionProcessor &processor) const override; + void processFailures(const SolutionProcessor &processor) const override { return; } protected: - solvers::PlannerInterfacePtr planner_; + GroupPlannerVector planner_; + std::list subsolutions_; + std::list solutions_; + std::list states_; }; } } } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 11ace2c7..0ddf26a1 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -655,11 +655,13 @@ void Connecting::reset() } void Connecting::connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& t) { - auto impl = pimpl(); - SubTrajectory& trajectory = impl->addTrajectory(std::move(t)); - trajectory.setStartState(from); - trajectory.setEndState(to); - impl->newSolution(trajectory); + newSolution(from, to, pimpl()->addTrajectory(std::move(t))); +} + +void Connecting::newSolution(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution) { + solution.setStartState(from); + solution.setEndState(to); + pimpl()->newSolution(solution); } std::ostream& operator<<(std::ostream& os, const Stage& stage) { diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 8e1c83ea..4a86e049 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -41,9 +41,9 @@ namespace moveit { namespace task_constructor { namespace stages { -Connect::Connect(std::string name, const solvers::PlannerInterfacePtr& planner) +Connect::Connect(std::string name, const GroupPlannerVector& planners) : Connecting(name) - , planner_(planner) + , planner_(planners) { auto& p = properties(); p.declare("timeout", 10.0, "planning timeout"); @@ -52,25 +52,80 @@ Connect::Connect(std::string name, const solvers::PlannerInterfacePtr& planner) "constraints to maintain during trajectory"); } +void Connect::reset() +{ + Connecting::reset(); + solutions_.clear(); + subsolutions_.clear(); + states_.clear(); +} + void Connect::init(const core::RobotModelConstPtr& robot_model) { Connecting::init(robot_model); - planner_->init(robot_model); + + InitStageException errors; + if (planner_.empty()) + errors.push_back(*this, "empty set of groups"); + + for (const GroupPlannerVector::value_type& pair : planner_) { + if (!robot_model->hasJointModelGroup(pair.first)) + errors.push_back(*this, "invalid group: " + pair.first); + else if (!pair.second) + errors.push_back(*this, "invalid planner for group: " + pair.first); + else + pair.second->init(robot_model); + } + + if (errors) + throw errors; } bool Connect::compute(const InterfaceState &from, const InterfaceState &to) { const auto& props = properties(); - const std::string& group = props.get("group"); double timeout = props.get("timeout"); - const moveit::core::JointModelGroup* jmg = from.scene()->getRobotModel()->getJointModelGroup(group); + const auto& path_constraints = props.get("path_constraints"); - robot_trajectory::RobotTrajectoryPtr trajectory; - if (!planner_->plan(from.scene(), to.scene(), jmg, timeout, trajectory, - props.get("path_constraints"))) - return false; + SolutionSequence::container_type subsolutions; + planning_scene::PlanningScenePtr start = from.scene()->diff(); + const moveit::core::RobotState& goal_state = to.scene()->getCurrentState(); - connect(from, to, trajectory); + for (const GroupPlannerVector::value_type& pair : planner_) { + // set intermediate goal state + planning_scene::PlanningScenePtr end = start->diff(); + const moveit::core::JointModelGroup *jmg = goal_state.getJointModelGroup(pair.first); + std::vector positions; + goal_state.copyJointGroupPositions(jmg, positions); + end->getCurrentStateNonConst().setJointGroupPositions(jmg, positions); + + robot_trajectory::RobotTrajectoryPtr trajectory; + bool success = pair.second->plan(start, to.scene(), jmg, timeout, trajectory, path_constraints); + + // store solution + auto inserted = subsolutions_.insert(subsolutions_.end(), SubTrajectory(trajectory)); + inserted->setCreator(pimpl_); + // push back solution pointer + subsolutions.push_back(&*inserted); + + // provide meaningful start/end states + states_.emplace_back(InterfaceState(start)); + subsolutions_.back().setStartState(states_.back()); + states_.emplace_back(InterfaceState(end)); + subsolutions_.back().setEndState(states_.back()); + if (!success) return false; + + // continue from reached state + start = end; + } + + solutions_.emplace_back(SolutionSequence(std::move(subsolutions), 0.0)); + newSolution(from, to, solutions_.back()); return true; } +void Connect::processSolutions(const Stage::SolutionProcessor& processor) const +{ + +} + } } }