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/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/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/merge.h b/core/include/moveit/task_constructor/merge.h new file mode 100644 index 00000000..1d6a97d1 --- /dev/null +++ b/core/include/moveit/task_constructor/merge.h @@ -0,0 +1,69 @@ +/********************************************************************* + * 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); + +/** 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 + * (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/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index a627d8fb..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); @@ -346,13 +349,13 @@ 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)); } + + void newSolution(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution); }; } } diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index ecc5eb43..9ef2d6ae 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 @@ -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 566d3489..8254598c 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 */ @@ -46,8 +46,12 @@ 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: - 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 +61,27 @@ 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; + void processSolutions(const SolutionProcessor &processor) const override; + void processFailures(const SolutionProcessor &processor) const override; protected: - solvers::PlannerInterfacePtr planner_; + 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_; + // TODO: ComputeBase should handle any SolutionBase -> shared_ptr + std::list subsolutions_; + std::list solutions_; + std::list states_; }; } } } 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/include/moveit/task_constructor/stages/generate_pose.h b/core/include/moveit/task_constructor/stages/generate_pose.h new file mode 100644 index 00000000..0817e494 --- /dev/null +++ b/core/include/moveit/task_constructor/stages/generate_pose.h @@ -0,0 +1,64 @@ +/********************************************************************* + * 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); + + void reset() override; + 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/include/moveit/task_constructor/stages/pick.h b/core/include/moveit/task_constructor/stages/pick.h index 0b828304..10c2c8db 100644 --- a/core/include/moveit/task_constructor/stages/pick.h +++ b/core/include/moveit/task_constructor/stages/pick.h @@ -44,32 +44,34 @@ namespace moveit { namespace task_constructor { namespace solvers { MOVEIT_CLASS_FORWARD(CartesianPath) -MOVEIT_CLASS_FORWARD(PipelinePlanner) } 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_; - solvers::PipelinePlannerPtr pipeline_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; @@ -81,12 +83,50 @@ 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); - 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/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/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/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/container.cpp b/core/src/container.cpp index aa2ff79c..f8cc800f 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 @@ -878,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/merge.cpp b/core/src/merge.cpp new file mode 100644 index 00000000..25ea3dee --- /dev/null +++ b/core/src/merge.cpp @@ -0,0 +1,175 @@ +/********************************************************************* + * 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); +} + +bool findDuplicates(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) +{ + 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; + 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()); + 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 + 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; +} + +} } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 67260434..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) @@ -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()) { @@ -309,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(); } @@ -572,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; } } @@ -581,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())) { @@ -602,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)); } } } @@ -653,13 +648,38 @@ void Connecting::reset() ComputeBase::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); +/// 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))); +} + +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/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 03959900..7ffdc1fe 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -4,6 +4,7 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/current_state.h ${PROJECT_INCLUDE}/stages/fixed_state.h + ${PROJECT_INCLUDE}/stages/generate_pose.h ${PROJECT_INCLUDE}/stages/generate_grasp_pose.h ${PROJECT_INCLUDE}/stages/compute_ik.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/connect.cpp b/core/src/stages/connect.cpp index 8e1c83ea..ad18b3dd 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -37,13 +37,14 @@ */ #include +#include #include 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 +53,203 @@ Connect::Connect(std::string name, const solvers::PlannerInterfacePtr& planner) "constraints to maintain during trajectory"); } +void Connect::reset() +{ + Connecting::reset(); + merged_jmg_.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"); + + 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 { + 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) { // enable merging? + merged_jmg_.reset(task_constructor::merge(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) + 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(); - 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; + std::vector sub_trajectories; + planning_scene::PlanningScenePtr start = from.scene()->diff(); + const moveit::core::RobotState& goal_state = to.scene()->getCurrentState(); - connect(from, to, trajectory); - return true; + 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); + goal_state.copyJointGroupPositions(jmg, positions); + end->getCurrentStateNonConst().setJointGroupPositions(jmg, positions); + + robot_trajectory::RobotTrajectoryPtr trajectory; + if (!pair.second->plan(start, to.scene(), jmg, timeout, trajectory, path_constraints)) + break; + + 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 + 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()); + + start = end; + } + + 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; +} + +size_t Connect::numSolutions() const +{ + return solutions_.size() + Connecting::numSolutions(); +} + +void Connect::processSolutions(const Stage::SolutionProcessor& processor) const +{ + // TODO: This is not nice, but necessary to process simple SubTrajectory + SolutionSequence + for (const auto& s : solutions_) { + if (s.isFailure()) continue; + if (!processor(s)) + break; + } + Connecting::processSolutions(processor); +} + +void Connect::processFailures(const Stage::SolutionProcessor &processor) const +{ + // TODO: This is not nice, but necessary to process simple SubTrajectory + SolutionSequence + for (const auto& s : solutions_) { + if (!s.isFailure()) continue; + if (!processor(s)) + break; + } + Connecting::processFailures(processor); } } } } diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index 20526f57..9609c0d1 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,73 @@ 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(); - const std::string& joint_pose = props.get("pregrasp"); - if(!joint_pose.empty()){ - robot_state.setToDefaultValues(jmg , joint_pose); - } + 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)) - throw std::runtime_error("requested object does not exist or could not be retrieved"); + if (!scene->knowsFrameTransform(object_name)) { + ROS_WARN_STREAM_NAMED("GenerateGraspPose", "unknown object: " << object_name); + return; + } + + 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(); geometry_msgs::PoseStamped target_pose_msg; - target_pose_msg.header.frame_id = object_name; + target_pose_msg.header.frame_id = props.get("object"); - 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 +150,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; } } } } diff --git a/core/src/stages/generate_pose.cpp b/core/src/stages/generate_pose.cpp new file mode 100644 index 00000000..a73f923c --- /dev/null +++ b/core/src/stages/generate_pose.cpp @@ -0,0 +1,93 @@ +/********************************************************************* + * 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 + +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::reset() +{ + MonitoringGenerator::reset(); + scenes_.clear(); +} + +void GeneratePose::onNewSolution(const SolutionBase& s) +{ + scenes_.push_back(s.end()->scene()->diff()); +} + +bool GeneratePose::canCompute() const { + return scenes_.size() > 0; +} + +bool GeneratePose::compute() { + if (scenes_.empty()) + return false; + planning_scene::PlanningSceneConstPtr scene = scenes_[0]; + scenes_.pop_front(); + + 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); + 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; +} + +} } } diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index 122c35d8..b098749f 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -1,19 +1,15 @@ #include #include -#include #include -#include #include -#include -#include #include 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(); @@ -26,51 +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(); - pipeline_solver_ = std::make_shared(); - pipeline_solver_->setTimeout(8.0); - pipeline_solver_->setPlannerId("RRTConnectkConfigDefault"); + int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order { - 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_); + 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(); @@ -79,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); @@ -86,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); @@ -94,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); 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); 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); +} + + } }