From 76c6ba14fa78f262a41ebdecc07e9fddb6aea691 Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 23 Feb 2018 22:04:07 +0100 Subject: [PATCH 01/16] add a simple GeneratePose stage ComputeIK is a wrapper, so we can't just give it a pose to compute. --- .../task_constructor/stages/generate_pose.h | 63 +++++++++++ core/src/stages/CMakeLists.txt | 2 + core/src/stages/generate_pose.cpp | 100 ++++++++++++++++++ 3 files changed, 165 insertions(+) create mode 100644 core/include/moveit/task_constructor/stages/generate_pose.h create mode 100644 core/src/stages/generate_pose.cpp diff --git a/core/include/moveit/task_constructor/stages/generate_pose.h b/core/include/moveit/task_constructor/stages/generate_pose.h new file mode 100644 index 00000000..d7893639 --- /dev/null +++ b/core/include/moveit/task_constructor/stages/generate_pose.h @@ -0,0 +1,63 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Hamburg University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Michael Goerner + Desc: Generator Stage for poses +*/ + +#pragma once + +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +class GeneratePose : public MonitoringGenerator { +public: + GeneratePose(const std::string& name); + + bool canCompute() const override; + bool compute() override; + + void setPose(const geometry_msgs::PoseStamped pose){ + setProperty("pose", std::move(pose)); + } + +protected: + void onNewSolution(const SolutionBase& s) override; + + std::deque scenes_; +}; + +} } } diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 03959900..1949a14c 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -5,6 +5,7 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/current_state.h ${PROJECT_INCLUDE}/stages/fixed_state.h ${PROJECT_INCLUDE}/stages/generate_grasp_pose.h + ${PROJECT_INCLUDE}/stages/generate_pose.h ${PROJECT_INCLUDE}/stages/compute_ik.h ${PROJECT_INCLUDE}/stages/connect.h @@ -24,6 +25,7 @@ add_library(${PROJECT_NAME}_stages current_state.cpp fixed_state.cpp generate_grasp_pose.cpp + generate_pose.cpp compute_ik.cpp connect.cpp diff --git a/core/src/stages/generate_pose.cpp b/core/src/stages/generate_pose.cpp new file mode 100644 index 00000000..d54eef54 --- /dev/null +++ b/core/src/stages/generate_pose.cpp @@ -0,0 +1,100 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld + Hamburg University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Robert Haschke, Michael Goerner */ + +#include +#include +#include +#include + +#include + +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +GeneratePose::GeneratePose(const std::string& name) + : MonitoringGenerator(name) +{ + auto& p = properties(); + p.declare("pose", "target pose to pass on in spawned states"); +} + +void GeneratePose::onNewSolution(const SolutionBase& s) +{ + scenes_.push_back(s.end()->scene()->diff()); +} + +bool GeneratePose::canCompute() const { + return scenes_.size() > 0; +} + +bool GeneratePose::compute(){ + const auto& props = properties(); + + geometry_msgs::PoseStamped target_pose = props.get("pose"); + + if(scenes_.empty()) throw std::runtime_error("GeneratePose called without checking canCompute."); + + planning_scene::PlanningSceneConstPtr scene = scenes_[0]; + scenes_.pop_front(); + + // take care of frame transforms + const std::string& frame = target_pose.header.frame_id; + if( !frame.empty() && frame != scene->getPlanningFrame() ){ + if ( !scene->knowsFrameTransform(frame) ) + throw std::runtime_error("GeneratePose does not know frame '" + frame + "'"); + + Eigen::Affine3d pose; + tf::poseMsgToEigen(target_pose.pose, pose); + pose = scene->getFrameTransform(target_pose.header.frame_id) * pose; + tf::poseEigenToMsg(pose, target_pose.pose); + target_pose.header.frame_id = scene->getPlanningFrame(); + } + + InterfaceState state(scene); + state.properties().set("target_pose", target_pose); + + SubTrajectory trajectory; + trajectory.setCost(0.0); + + rviz_marker_tools::appendFrame(trajectory.markers(), target_pose, 0.1, "pose frame"); + + spawn(std::move(state), std::move(trajectory)); + return true; +} + +} } } From 603696f29405c1a30211e31b487be8dfa70f09b3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 24 Mar 2018 11:02:03 +0100 Subject: [PATCH 02/16] add missing reset() --- core/include/moveit/task_constructor/stages/generate_pose.h | 3 ++- core/src/stages/generate_pose.cpp | 6 ++++++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/stages/generate_pose.h b/core/include/moveit/task_constructor/stages/generate_pose.h index d7893639..0817e494 100644 --- a/core/include/moveit/task_constructor/stages/generate_pose.h +++ b/core/include/moveit/task_constructor/stages/generate_pose.h @@ -47,6 +47,7 @@ class GeneratePose : public MonitoringGenerator { public: GeneratePose(const std::string& name); + void reset() override; bool canCompute() const override; bool compute() override; @@ -57,7 +58,7 @@ public: protected: void onNewSolution(const SolutionBase& s) override; - std::deque scenes_; + std::deque scenes_; }; } } } diff --git a/core/src/stages/generate_pose.cpp b/core/src/stages/generate_pose.cpp index d54eef54..3d1307a0 100644 --- a/core/src/stages/generate_pose.cpp +++ b/core/src/stages/generate_pose.cpp @@ -53,6 +53,12 @@ GeneratePose::GeneratePose(const std::string& name) p.declare("pose", "target pose to pass on in spawned states"); } +void GeneratePose::reset() +{ + MonitoringGenerator::reset(); + scenes_.clear(); +} + void GeneratePose::onNewSolution(const SolutionBase& s) { scenes_.push_back(s.end()->scene()->diff()); From 3640928a76799d66a5156e5d8c3325d011e80e5a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 24 Mar 2018 11:01:48 +0100 Subject: [PATCH 03/16] postpone pose transformation --- core/src/stages/generate_pose.cpp | 31 +++++++++---------------------- 1 file changed, 9 insertions(+), 22 deletions(-) diff --git a/core/src/stages/generate_pose.cpp b/core/src/stages/generate_pose.cpp index 3d1307a0..a73f923c 100644 --- a/core/src/stages/generate_pose.cpp +++ b/core/src/stages/generate_pose.cpp @@ -37,12 +37,8 @@ #include #include #include -#include - #include - -#include -#include +#include namespace moveit { namespace task_constructor { namespace stages { @@ -68,27 +64,18 @@ bool GeneratePose::canCompute() const { return scenes_.size() > 0; } -bool GeneratePose::compute(){ - const auto& props = properties(); - - geometry_msgs::PoseStamped target_pose = props.get("pose"); - - if(scenes_.empty()) throw std::runtime_error("GeneratePose called without checking canCompute."); - +bool GeneratePose::compute() { + if (scenes_.empty()) + return false; planning_scene::PlanningSceneConstPtr scene = scenes_[0]; scenes_.pop_front(); - // take care of frame transforms - const std::string& frame = target_pose.header.frame_id; - if( !frame.empty() && frame != scene->getPlanningFrame() ){ - if ( !scene->knowsFrameTransform(frame) ) - throw std::runtime_error("GeneratePose does not know frame '" + frame + "'"); - - Eigen::Affine3d pose; - tf::poseMsgToEigen(target_pose.pose, pose); - pose = scene->getFrameTransform(target_pose.header.frame_id) * pose; - tf::poseEigenToMsg(pose, target_pose.pose); + geometry_msgs::PoseStamped target_pose = properties().get("pose"); + if (target_pose.header.frame_id.empty()) target_pose.header.frame_id = scene->getPlanningFrame(); + else if (!scene->knowsFrameTransform(target_pose.header.frame_id)) { + ROS_WARN_NAMED("GeneratePose", "Unknown frame: '%s'", target_pose.header.frame_id.c_str()); + return false; } InterfaceState state(scene); From 9e1c3059e13b0b64483168259edb9462a0f5bae5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 24 Mar 2018 15:14:13 +0100 Subject: [PATCH 04/16] derive GenerateGraspPose from GeneratePose --- .../stages/generate_grasp_pose.h | 11 +-- core/src/stages/CMakeLists.txt | 2 +- core/src/stages/generate_grasp_pose.cpp | 80 ++++++++++++------- 3 files changed, 53 insertions(+), 40 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h index b1974860..1cfc79bf 100644 --- a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h +++ b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h @@ -38,16 +38,15 @@ #pragma once -#include +#include namespace moveit { namespace task_constructor { namespace stages { -class GenerateGraspPose : public MonitoringGenerator { +class GenerateGraspPose : public GeneratePose { public: GenerateGraspPose(const std::string& name); - void reset() override; - bool canCompute() const override; + void init(const core::RobotModelConstPtr &robot_model) override; bool compute() override; void setEndEffector(const std::string &eef); @@ -57,10 +56,6 @@ public: protected: void onNewSolution(const SolutionBase& s) override; - -protected: - planning_scene::PlanningScenePtr scene_; - double current_angle_ = 0.0; }; } } } diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 1949a14c..7ffdc1fe 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -4,8 +4,8 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/current_state.h ${PROJECT_INCLUDE}/stages/fixed_state.h - ${PROJECT_INCLUDE}/stages/generate_grasp_pose.h ${PROJECT_INCLUDE}/stages/generate_pose.h + ${PROJECT_INCLUDE}/stages/generate_grasp_pose.h ${PROJECT_INCLUDE}/stages/compute_ik.h ${PROJECT_INCLUDE}/stages/connect.h diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index 20526f57..441da552 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -47,7 +47,7 @@ namespace moveit { namespace task_constructor { namespace stages { GenerateGraspPose::GenerateGraspPose(const std::string& name) - : MonitoringGenerator(name) + : GeneratePose(name) { auto& p = properties(); p.declare("eef", "name of end-effector"); @@ -56,15 +56,15 @@ GenerateGraspPose::GenerateGraspPose(const std::string& name) p.declare("angle_delta", 0.1, "angular steps (rad)"); } -void GenerateGraspPose::setEndEffector(const std::string &eef){ +void GenerateGraspPose::setEndEffector(const std::string &eef) { setProperty("eef", eef); } -void GenerateGraspPose::setNamedPose(const std::string &pose_name){ +void GenerateGraspPose::setNamedPose(const std::string &pose_name) { setProperty("pregrasp", pose_name); } -void GenerateGraspPose::setObject(const std::string &object){ +void GenerateGraspPose::setObject(const std::string &object) { setProperty("object", object); } @@ -72,50 +72,70 @@ void GenerateGraspPose::setAngleDelta(double delta){ setProperty("angle_delta", delta); } -void GenerateGraspPose::reset() +void GenerateGraspPose::init(const core::RobotModelConstPtr& robot_model) { - scene_.reset(); - current_angle_ = 0.0; - MonitoringGenerator::reset(); + InitStageException errors; + try { GeneratePose::init(robot_model); } + catch (InitStageException &e) { errors.append(e); } + + const auto& props = properties(); + + // check angle_delta + if (props.get("angle_delta") == 0.) + errors.push_back(*this, "angle_delta must be non-zero"); + + // check availability of eef + const std::string& eef = props.get("eef"); + if (!robot_model->hasEndEffector(eef)) + errors.push_back(*this, "unknown end effector: " + eef); + else { + // check availability of eef pose + const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef); + const std::string& name = props.get("pregrasp"); + std::map m; + if (!jmg->getVariableDefaultPositions(name, m)) + errors.push_back(*this, "unknown end effector pose: " + name); + } + + if (errors) throw errors; } void GenerateGraspPose::onNewSolution(const SolutionBase& s) { - if (scene_) - ROS_WARN_NAMED("GenerateGraspPose", "got additional solution from monitored stage"); - scene_ = s.end()->scene()->diff(); -} + planning_scene::PlanningScenePtr scene = s.end()->scene()->diff(); -bool GenerateGraspPose::canCompute() const{ - return scene_ && current_angle_ < 2*M_PI && current_angle_ > -2*M_PI; -} - -bool GenerateGraspPose::compute(){ + // set end effector pose const auto& props = properties(); const std::string& eef = props.get("eef"); + const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef); - assert(scene_->getRobotModel()->hasEndEffector(eef) && "The specified end effector is not defined in the srdf"); - const moveit::core::JointModelGroup* jmg = scene_->getRobotModel()->getEndEffector(eef); + robot_state::RobotState &robot_state = scene->getCurrentStateNonConst(); + robot_state.setToDefaultValues(jmg , props.get("pregrasp")); - robot_state::RobotState &robot_state = scene_->getCurrentStateNonConst(); - const std::string& joint_pose = props.get("pregrasp"); - if(!joint_pose.empty()){ - robot_state.setToDefaultValues(jmg , joint_pose); - } + scenes_.push_back(scene); +} +bool GenerateGraspPose::compute() { + if (scenes_.empty()) + return false; + planning_scene::PlanningSceneConstPtr scene = scenes_[0]; + scenes_.pop_front(); + + const auto& props = properties(); const std::string& object_name = props.get("object"); - if (!scene_->knowsFrameTransform(object_name)) - throw std::runtime_error("requested object does not exist or could not be retrieved"); + if (!scene->knowsFrameTransform(object_name)) + throw std::runtime_error("unknown object: " + object_name); geometry_msgs::PoseStamped target_pose_msg; target_pose_msg.header.frame_id = object_name; - while( canCompute() ) { + double current_angle_ = 0.0; + while (current_angle_ < 2.*M_PI && current_angle_ > -2.*M_PI) { // rotate object pose about z-axis Eigen::Affine3d target_pose(Eigen::AngleAxisd(current_angle_, Eigen::Vector3d::UnitZ())); current_angle_ += props.get("angle_delta"); - InterfaceState state(scene_); + InterfaceState state(scene); tf::poseEigenToMsg(target_pose, target_pose_msg.pose); state.properties().set("target_pose", target_pose_msg); @@ -127,10 +147,8 @@ bool GenerateGraspPose::compute(){ rviz_marker_tools::appendFrame(trajectory.markers(), target_pose_msg, 0.1, "grasp frame"); spawn(std::move(state), std::move(trajectory)); - return true; } - - return false; + return true; } } } } From 4d77390d77716262ba0a9fe1ff241f73295b5582 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 25 Mar 2018 07:33:56 +0200 Subject: [PATCH 05/16] validate existence of object frame in init() --- core/src/stages/generate_grasp_pose.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index 441da552..9609c0d1 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -112,6 +112,12 @@ void GenerateGraspPose::onNewSolution(const SolutionBase& s) robot_state::RobotState &robot_state = scene->getCurrentStateNonConst(); robot_state.setToDefaultValues(jmg , props.get("pregrasp")); + const std::string& object_name = props.get("object"); + if (!scene->knowsFrameTransform(object_name)) { + ROS_WARN_STREAM_NAMED("GenerateGraspPose", "unknown object: " << object_name); + return; + } + scenes_.push_back(scene); } @@ -122,12 +128,9 @@ bool GenerateGraspPose::compute() { scenes_.pop_front(); const auto& props = properties(); - const std::string& object_name = props.get("object"); - if (!scene->knowsFrameTransform(object_name)) - throw std::runtime_error("unknown object: " + object_name); geometry_msgs::PoseStamped target_pose_msg; - target_pose_msg.header.frame_id = object_name; + target_pose_msg.header.frame_id = props.get("object"); double current_angle_ = 0.0; while (current_angle_ < 2.*M_PI && current_angle_ > -2.*M_PI) { From 9ceecd235f7b8ff8fda649dac2dc8e7fcc45fa9f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 12 Mar 2018 22:11:14 +0100 Subject: [PATCH 06/16] SerialSolution -> SolutionSequence --- .../moveit/task_constructor/container.h | 7 +--- .../moveit/task_constructor/container_p.h | 23 +---------- .../include/moveit/task_constructor/storage.h | 35 +++++++++++++++-- core/src/container.cpp | 38 ++++--------------- core/src/storage.cpp | 30 ++++++++++++++- 5 files changed, 71 insertions(+), 62 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index a9098c0d..b5bb0c43 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -107,14 +107,11 @@ public: size_t numSolutions() const override; void processSolutions(const SolutionProcessor &processor) const override; - /// container used to represent a serial solution - typedef std::vector solution_container; - protected: /// called by a (direct) child when a new solution becomes available void onNewSolution(const SolutionBase &s) override; - typedef std::function SolutionProcessor; /// Traverse all solution pathes starting at start and going in given direction dir @@ -123,7 +120,7 @@ protected: /// the full trace (from start to end, but not including start) and its accumulated costs template void traverse(const SolutionBase &start, const SolutionProcessor &cb, - solution_container &trace, double trace_cost = 0); + SolutionSequence::container_type &trace, double trace_cost = 0); protected: SerialContainer(SerialContainerPrivate* impl); diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 79c184cf..e9439548 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -136,27 +136,6 @@ protected: }; PIMPL_FUNCTIONS(ContainerBase) -/** Representation of a single, full solution path of a SerialContainer. - * - * A serial solution describes a full solution path through all children - * of a SerialContainer. This is a vector (of children().size()) of pointers - * to all solutions of the children. Hence, we don't need to copy those solutions. */ -class SerialSolution : public SolutionBase { -public: - explicit SerialSolution(StagePrivate* creator, SerialContainer::solution_container&& subsolutions, double cost) - : SolutionBase(creator, cost), subsolutions_(subsolutions) - {} - /// append all subsolutions to solution - void fillMessage(moveit_task_constructor_msgs::Solution &msg, Introspection *introspection) const override; - - inline const InterfaceState* internalStart() const { return subsolutions_.front()->start(); } - inline const InterfaceState* internalEnd() const { return subsolutions_.back()->end(); } - -private: - /// series of sub solutions - SerialContainer::solution_container subsolutions_; -}; - /* A solution of a SerialContainer needs to connect start to end via a full path. * The solution of a single child stage is usually disconnected to the container's start or end. @@ -188,7 +167,7 @@ private: InterfaceFlags accepted); // set of all solutions - ordered solutions_; + ordered solutions_; }; PIMPL_FUNCTIONS(SerialContainer) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index c91e2441..163c0017 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -172,11 +172,9 @@ private: class StagePrivate; -class SubTrajectory; -/// SolutionTrajectory is composed of a series of SubTrajectories -typedef std::vector SolutionTrajectory; +/// abstract base class for solutions (primitive and sequences) class SolutionBase { public: inline const InterfaceState* start() const { return start_; } @@ -241,7 +239,7 @@ private: }; -// SubTrajectory connects interface states of ComputeStages +/// SubTrajectory connects interface states of ComputeStages class SubTrajectory : public SolutionBase { public: SubTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory = robot_trajectory::RobotTrajectoryPtr()) @@ -260,6 +258,35 @@ private: }; +/** Sequence of individual sub solutions + * + * A solution sequence describes a solution that is composed from several individual + * sub solutions that need to be chained together to yield the overall solutions. + */ +class SolutionSequence : public SolutionBase { +public: + typedef std::vector container_type; + + explicit SolutionSequence() + : SolutionBase() + {} + SolutionSequence(container_type&& subsolutions, double cost = 0.0, StagePrivate* creator = nullptr) + : SolutionBase(creator, cost), subsolutions_(subsolutions) + {} + + void push_back(const SolutionBase& solution); + + /// append all subsolutions to solution + void fillMessage(moveit_task_constructor_msgs::Solution &msg, Introspection *introspection) const override; + + inline const InterfaceState* internalStart() const { return subsolutions_.front()->start(); } + inline const InterfaceState* internalEnd() const { return subsolutions_.back()->end(); } + +private: + /// series of sub solutions + container_type subsolutions_; +}; + template <> inline const InterfaceState::Solutions& SolutionBase::trajectories() const { return end_->outgoingTrajectories(); diff --git a/core/src/container.cpp b/core/src/container.cpp index aa2ff79c..7b12e37e 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -283,18 +283,18 @@ std::ostream& operator<<(std::ostream& os, const ContainerBase& container) { struct SolutionCollector { SolutionCollector(size_t max_depth) : max_depth(max_depth) {} - void operator()(const SerialContainer::solution_container& trace, double cost) { + void operator()(const SolutionSequence::container_type& trace, double cost) { // traced path should not extend past container boundaries assert(trace.size() <= max_depth); solutions.emplace_back(std::make_pair(trace, cost)); } - typedef std::list> SolutionCostPairs; + typedef std::list> SolutionCostPairs; SolutionCostPairs solutions; const size_t max_depth; }; -void updateStateCosts(const SerialContainer::solution_container &partial_solution_path, +void updateStateCosts(const SolutionSequence::container_type &partial_solution_path, const InterfaceState::Priority &prio) { for (const SolutionBase* solution : partial_solution_path) { // here it suffices to update the start state, because the end state is the start state @@ -322,7 +322,7 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t) assert(num_before < children.size()); // creator should be one of our children num_after = children.size()-1 - num_before; - SerialContainer::solution_container trace; trace.reserve(children.size()); + SolutionSequence::container_type trace; trace.reserve(children.size()); // find all incoming solution paths ending at current solution SolutionCollector incoming(num_before); @@ -333,8 +333,8 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t) traverse(current, std::ref(outgoing), trace); // collect (and sort) all solutions spanning from start to end of this container - ordered sorted; - SerialContainer::solution_container solution; + ordered sorted; + SolutionSequence::container_type solution; solution.reserve(children.size()); for (auto& in : incoming.solutions) { for (auto& out : outgoing.solutions) { @@ -352,7 +352,7 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t) // insert outgoing solutions in normal order solution.insert(solution.end(), out.first.begin(), out.first.end()); // store solution in sorted list - sorted.insert(SerialSolution(impl, std::move(solution), prio.cost())); + sorted.insert(SolutionSequence(std::move(solution), prio.cost(), impl)); } else if (prio.depth() > 1) { // update state priorities along the whole partial solution path updateStateCosts(in.first, prio); @@ -655,7 +655,7 @@ void SerialContainer::processSolutions(const ContainerBase::SolutionProcessor &p template void SerialContainer::traverse(const SolutionBase &start, const SolutionProcessor &cb, - solution_container &trace, double trace_cost) + SolutionSequence::container_type &trace, double trace_cost) { const InterfaceState::Solutions& solutions = start.trajectories(); if (solutions.empty()) // if we reached the end, call the callback @@ -671,28 +671,6 @@ void SerialContainer::traverse(const SolutionBase &start, const SolutionProcesso } } -void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg, - Introspection* introspection) const -{ - moveit_task_constructor_msgs::SubSolution sub_msg; - sub_msg.id = introspection ? introspection->solutionId(*this) : 0; - sub_msg.cost = this->cost(); - - const Introspection *ci = introspection; - sub_msg.stage_id = ci ? ci->stageId(this->creator()->me()) : 0; - - sub_msg.sub_solution_id.reserve(subsolutions_.size()); - if (introspection) { - for (const SolutionBase* s : subsolutions_) - sub_msg.sub_solution_id.push_back(introspection->solutionId(*s)); - msg.sub_solution.push_back(sub_msg); - } - - msg.sub_trajectory.reserve(msg.sub_trajectory.size() + subsolutions_.size()); - for (const SolutionBase* s : subsolutions_) - s->fillMessage(msg, introspection); -} - void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution &solution, Introspection *introspection) const diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 360ceb21..bd0b03d2 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -127,7 +127,6 @@ void SolutionBase::setCost(double cost) { - void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution &msg, Introspection *introspection) const { msg.sub_trajectory.emplace_back(); @@ -150,4 +149,33 @@ void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution &msg, } + +void SolutionSequence::push_back(const SolutionBase& solution) +{ + subsolutions_.push_back(&solution); +} + +void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution &msg, + Introspection* introspection) const +{ + moveit_task_constructor_msgs::SubSolution sub_msg; + sub_msg.id = introspection ? introspection->solutionId(*this) : 0; + sub_msg.cost = this->cost(); + + const Introspection *ci = introspection; + sub_msg.stage_id = ci ? ci->stageId(this->creator()->me()) : 0; + + sub_msg.sub_solution_id.reserve(subsolutions_.size()); + if (introspection) { + for (const SolutionBase* s : subsolutions_) + sub_msg.sub_solution_id.push_back(introspection->solutionId(*s)); + msg.sub_solution.push_back(sub_msg); + } + + msg.sub_trajectory.reserve(msg.sub_trajectory.size() + subsolutions_.size()); + for (const SolutionBase* s : subsolutions_) + s->fillMessage(msg, introspection); +} + + } } From 8811045f89386317caf14a682398080f3481f68f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 12 Mar 2018 23:23:54 +0100 Subject: [PATCH 07/16] setCreator() once in StagePrivate::newSolution() --- core/include/moveit/task_constructor/stage.h | 6 ++---- core/include/moveit/task_constructor/stage_p.h | 2 +- core/src/container.cpp | 1 - core/src/stage.cpp | 8 ++++---- 4 files changed, 7 insertions(+), 10 deletions(-) diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index a627d8fb..67027150 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -346,10 +346,8 @@ public: void reset() override; virtual bool compute(const InterfaceState& from, const InterfaceState& to) = 0; - void connect(const InterfaceState& from, const InterfaceState& to, - SubTrajectory&& trajectory); - void connect(const InterfaceState& from, const InterfaceState& to, - SubTrajectory&& trajectory, double cost) { + void connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& trajectory); + void connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& trajectory, double cost) { trajectory.setCost(cost); connect(from, to, std::move(trajectory)); } diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index ecc5eb43..90320699 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -103,7 +103,7 @@ public: inline void setIntrospection(Introspection* introspection) { introspection_ = introspection; } void composePropertyErrorMsg(const std::string& name, std::ostream& os); - void newSolution(const SolutionBase ¤t); + void newSolution(SolutionBase& current); protected: Stage* const me_; // associated/owning Stage instance diff --git a/core/src/container.cpp b/core/src/container.cpp index 7b12e37e..f8cc800f 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -856,7 +856,6 @@ void ParallelContainerBase::spawn(InterfaceState &&state, SubTrajectory&& t) auto impl = pimpl(); assert(impl->prevEnds() && impl->nextStarts()); - t.setCreator(impl); // store newly created solution (otherwise it's lost) auto it = impl->created_solutions_.insert(impl->created_solutions_.end(), std::move(t)); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 67260434..11ace2c7 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -83,8 +83,10 @@ InterfaceFlags StagePrivate::interfaceFlags() const return f; } -void StagePrivate::newSolution(const SolutionBase &solution) +void StagePrivate::newSolution(SolutionBase &solution) { + solution.setCreator(this); + if (introspection_) introspection_->registerSolution(solution); @@ -249,7 +251,6 @@ std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) { } SubTrajectory& ComputeBasePrivate::addTrajectory(SubTrajectory&& trajectory) { - trajectory.setCreator(this); if (!trajectory.isFailure()) { return *solutions_.insert(std::move(trajectory)); } else if (me()->storeFailures()) { @@ -653,8 +654,7 @@ void Connecting::reset() ComputeBase::reset(); } -void Connecting::connect(const InterfaceState& from, const InterfaceState& to, - SubTrajectory&& t) { +void Connecting::connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& t) { auto impl = pimpl(); SubTrajectory& trajectory = impl->addTrajectory(std::move(t)); trajectory.setStartState(from); From 77442c0eed608168127415b3f865d3e98e2d13e5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 12 Mar 2018 23:32:56 +0100 Subject: [PATCH 08/16] Connect: allow multiple groups to be processed in series --- core/demo/plan_pick_pa10.cpp | 10 +-- core/include/moveit/task_constructor/stage.h | 2 + .../moveit/task_constructor/stages/connect.h | 20 +++-- core/src/stage.cpp | 12 +-- core/src/stages/connect.cpp | 75 ++++++++++++++++--- 5 files changed, 91 insertions(+), 28 deletions(-) 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 +{ + +} + } } } From 85e88eabd1c6fe0c9912f6a50cbc995306be334e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 13 Mar 2018 06:30:07 +0100 Subject: [PATCH 09/16] Connecting::compatible() to check compatibility of states --- core/include/moveit/task_constructor/stage.h | 3 + .../include/moveit/task_constructor/stage_p.h | 8 +- .../moveit/task_constructor/stages/connect.h | 3 + core/src/stage.cpp | 76 ++++++++++++------- core/src/stages/connect.cpp | 33 ++++++++ 5 files changed, 92 insertions(+), 31 deletions(-) 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"); From 97d2eb5c6a8a14def1bc980da59162d5ccb18799 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 12 Mar 2018 23:33:59 +0100 Subject: [PATCH 10/16] Pick: remove Connect --- core/demo/plan_pick_trixi.cpp | 16 ++++++++++++-- .../moveit/task_constructor/stages/pick.h | 3 --- core/src/stages/pick.cpp | 22 ------------------- 3 files changed, 14 insertions(+), 27 deletions(-) diff --git a/core/demo/plan_pick_trixi.cpp b/core/demo/plan_pick_trixi.cpp index e49d0209..9a04253c 100644 --- a/core/demo/plan_pick_trixi.cpp +++ b/core/demo/plan_pick_trixi.cpp @@ -3,10 +3,10 @@ #include #include #include +#include +#include #include -#include -#include #include @@ -45,6 +45,17 @@ int main(int argc, char** argv){ initial_stage = initial.get(); t.add(std::move(initial)); + // planner used for connect + auto pipeline = std::make_shared(); + pipeline->setTimeout(8.0); + pipeline->setPlannerId("RRTConnectkConfigDefault"); + // connect to pick + stages::Connect::GroupPlannerVector planners = {{"left_arm", pipeline}, {"left_gripper", pipeline}}; + auto connect = std::make_unique("connect", planners); + connect->properties().configureInitFrom(Stage::PARENT); + t.add(std::move(connect)); + + // grasp generator auto grasp_generator = std::make_unique(); grasp_generator->setIKFrame(Eigen::Affine3d::Identity(), "l_gripper_tool_frame"); grasp_generator->setAngleDelta(.2); @@ -52,6 +63,7 @@ int main(int argc, char** argv){ grasp_generator->setGraspPose("closed"); grasp_generator->setMonitoredStage(initial_stage); + // pick stage auto pick = std::make_unique(std::move(grasp_generator)); pick->setProperty("eef", std::string("left_gripper")); pick->setProperty("object", std::string("object")); diff --git a/core/include/moveit/task_constructor/stages/pick.h b/core/include/moveit/task_constructor/stages/pick.h index 0b828304..a12c65f3 100644 --- a/core/include/moveit/task_constructor/stages/pick.h +++ b/core/include/moveit/task_constructor/stages/pick.h @@ -44,7 +44,6 @@ namespace moveit { namespace task_constructor { namespace solvers { MOVEIT_CLASS_FORWARD(CartesianPath) -MOVEIT_CLASS_FORWARD(PipelinePlanner) } namespace stages { @@ -63,7 +62,6 @@ namespace stages { */ class Pick : public SerialContainer { solvers::CartesianPathPtr cartesian_solver_; - solvers::PipelinePlannerPtr pipeline_solver_; Stage* grasp_stage_ = nullptr; Stage* approach_stage_ = nullptr; Stage* lift_stage_ = nullptr; @@ -81,7 +79,6 @@ public: } solvers::CartesianPathPtr cartesianSolver() { return cartesian_solver_; } - solvers::PipelinePlannerPtr pipelineSolver() { return pipeline_solver_; } void setApproachMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance); diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index 122c35d8..50504889 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -1,13 +1,9 @@ #include #include -#include #include -#include #include -#include -#include #include @@ -26,24 +22,6 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name) p.declare("eef_parent_group", "JMG of eef's parent"); cartesian_solver_ = std::make_shared(); - pipeline_solver_ = std::make_shared(); - pipeline_solver_->setTimeout(8.0); - pipeline_solver_->setPlannerId("RRTConnectkConfigDefault"); - - { - auto move = std::make_unique("open gripper", pipeline_solver_); - PropertyMap& p = move->properties(); - p.property("group").configureInitFrom(Stage::PARENT, "eef_group"); - move->setGoal("open"); // TODO: retrieve from grasp stage - insert(std::move(move)); - } - - { - auto move = std::make_unique("move to object", pipeline_solver_); - PropertyMap& p = move->properties(); - p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); - insert(std::move(move)); - } { auto approach = std::make_unique("approach object", cartesian_solver_); From ce5d7c63f08f149ee53fc97d0d10e67cee8179e7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 13 Mar 2018 00:53:19 +0100 Subject: [PATCH 11/16] SimpleGrasp, SimpleUnGrasp as specializations of SimpleGraspBase --- .../task_constructor/stages/simple_grasp.h | 28 +++++++++++---- core/src/stages/simple_grasp.cpp | 36 ++++++++++--------- 2 files changed, 41 insertions(+), 23 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/simple_grasp.h b/core/include/moveit/task_constructor/stages/simple_grasp.h index 5f903f0b..2e6e9207 100644 --- a/core/include/moveit/task_constructor/stages/simple_grasp.h +++ b/core/include/moveit/task_constructor/stages/simple_grasp.h @@ -46,18 +46,20 @@ namespace moveit { namespace task_constructor { namespace stages { class GenerateGraspPose; -/** Simple Grasp Stage +/** base class for simple grasping / ungrasping * - * Given a named pre-grasp and grasp posture the stage generates - * fully-qualified pre-grasp and grasp robot states, connected - * by a grasping trajectory of the end-effector. + * Given a named pre-grasp and grasp posture the stage generates fully-qualified + * pre-grasp and grasp robot states, connected by a grasping trajectory of the end-effector. + * + * Grasping and UnGrasping only differs in the order of subtasks. Hence, the base class + * provides the common functionality for grasping (forward = true) and ungrasping (forward = false). */ -class SimpleGrasp : public SerialContainer { +class SimpleGraspBase : public SerialContainer { moveit::core::RobotModelConstPtr model_; GenerateGraspPose* grasp_generator_ = nullptr; public: - SimpleGrasp(const std::string& name = "grasp"); + SimpleGraspBase(const std::string& name, bool forward); void init(const moveit::core::RobotModelConstPtr& robot_model) override; void setMonitoredStage(Stage* monitored); @@ -101,4 +103,18 @@ public: } }; + +/// specialization of SimpleGraspBase to realize grasping +class SimpleGrasp : public SimpleGraspBase { +public: + SimpleGrasp(const std::string& name = "grasp") : SimpleGraspBase(name, true) {} +}; + + +/// specialization of SimpleGraspBase to realize ungrasping +class SimpleUnGrasp : public SimpleGraspBase { +public: + SimpleUnGrasp(const std::string& name = "ungrasp") : SimpleGraspBase(name, false) {} +}; + } } } diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp index 1eb919fd..114ec0a4 100644 --- a/core/src/stages/simple_grasp.cpp +++ b/core/src/stages/simple_grasp.cpp @@ -49,23 +49,24 @@ namespace moveit { namespace task_constructor { namespace stages { -SimpleGrasp::SimpleGrasp(const std::string& name) +SimpleGraspBase::SimpleGraspBase(const std::string& name, bool forward) : SerialContainer(name) { + int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order { - auto gengrasp = std::make_unique("generate grasp pose"); + auto gengrasp = std::make_unique(forward ? "generate grasp pose" : "generate release pose"); grasp_generator_ = gengrasp.get(); auto ik = std::make_unique("compute ik", std::move(gengrasp)); const std::initializer_list& grasp_prop_names = { "eef", "pregrasp", "object", "angle_delta" }; ik->exposePropertiesOfChild(0, grasp_prop_names); - insert(std::move(ik)); + insert(std::move(ik), insertion_position); - exposePropertiesOfChild(-1, grasp_prop_names); - exposePropertiesOfChild(-1, { "max_ik_solutions", "timeout", "ik_frame" }); + exposePropertiesOfChild(insertion_position, grasp_prop_names); + exposePropertiesOfChild(insertion_position, { "max_ik_solutions", "timeout", "ik_frame" }); } { - auto allow_touch = std::make_unique("allow object collision"); + auto allow_touch = std::make_unique(forward ? "allow object collision" : "forbid object collision"); PropertyMap& p = allow_touch->properties(); p.declare("eef"); p.declare("object"); @@ -78,53 +79,54 @@ SimpleGrasp::SimpleGrasp(const std::string& name) acm.setEntry(object, scene->getRobotModel()->getEndEffector(eef) ->getLinkModelNamesWithCollisionGeometry(), true); }); - insert(std::move(allow_touch)); + insert(std::move(allow_touch), insertion_position); } { auto pipeline = std::make_shared(); pipeline->setTimeout(8.0); pipeline->setPlannerId("RRTConnectkConfigDefault"); - auto move = std::make_unique("close gripper", pipeline); + auto move = std::make_unique(forward ? "close gripper" : "open gripper", pipeline); PropertyMap& p = move->properties(); p.property("group").configureInitFrom(Stage::PARENT, [this](const PropertyMap& parent_map){ const std::string& eef = parent_map.get("eef"); const moveit::core::JointModelGroup* jmg = model_->getEndEffector(eef); return boost::any(jmg->getName()); }); - insert(std::move(move)); - exposePropertyOfChildAs(-1, "joint_pose", "grasp"); + insert(std::move(move), insertion_position); + exposePropertyOfChildAs(insertion_position, "joint_pose", forward ? "grasp" : "pregrasp"); } { - auto attach = std::make_unique("attach object"); + auto attach = std::make_unique(forward ? "attach object" : "detach object"); PropertyMap& p = attach->properties(); p.declare("eef"); p.declare("object"); p.configureInitFrom(Stage::PARENT, { "eef", "object" }); - attach->setCallback([this](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){ + attach->setCallback([this, forward](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){ const std::string& eef = p.get("eef"); moveit_msgs::AttachedCollisionObject obj; - obj.object.operation = moveit_msgs::CollisionObject::ADD; + obj.object.operation = forward ? (int8_t) moveit_msgs::CollisionObject::ADD + : (int8_t) moveit_msgs::CollisionObject::REMOVE; obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second; obj.object.id = p.get("object"); scene->processAttachedCollisionObjectMsg(obj); }); - insert(std::move(attach)); + insert(std::move(attach), insertion_position); } } -void SimpleGrasp::init(const moveit::core::RobotModelConstPtr& robot_model) +void SimpleGraspBase::init(const moveit::core::RobotModelConstPtr& robot_model) { model_ = robot_model; SerialContainer::init(robot_model); } -void SimpleGrasp::setMonitoredStage(Stage* monitored) +void SimpleGraspBase::setMonitoredStage(Stage* monitored) { grasp_generator_->setMonitoredStage(monitored); } -void SimpleGrasp::setIKFrame(const Eigen::Affine3d& pose, const std::string& link) { +void SimpleGraspBase::setIKFrame(const Eigen::Affine3d& pose, const std::string& link) { geometry_msgs::PoseStamped pose_msg; pose_msg.header.frame_id = link; tf::poseEigenToMsg(pose, pose_msg.pose); From 2c08d9080dae6036e28e982098d012fa466c588f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 13 Mar 2018 01:16:36 +0100 Subject: [PATCH 12/16] Pick, Place as specializations of PickPlaceBase --- .../moveit/task_constructor/stages/pick.h | 59 ++++++++++++++++--- core/src/stages/pick.cpp | 25 ++++---- 2 files changed, 65 insertions(+), 19 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/pick.h b/core/include/moveit/task_constructor/stages/pick.h index a12c65f3..10c2c8db 100644 --- a/core/include/moveit/task_constructor/stages/pick.h +++ b/core/include/moveit/task_constructor/stages/pick.h @@ -48,26 +48,30 @@ MOVEIT_CLASS_FORWARD(CartesianPath) namespace stages { -/** Pick wraps a complete pipeline to pick up an object with a given end effector. +/** PickPlaceBase wraps the pipeline to pick or place an object with a given end effector. * * Picking consist of the following sub stages: - * - reaching to the object + "pre-grasp" end effector posture * - linearly approaching the object along an approach direction/twist * - "grasp" end effector posture * - attach object * - lift along along a given direction/twist * + * Placing consist of the inverse order of stages: + * - place down along a given direction + * - detach the object + * - linearly retract end effector + * * The end effector postures corresponding to pre-grasp and grasp as well as * the end effector's Cartesian pose needs to be provided by an external grasp stage. */ -class Pick : public SerialContainer { +class PickPlaceBase : public SerialContainer { solvers::CartesianPathPtr cartesian_solver_; Stage* grasp_stage_ = nullptr; Stage* approach_stage_ = nullptr; Stage* lift_stage_ = nullptr; public: - Pick(Stage::pointer &&grasp_stage, const std::string& name = "pick"); + PickPlaceBase(Stage::pointer &&grasp_stage, const std::string& name, bool forward); void init(const moveit::core::RobotModelConstPtr& robot_model) override; @@ -80,10 +84,49 @@ public: solvers::CartesianPathPtr cartesianSolver() { return cartesian_solver_; } - void setApproachMotion(const geometry_msgs::TwistStamped& motion, - double min_distance, double max_distance); - void setLiftMotion(const geometry_msgs::TwistStamped& motion, - double min_distance, double max_distance); + void setApproachRetract(const geometry_msgs::TwistStamped& motion, + double min_distance, double max_distance); + void setLiftPlace(const geometry_msgs::TwistStamped& motion, + double min_distance, double max_distance); }; + +/// specialization of PickPlaceBase to realize picking +class Pick : public PickPlaceBase { +public: + Pick(Stage::pointer &&grasp_stage, const std::string& name = "pick") + : PickPlaceBase(std::move(grasp_stage), name, true) + {} + + void setApproachMotion(const geometry_msgs::TwistStamped& motion, + double min_distance, double max_distance) { + setApproachRetract(motion, min_distance, max_distance); + } + + void setLiftMotion(const geometry_msgs::TwistStamped& motion, + double min_distance, double max_distance) { + setLiftPlace(motion, min_distance, max_distance); + } +}; + + +/// specialization of PickPlaceBase to realize placing +class Place : public PickPlaceBase { +public: + Place(Stage::pointer &&ungrasp_stage, const std::string& name = "place") + : PickPlaceBase(std::move(ungrasp_stage), name, false) + {} + + void setRetractMotion(const geometry_msgs::TwistStamped& motion, + double min_distance, double max_distance) { + setApproachRetract(motion, min_distance, max_distance); + } + + void setPlaceMotion(const geometry_msgs::TwistStamped& motion, + double min_distance, double max_distance) { + setLiftPlace(motion, min_distance, max_distance); + } +}; + + } } } diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index 50504889..b098749f 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -9,7 +9,7 @@ namespace moveit { namespace task_constructor { namespace stages { -Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name) +PickPlaceBase::PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& name, bool forward) : SerialContainer(name) { PropertyMap& p = properties(); @@ -22,33 +22,34 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name) p.declare("eef_parent_group", "JMG of eef's parent"); cartesian_solver_ = std::make_shared(); + int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order { - auto approach = std::make_unique("approach object", cartesian_solver_); + auto approach = std::make_unique(forward ? "approach object" : "retract", cartesian_solver_); PropertyMap& p = approach->properties(); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); p.property("link").configureInitFrom(Stage::PARENT, "eef_frame"); - p.set("marker_ns", std::string("approach")); + p.set("marker_ns", std::string(forward ? "approach" : "retract")); approach_stage_ = approach.get(); - insert(std::move(approach)); + insert(std::move(approach), insertion_position); } grasp_stage_ = grasp_stage.get(); grasp_stage->properties().configureInitFrom(Stage::PARENT, {"eef", "object"}); - insert(std::move(grasp_stage)); + insert(std::move(grasp_stage), insertion_position); { - auto lift = std::make_unique("lift object", cartesian_solver_); + auto lift = std::make_unique(forward ? "lift object" : "place object", cartesian_solver_); PropertyMap& p = lift->properties(); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); p.property("link").configureInitFrom(Stage::PARENT, "eef_frame"); - p.set("marker_ns", std::string("lift")); + p.set("marker_ns", std::string(forward ? "lift" : "place")); lift_stage_ = lift.get(); - insert(std::move(lift)); + insert(std::move(lift), insertion_position); } } -void Pick::init(const moveit::core::RobotModelConstPtr& robot_model) +void PickPlaceBase::init(const moveit::core::RobotModelConstPtr& robot_model) { // inherit properties from parent PropertyMap* p = &properties(); @@ -57,6 +58,8 @@ void Pick::init(const moveit::core::RobotModelConstPtr& robot_model) // init internal properties const std::string &eef = p->get("eef"); const moveit::core::JointModelGroup *jmg = robot_model->getEndEffector(eef); + if (!jmg) throw InitStageException(*this, "unknown end effector: " + eef); + p->set("eef_group", jmg->getName()); p->set("eef_parent_group", jmg->getEndEffectorParentGroup().first); @@ -64,7 +67,7 @@ void Pick::init(const moveit::core::RobotModelConstPtr& robot_model) SerialContainer::init(robot_model); } -void Pick::setApproachMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) +void PickPlaceBase::setApproachRetract(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { auto& p = approach_stage_->properties(); p.set("twist", motion); @@ -72,7 +75,7 @@ void Pick::setApproachMotion(const geometry_msgs::TwistStamped& motion, double m p.set("max_distance", max_distance); } -void Pick::setLiftMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) +void PickPlaceBase::setLiftPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { auto& p = lift_stage_->properties(); p.set("twist", motion); From 03092e7e45614588a63178488c8555b26aa69d17 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 5 Apr 2018 16:12:48 +0200 Subject: [PATCH 13/16] utility functions to merge multiple RobotTrajectories --- core/include/moveit/task_constructor/merge.h | 59 ++++++++ core/src/CMakeLists.txt | 2 + core/src/merge.cpp | 133 +++++++++++++++++++ 3 files changed, 194 insertions(+) create mode 100644 core/include/moveit/task_constructor/merge.h create mode 100644 core/src/merge.cpp diff --git a/core/include/moveit/task_constructor/merge.h b/core/include/moveit/task_constructor/merge.h new file mode 100644 index 00000000..c8746567 --- /dev/null +++ b/core/include/moveit/task_constructor/merge.h @@ -0,0 +1,59 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Luca Lach, Robert Haschke */ + +#pragma once + +#include +#include +#include + +namespace moveit { namespace task_constructor { + + +/// create a new JointModelGroup comprising all joints of the given groups +moveit::core::JointModelGroup* merge(const std::vector& groups); + +/** merge all sub trajectories into a single RobotTrajectory for parallel execution + * + * As the RobotTrajectory maintains a pointer to the underlying JointModelGroup + * (to know about the involved joint names), a merged JointModelGroup needs to be passed + * or created on the fly. This JMG needs to stay alive during the lifetime of the trajectory. + * For now, only the trajectory path is considered. Timings, velocities, etc. are ignored. + */ +robot_trajectory::RobotTrajectoryPtr merge(const std::vector& sub_trajectories, + core::JointModelGroup*& merged_group); + +} } diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index b5d02dc8..4381bc24 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -4,6 +4,7 @@ add_library(${PROJECT_NAME} ${PROJECT_INCLUDE}/cost_queue.h ${PROJECT_INCLUDE}/introspection.h ${PROJECT_INCLUDE}/marker_tools.h + ${PROJECT_INCLUDE}/merge.h ${PROJECT_INCLUDE}/properties.h ${PROJECT_INCLUDE}/stage.h ${PROJECT_INCLUDE}/stage_p.h @@ -18,6 +19,7 @@ add_library(${PROJECT_NAME} container.cpp introspection.cpp marker_tools.cpp + merge.cpp properties.cpp stage.cpp storage.cpp diff --git a/core/src/merge.cpp b/core/src/merge.cpp new file mode 100644 index 00000000..7a24ec72 --- /dev/null +++ b/core/src/merge.cpp @@ -0,0 +1,133 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Hamburg University + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Luca Lach, Robert Haschke */ + +#include + +namespace moveit { namespace task_constructor { + +moveit::core::JointModelGroup* merge(const std::vector& groups) +{ + if (groups.size() <= 1) + throw std::runtime_error("Expected multiple groups"); + + const moveit::core::RobotModel* const robot_model = &groups[0]->getParentModel(); + + std::set jset; + std::vector names; + for (const moveit::core::JointModelGroup* jmg : groups) { + // sanity check: all groups must share the same robot model + if (&jmg->getParentModel() != robot_model) + throw std::runtime_error("groups refer to different robot models"); + + const auto& joints = jmg->getJointModels(); + jset.insert(joints.cbegin(), joints.cend()); + names.push_back(jmg->getName()); + } + + std::vector joints(jset.cbegin(), jset.cend()); + // attention: do not use getConfig() + return new moveit::core::JointModelGroup("", srdf::Model::Group(), joints, robot_model); +} + +robot_trajectory::RobotTrajectoryPtr merge(const std::vector& sub_trajectories, + moveit::core::JointModelGroup*& merged_group) +{ + if (sub_trajectories.size() <= 1) + throw std::runtime_error("Expected multiple sub solutions"); + + const std::vector *merged_joints + = merged_group ? &merged_group->getJointModels() : nullptr; + std::set jset; + + // sanity checks: all sub solutions must share the same robot model and use disjoint joint sets + const moveit::core::RobotModelConstPtr& robot_model = sub_trajectories[0]->getRobotModel(); + size_t max_num_joints = 0; // maximum number of joints in sub groups + for (const robot_trajectory::RobotTrajectoryPtr& sub : sub_trajectories) { + if (sub->getRobotModel() != robot_model) + throw std::runtime_error("subsolutions refer to multiple robot models"); + + const moveit::core::JointModelGroup* jmg = sub->getGroup(); + const auto& joints = jmg->getJointModels(); + if (merged_joints) { // validate that the joint model is known in merged_group + for (const moveit::core::JointModel* jm : joints) { + if (std::find(merged_joints->cbegin(), merged_joints->cend(), jm) == merged_joints->cend()) + throw std::runtime_error("subsolutions refers to unknown joint: " + jm->getName()); + } + } + max_num_joints = std::max(max_num_joints, joints.size()); + size_t expected_size = jset.size() + joints.size(); + jset.insert(joints.cbegin(), joints.cend()); + if (jset.size() != expected_size) + throw std::runtime_error("joint sets of subsolutions are not disjoint"); + } + + // create merged_group if necessary + if (!merged_group) { + std::vector joints(jset.cbegin(), jset.cend()); + merged_group = new moveit::core::JointModelGroup("", srdf::Model::Group(), joints, robot_model.get()); + } + + // do the actual trajectory merging + auto merged_traj = std::make_shared(robot_model, merged_group); + std::vector values; + values.reserve(max_num_joints); + + while (true) { + bool finished = true; + size_t index = merged_traj->getWayPointCount(); + auto merged_state = index == 0 ? std::make_shared(robot_model) + : std::make_shared(merged_traj->getLastWayPoint()); + for (const robot_trajectory::RobotTrajectoryPtr& sub : sub_trajectories) { + if (index >= sub->getWayPointCount()) + continue; // no more waypoints in this sub solution + + finished = false; // there was a waypoint, continue while loop + const robot_state::RobotState& sub_state = sub->getWayPoint(index); + sub_state.copyJointGroupPositions(sub->getGroup(), values); + merged_state->setJointGroupPositions(sub->getGroup(), values); + } + if (finished) + break; + + // add waypoint without timing + merged_state->update(); + merged_traj->addSuffixWayPoint(merged_state, 0.0); + } + return merged_traj; +} + +} } From 55e3aae30ac694ee447a638d9a4a7bcaa4b4b6a4 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 5 Apr 2018 19:56:36 +0200 Subject: [PATCH 14/16] merge trajectories --- .../moveit/task_constructor/stages/connect.h | 7 ++ core/src/stages/connect.cpp | 86 ++++++++++++++++--- 2 files changed, 80 insertions(+), 13 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index 60192d21..840466ee 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -70,8 +70,15 @@ public: void processSolutions(const SolutionProcessor &processor) const override; void processFailures(const SolutionProcessor &processor) const override { return; } +protected: + SolutionBase* storeSequential(const std::vector& sub_trajectories, + const std::vector& intermediate_scenes); + robot_trajectory::RobotTrajectoryPtr merge(const std::vector& sub_trajectories, + const std::vector& intermediate_scenes); + protected: GroupPlannerVector planner_; + moveit::core::JointModelGroupPtr merged_jmg_; std::list subsolutions_; std::list solutions_; std::list states_; diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 0a12c5fd..c25cdf81 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -37,6 +37,7 @@ */ #include +#include #include namespace moveit { namespace task_constructor { namespace stages { @@ -55,6 +56,7 @@ Connect::Connect(std::string name, const GroupPlannerVector& planners) void Connect::reset() { Connecting::reset(); + merged_jmg_.reset(); solutions_.clear(); subsolutions_.clear(); states_.clear(); @@ -68,13 +70,25 @@ void Connect::init(const core::RobotModelConstPtr& robot_model) if (planner_.empty()) errors.push_back(*this, "empty set of groups"); + size_t num_joints = 0; + std::vector 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 + else { pair.second->init(robot_model); + + auto jmg = robot_model->getJointModelGroup(pair.first); + groups.push_back(jmg); + num_joints += jmg->getJointModels().size(); + } + } + if (!errors && groups.size() >= 2) { + merged_jmg_.reset(task_constructor::merge(groups)); + if (merged_jmg_->getJointModels().size() != num_joints) + errors.push_back(*this, "overlapping joint groups"); } if (errors) @@ -119,46 +133,92 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) { double timeout = props.get("timeout"); const auto& path_constraints = props.get("path_constraints"); - SolutionSequence::container_type subsolutions; + std::vector sub_trajectories; planning_scene::PlanningScenePtr start = from.scene()->diff(); const moveit::core::RobotState& goal_state = to.scene()->getCurrentState(); + std::vector intermediate_scenes; + intermediate_scenes.push_back(start); + + std::vector positions; for (const GroupPlannerVector::value_type& pair : planner_) { // set intermediate goal state planning_scene::PlanningScenePtr end = start->diff(); + intermediate_scenes.push_back(end); 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); + if (!pair.second->plan(start, to.scene(), jmg, timeout, trajectory, path_constraints)) + break; - // store solution - auto inserted = subsolutions_.insert(subsolutions_.end(), SubTrajectory(trajectory)); + sub_trajectories.push_back(trajectory); + // continue from reached state + start = end; + } + + SolutionBase* solution = nullptr; + if (sub_trajectories.size() != planner_.size()) { // error during sequential planning + if (!storeFailures()) + return false; + sub_trajectories.push_back(robot_trajectory::RobotTrajectoryPtr()); + solution = storeSequential(sub_trajectories, intermediate_scenes); + // mark solution as failure + solution->setCost(std::numeric_limits::infinity()); + } else { + robot_trajectory::RobotTrajectoryPtr t = merge(sub_trajectories, intermediate_scenes); + if (t) { + connect(from, to, SubTrajectory(t)); + return true; + } + // merging failed, store sequentially + solution = storeSequential(sub_trajectories, intermediate_scenes); + } + + newSolution(from, to, *solution); + return !solution->isFailure(); +} + +SolutionBase* Connect::storeSequential(const std::vector& sub_trajectories, + const std::vector& intermediate_scenes) +{ + assert(sub_trajectories.size() + 1 == intermediate_scenes.size()); + auto scene_it = intermediate_scenes.begin(); + planning_scene::PlanningScenePtr start = *scene_it; + + SolutionSequence::container_type sub_solutions; + for (const auto &sub : sub_trajectories) { + planning_scene::PlanningScenePtr end = *++scene_it; + + auto inserted = subsolutions_.insert(subsolutions_.end(), SubTrajectory(sub)); inserted->setCreator(pimpl_); // push back solution pointer - subsolutions.push_back(&*inserted); + sub_solutions.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; + solutions_.emplace_back(SolutionSequence(std::move(sub_solutions))); + return &solutions_.back(); +} + +robot_trajectory::RobotTrajectoryPtr Connect::merge(const std::vector& sub_trajectories, + const std::vector& intermediate_scenes) +{ + auto jmg = merged_jmg_.get(); + robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, jmg); + return trajectory; } void Connect::processSolutions(const Stage::SolutionProcessor& processor) const { - } } } } From 047a3f7db4ceac666e877a85a6fb71e7a5bb69d3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 8 Apr 2018 02:24:45 +0200 Subject: [PATCH 15/16] report duplicate joints, accept fixed joints as duplicates --- core/include/moveit/task_constructor/merge.h | 10 ++++ core/src/merge.cpp | 52 ++++++++++++++++++-- core/src/stages/connect.cpp | 14 ++++-- 3 files changed, 68 insertions(+), 8 deletions(-) diff --git a/core/include/moveit/task_constructor/merge.h b/core/include/moveit/task_constructor/merge.h index c8746567..1d6a97d1 100644 --- a/core/include/moveit/task_constructor/merge.h +++ b/core/include/moveit/task_constructor/merge.h @@ -46,6 +46,16 @@ namespace moveit { namespace task_constructor { /// create a new JointModelGroup comprising all joints of the given groups moveit::core::JointModelGroup* merge(const std::vector& groups); +/** find duplicate, non-fixed joints + * + * Merging is only allowed for disjoint joint sets. Fixed joints are tolerated. + * Assumes that \e joints is the the union of the joint sets of all \e groups (w/o duplicates). + * The list of duplicate joints is returned in \e duplicates and in \e names (as a comma-separated list) */ +bool findDuplicates(const std::vector& groups, + std::vector joints, + std::vector& duplicates, + std::string& names); + /** merge all sub trajectories into a single RobotTrajectory for parallel execution * * As the RobotTrajectory maintains a pointer to the underlying JointModelGroup diff --git a/core/src/merge.cpp b/core/src/merge.cpp index 7a24ec72..25ea3dee 100644 --- a/core/src/merge.cpp +++ b/core/src/merge.cpp @@ -63,6 +63,31 @@ moveit::core::JointModelGroup* merge(const std::vector& groups, + std::vector joints, + std::vector& duplicates, + std::string& names) +{ + duplicates.clear(); + names.clear(); + for (const moveit::core::JointModelGroup* jmg : groups) { + for (const moveit::core::JointModel* jm : jmg->getJointModels()) { + auto it = std::find(joints.begin(), joints.end(), jm); + if (it == joints.end()) { // jm not found anymore -> duplicate + if (jm->getType() != moveit::core::JointModel::FIXED && // fixed joints are OK + std::find(duplicates.begin(), duplicates.end(), jm) == duplicates.end()) { + duplicates.push_back(jm); + if (!names.empty()) names.append(", "); + names.append(jm->getName()); + } + continue; + } else + joints.erase(it); // remove from list as processed + } + } + return duplicates.size() > 0; +} + robot_trajectory::RobotTrajectoryPtr merge(const std::vector& sub_trajectories, moveit::core::JointModelGroup*& merged_group) { @@ -72,27 +97,44 @@ robot_trajectory::RobotTrajectoryPtr merge(const std::vector *merged_joints = merged_group ? &merged_group->getJointModels() : nullptr; std::set jset; + std::vector groups; + groups.reserve(sub_trajectories.size()); // sanity checks: all sub solutions must share the same robot model and use disjoint joint sets const moveit::core::RobotModelConstPtr& robot_model = sub_trajectories[0]->getRobotModel(); size_t max_num_joints = 0; // maximum number of joints in sub groups + size_t num_joints = 0; // sum of joints in all sub groups + for (const robot_trajectory::RobotTrajectoryPtr& sub : sub_trajectories) { if (sub->getRobotModel() != robot_model) throw std::runtime_error("subsolutions refer to multiple robot models"); const moveit::core::JointModelGroup* jmg = sub->getGroup(); + groups.push_back(jmg); const auto& joints = jmg->getJointModels(); if (merged_joints) { // validate that the joint model is known in merged_group for (const moveit::core::JointModel* jm : joints) { if (std::find(merged_joints->cbegin(), merged_joints->cend(), jm) == merged_joints->cend()) throw std::runtime_error("subsolutions refers to unknown joint: " + jm->getName()); } - } + } else // accumulate set of joints + jset.insert(joints.cbegin(), joints.cend()); + max_num_joints = std::max(max_num_joints, joints.size()); - size_t expected_size = jset.size() + joints.size(); - jset.insert(joints.cbegin(), joints.cend()); - if (jset.size() != expected_size) - throw std::runtime_error("joint sets of subsolutions are not disjoint"); + num_joints += joints.size(); + } + + size_t num_merged = merged_joints ? merged_joints->size() : jset.size(); + if (num_merged != num_joints) { + // overlapping joint groups: analyse in more detail + std::vector joints; + if (merged_joints) joints = *merged_joints; + else joints.insert(joints.end(), jset.cbegin(), jset.cend()); + + std::vector duplicates; + std::string names; + if (findDuplicates(groups, joints, duplicates, names)) + throw std::runtime_error("overlapping joint groups: " + names); } // create merged_group if necessary diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index c25cdf81..eaa78e94 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -85,10 +85,18 @@ void Connect::init(const core::RobotModelConstPtr& robot_model) num_joints += jmg->getJointModels().size(); } } - if (!errors && groups.size() >= 2) { + + if (!errors && groups.size() >= 2) { // enable merging? merged_jmg_.reset(task_constructor::merge(groups)); - if (merged_jmg_->getJointModels().size() != num_joints) - errors.push_back(*this, "overlapping joint groups"); + if (merged_jmg_->getJointModels().size() != num_joints) { + // overlapping joint groups: analyse in more detail + std::vector duplicates; + std::string names; + if (findDuplicates(groups, merged_jmg_->getJointModels(), duplicates, names)) { + ROS_INFO_STREAM_NAMED("Connect", this->name() << ": overlapping joint groups: " << names << ". Disabling merging."); + merged_jmg_.reset(); // fallback to serial connect + } + } } if (errors) From 21242ea9766d7e636f8f0e7e7697c7482f244e3e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 5 Apr 2018 22:46:09 +0200 Subject: [PATCH 16/16] publish failures --- .../moveit/task_constructor/stages/connect.h | 6 ++--- core/src/stages/connect.cpp | 23 +++++++++++++++++++ 2 files changed, 26 insertions(+), 3 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index 840466ee..8254598c 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -65,10 +65,9 @@ public: 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; } + size_t numSolutions() const override; void processSolutions(const SolutionProcessor &processor) const override; - void processFailures(const SolutionProcessor &processor) const override { return; } + void processFailures(const SolutionProcessor &processor) const override; protected: SolutionBase* storeSequential(const std::vector& sub_trajectories, @@ -79,6 +78,7 @@ protected: protected: GroupPlannerVector planner_; moveit::core::JointModelGroupPtr merged_jmg_; + // TODO: ComputeBase should handle any SolutionBase -> shared_ptr std::list subsolutions_; std::list solutions_; std::list states_; diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index eaa78e94..ad18b3dd 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -225,8 +225,31 @@ robot_trajectory::RobotTrajectoryPtr Connect::merge(const std::vector