diff --git a/capabilities/CMakeLists.txt b/capabilities/CMakeLists.txt index 68770339f..168edd926 100644 --- a/capabilities/CMakeLists.txt +++ b/capabilities/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS moveit_core moveit_ros_move_group moveit_task_constructor_msgs + moveit_task_constructor_core pluginlib std_msgs ) @@ -17,6 +18,7 @@ catkin_package( CATKIN_DEPENDS actionlib moveit_task_constructor_msgs + moveit_task_constructor_core std_msgs ) @@ -36,6 +38,7 @@ include_directories( add_library(${PROJECT_NAME} src/execute_task_solution_capability.cpp + src/plan_pick_place_capability.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/capabilities/capabilities_plugin_description.xml b/capabilities/capabilities_plugin_description.xml index eba669c8f..a0ecf7adf 100644 --- a/capabilities/capabilities_plugin_description.xml +++ b/capabilities/capabilities_plugin_description.xml @@ -4,4 +4,10 @@ Action server to execute solutions generated through the MoveIt Task Constructor. + + + + Action server to plan full pick and place motions using the MoveIt Task Constructor. + + diff --git a/capabilities/package.xml b/capabilities/package.xml index 11eaa3065..27b5cfb5a 100644 --- a/capabilities/package.xml +++ b/capabilities/package.xml @@ -18,6 +18,7 @@ pluginlib std_msgs moveit_task_constructor_msgs + moveit_task_constructor_core diff --git a/capabilities/src/plan_pick_place_capability.cpp b/capabilities/src/plan_pick_place_capability.cpp new file mode 100644 index 000000000..850bbd46d --- /dev/null +++ b/capabilities/src/plan_pick_place_capability.cpp @@ -0,0 +1,98 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, Kentaro Wada. + * 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 Willow Garage 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. + *********************************************************************/ + +/* Author: Henning Kayser, Artur Karoly*/ + +#include "plan_pick_place_capability.h" + +#include +#include + + +namespace move_group { + +PlanPickPlaceCapability::PlanPickPlaceCapability() : MoveGroupCapability("PlanPickPlace") {} + +void PlanPickPlaceCapability::initialize() { + // Configure the action server + as_.reset(new actionlib::SimpleActionServer( + root_node_handle_, "plan_pick_place", + std::bind(&PlanPickPlaceCapability::goalCallback, this, std::placeholders::_1), false)); + as_->registerPreemptCallback(std::bind(&PlanPickPlaceCapability::preemptCallback, this)); + as_->start(); + pick_place_task_ = std::make_unique("pick_place_task"); +} + +void PlanPickPlaceCapability::goalCallback( + const moveit_task_constructor_msgs::PlanPickPlaceGoalConstPtr& goal) { + moveit_task_constructor_msgs::PlanPickPlaceResult result; + + // Fill parameters + PickPlaceTask::Parameters parameters; + parameters.task_type_ = goal->task_type; + parameters.arm_group_name_ = goal->arm_group_name; + parameters.hand_group_name_ = goal->hand_group_name; + parameters.eef_name_ = goal->eef_name; + parameters.hand_frame_ = goal->hand_frame; + parameters.object_name_ = goal->object_id; + parameters.support_surfaces_ = goal->support_surfaces; + parameters.grasps_ = goal->grasps; + parameters.grasp_provider_plugin_name_ = goal->grasp_provider_plugin_name; + tf::poseMsgToEigen(goal->grasp_frame_transform, parameters.grasp_frame_transform_); + + parameters.place_provider_plugin_name_ = goal->place_provider_plugin_name; + parameters.place_locations_ = goal->place_locations; + + // Initialize task and plan + if (pick_place_task_->init(parameters)){ + // Compute plan + result.success = pick_place_task_->plan(); + if (result.success) { + pick_place_task_->getSolutionMsg(result.solution); + } + } else { + result.success = false; + } + // Retrieve and return result + as_->setSucceeded(result); +} + +void PlanPickPlaceCapability::preemptCallback() { + pick_place_task_->preempt(); +} + +} // namespace move_group + +#include +CLASS_LOADER_REGISTER_CLASS(move_group::PlanPickPlaceCapability, move_group::MoveGroupCapability) diff --git a/capabilities/src/plan_pick_place_capability.h b/capabilities/src/plan_pick_place_capability.h new file mode 100644 index 000000000..6df190b1d --- /dev/null +++ b/capabilities/src/plan_pick_place_capability.h @@ -0,0 +1,71 @@ +/********************************************************************* + * 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 Hamburg 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. + *********************************************************************/ + +/* + * Capability to plan pick and place motions using the MoveIt Task Constructor. + * + * Author: Henning Kayser + * */ + +#pragma once + +#include +#include + +#include +#include + +#include + +namespace move_group { + +using moveit::task_constructor::tasks::PickPlaceTask; + +class PlanPickPlaceCapability : public MoveGroupCapability +{ +public: + PlanPickPlaceCapability(); + + virtual void initialize(); + +private: + void goalCallback(const moveit_task_constructor_msgs::PlanPickPlaceGoalConstPtr& goal); + void preemptCallback(); + + std::unique_ptr> as_; + + std::unique_ptr pick_place_task_; +}; + +} // namespace move_group diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 7032b57dc..a789dc917 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(catkin REQUIRED COMPONENTS moveit_ros_planning_interface moveit_task_constructor_msgs roscpp + rosparam_shortcuts visualization_msgs rviz_marker_tools ) @@ -18,6 +19,7 @@ catkin_package( LIBRARIES ${PROJECT_NAME} ${PROJECT_NAME}_stages + ${PROJECT_NAME}_tasks INCLUDE_DIRS include CATKIN_DEPENDS @@ -43,7 +45,7 @@ add_compile_options(-fvisibility-inlines-hidden) set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor) add_subdirectory(src) -add_subdirectory(test) +# add_subdirectory(test) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} PATTERN "*_p.h" EXCLUDE) diff --git a/core/include/moveit/task_constructor/stages/grasp_provider.h b/core/include/moveit/task_constructor/stages/grasp_provider.h new file mode 100644 index 000000000..ed13165f3 --- /dev/null +++ b/core/include/moveit/task_constructor/stages/grasp_provider.h @@ -0,0 +1,74 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, 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, Artur Karoly + Desc: Grasp provider plugins and default plugin +*/ + +#pragma once + +#include +#include "grasp_provider_base.h" + +namespace moveit { +namespace task_constructor { +namespace stages { + +/// Default Grasp Provider plugin implementing the functionality of the GenerateGraspPose stage + +class GraspProviderDefault : public GraspProviderBase +{ +public: + GraspProviderDefault(const std::string& name = "generate grasp pose"); + + void init(const core::RobotModelConstPtr& robot_model) override; + void compute() override; +}; + + +/// Grasp Provider plugin for setting a set of grasp poses + +class GraspProviderFixedPoses : public GraspProviderBase +{ +public: + GraspProviderFixedPoses(const std::string& name = "set grasp poses"); + + void init(const core::RobotModelConstPtr& robot_model) override; + void compute() override; +}; + + +} // namespace stages +} // namespace task_constructor +} // namespace moveit diff --git a/core/include/moveit/task_constructor/stages/grasp_provider_base.h b/core/include/moveit/task_constructor/stages/grasp_provider_base.h new file mode 100644 index 000000000..dfc831a3f --- /dev/null +++ b/core/include/moveit/task_constructor/stages/grasp_provider_base.h @@ -0,0 +1,73 @@ +/********************************************************************* + * 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: Artur Karoly, Jafar Abdi */ + +#ifndef MOVEIT_TASK_CONSTRUCTOR_CORE_GRASP_PROVIDER_BASE_H +#define MOVEIT_TASK_CONSTRUCTOR_CORE_GRASP_PROVIDER_BASE_H + +#include "memory" +#include "moveit/task_constructor/container.h" +#include "grasp_provider_base.h" + +namespace moveit { +namespace task_constructor { +namespace stages { +class GraspProviderBase : public GeneratePose +{ +public: + GraspProviderBase(const std::string& name = "grasp provider"); + + void init(const std::shared_ptr& robot_model) override; + + void setEndEffector(const std::string& eef) { setProperty("eef", eef); } + void setObject(const std::string& object) { setProperty("object", object); } + + void setPreGraspPose(const std::string& pregrasp) { properties().set("pregrasp", pregrasp); } + void setPreGraspPose(const ::moveit_msgs::RobotState_>& pregrasp) { + properties().set("pregrasp", pregrasp); + } + void setGraspPose(const std::string& grasp) { properties().set("grasp", grasp); } + void setGraspPose(const ::moveit_msgs::RobotState_>& grasp) { + properties().set("grasp", grasp); + } + +protected: + void onNewSolution(const SolutionBase& s) override; +}; +} // namespace stages +} // namespace task_constructor +} // namespace moveit +#include +#endif // MOVEIT_TASK_CONSTRUCTOR_CORE_GRASP_PROVIDER_BASE_H diff --git a/core/include/moveit/task_constructor/stages/pick.h b/core/include/moveit/task_constructor/stages/pick.h deleted file mode 100644 index 34d9f6ec1..000000000 --- a/core/include/moveit/task_constructor/stages/pick.h +++ /dev/null @@ -1,126 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018, 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: Robert Haschke */ - -#pragma once - -#include -#include -#include - -namespace moveit { -namespace task_constructor { - -namespace solvers { -MOVEIT_CLASS_FORWARD(CartesianPath) -} - -namespace stages { - -/** PickPlaceBase wraps the pipeline to pick or place an object with a given end effector. - * - * Picking consist of the following sub stages: - * - 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 PickPlaceBase : public SerialContainer -{ - solvers::CartesianPathPtr cartesian_solver_; - Stage* grasp_stage_ = nullptr; - Stage* approach_stage_ = nullptr; - Stage* lift_stage_ = nullptr; - -public: - PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& name, bool forward); - - void init(const moveit::core::RobotModelConstPtr& robot_model) override; - - void setEndEffector(const std::string& eef) { properties().set("eef", eef); } - void setObject(const std::string& object) { properties().set("object", object); } - - solvers::CartesianPathPtr cartesianSolver() { return cartesian_solver_; } - - 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); - void setLiftPlace(const std::map& joints); -}; - -/// specialization of PickPlaceBase to realize picking -class Pick : public PickPlaceBase -{ -public: - Pick(Stage::pointer&& grasp_stage = Stage::pointer(), 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); - } - void setLiftMotion(const std::map& joints) { setLiftPlace(joints); } -}; - -/// specialization of PickPlaceBase to realize placing -class Place : public PickPlaceBase -{ -public: - Place(Stage::pointer&& ungrasp_stage = Stage::pointer(), 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); - } - void setPlaceMotion(const std::map& joints) { setLiftPlace(joints); } -}; -} // namespace stages -} // namespace task_constructor -} // namespace moveit diff --git a/core/include/moveit/task_constructor/stages/pick_place.h b/core/include/moveit/task_constructor/stages/pick_place.h new file mode 100644 index 000000000..b192c2bb9 --- /dev/null +++ b/core/include/moveit/task_constructor/stages/pick_place.h @@ -0,0 +1,177 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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: Robert Haschke, Artur Karoly */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "grasp_provider_base.h" +#include "place_provider_base.h" + +namespace moveit { +namespace task_constructor { + +namespace solvers { +MOVEIT_CLASS_FORWARD(CartesianPath) +MOVEIT_CLASS_FORWARD(PipelinePlanner) +} + +namespace stages { + +/** PickPlaceBase wraps the pipeline to pick or place an object with a given end effector. + * + * Picking consist of the following sub stages: + * - linearly approaching the object along an approach direction/twist + * - GraspProviderPlugin stage wrapped in an IK computation + * - close hand + * - attach object + * - lift along along a given direction/twist + * + * Placing consist of the inverse order of stages: + * - place down along a given direction + * - PlaceProviderPlugin stage wrapped in an IK computation + * - open hand + * - detach the object + * - linearly retract end effector + */ +template +class PickPlaceBase : public SerialContainer +{ + + bool is_pick_; + + solvers::CartesianPathPtr cartesian_solver_; + solvers::PipelinePlannerPtr sampling_planner_; + + MoveRelative* move_there_stage_ = nullptr; + ComputeIK* compute_ik_stage_ = nullptr; + ModifyPlanningScene* set_collision_object_hand_stage_ = nullptr; + ModifyPlanningScene* allow_collision_object_support_stage_ = nullptr; + ModifyPlanningScene* forbid_collision_object_support_stage_ = nullptr; + MoveRelative* move_back_stage_ = nullptr; + + std::string provider_stage_plugin_name_; + +protected: + C* provider_plugin_stage_ = nullptr; + ModifyPlanningScene* attach_detach_stage_ = nullptr; + +public: + PickPlaceBase(const std::string& name, const std::string& provider_stage_plugin_name, bool is_pick, pluginlib::ClassLoader* class_loader); + + void init(const moveit::core::RobotModelConstPtr& robot_model) override; + + /// ------------------------- + /// setters of own properties + + /// eef + void setEndEffector(const std::string& eef) { properties().set("eef", eef); } + void setEndEffectorOpenClose(const std::string& open_pose_name, const std::string& close_pose_name){ + properties().set("eef_group_open_pose", open_pose_name); + properties().set("eef_group_close_pose", close_pose_name); + } + + /// object + void setObject(const std::string& object) {properties().set("object", object);} + + /// support surfaces + void setSupportSurfaces(const std::vector& surfaces) {properties().set>("support_surfaces", surfaces);} + + /// IK frame + void setIKFrame(const geometry_msgs::PoseStamped& pose) { setProperty("ik_frame", pose); } + void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link); + template + void setIKFrame(const T& p, const std::string& link) { + Eigen::Isometry3d pose; + pose = p; + setIKFrame(pose, link); + } + void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); } + + /// ------------------------- + /// setters of substage properties + + /// IK computation + void setMaxIKSolutions(const uint32_t& max_ik_solutions); + void setMinIKSolutionDistance(const double& min_ik_solution_distance); + void setIgnoreIKCollisions(const bool& ignore_ik_collisions); + + /// ------------------------- + /// getters, for further configuration + + solvers::CartesianPathPtr cartesianSolver() { return cartesian_solver_; } + solvers::PipelinePlannerPtr samplingPlanner() {return sampling_planner_;} + + // Use this to retrieve a pointer to the GraspProviderPlugin object, to set its custom properties + C* ProviderPlugin() {return provider_plugin_stage_;} +}; + +/// specialization of PickPlaceBase to realize picking +class Pick : public PickPlaceBase +{ +public: + Pick(const std::string& name = "pick", const std::string& provider_stage_plugin_name = "moveit_task_constructor/GraspProviderDefault", pluginlib::ClassLoader* class_loader = nullptr) + : PickPlaceBase(name, provider_stage_plugin_name, true, class_loader) {} + + void setMonitoredStage(Stage* monitored); + + moveit::task_constructor::Stage* attachStage() {return attach_detach_stage_;} +}; + +/// specialization of PickPlaceBase to realize placing +class Place : public PickPlaceBase +{ +public: + Place(const std::string& name = "place", const std::string& provider_stage_plugin_name = "moveit_task_constructor/PlaceProviderDefault", pluginlib::ClassLoader* class_loader = nullptr) + : PickPlaceBase(name, provider_stage_plugin_name, false, class_loader) {} + + void setMonitoredStage(Stage* monitored); + + moveit::task_constructor::Stage* detachStage() {return attach_detach_stage_;} +}; +} // namespace stages +} // namespace task_constructor +} // namespace moveit diff --git a/core/include/moveit/task_constructor/stages/place_provider.h b/core/include/moveit/task_constructor/stages/place_provider.h new file mode 100644 index 000000000..bfc6e1f8e --- /dev/null +++ b/core/include/moveit/task_constructor/stages/place_provider.h @@ -0,0 +1,70 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, 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: Robert Haschke, Artur Karoly + Desc: Place provider plaugins and default plugin +*/ + +#pragma once + +#include +#include "place_provider_base.h" + +namespace moveit { +namespace task_constructor { +namespace stages { + +/// Default Place Provider plugin implementing the functionality of the GeneratePlacePose stage + +class PlaceProviderDefault : public PlaceProviderBase +{ +public: + PlaceProviderDefault(const std::string& name = "place pose") : PlaceProviderBase(name){} + + void compute() override; +}; + +/// Place provider plugin for multiple place poses + +class PlaceProviderFixedPoses : public PlaceProviderBase +{ +public: + PlaceProviderFixedPoses(const std::string& name = "set place poses") : PlaceProviderBase(name){} + + void compute() override; +}; + +} // namespace stages +} // namespace task_constructor +} // namespace moveit diff --git a/core/include/moveit/task_constructor/stages/place_provider_base.h b/core/include/moveit/task_constructor/stages/place_provider_base.h new file mode 100644 index 000000000..4bf6d2f13 --- /dev/null +++ b/core/include/moveit/task_constructor/stages/place_provider_base.h @@ -0,0 +1,54 @@ +/********************************************************************* + * 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: Artur Karoly, Jafar Abdi */ + +#ifndef MOVEIT_TASK_CONSTRUCTOR_CORE_PLACE_PROVIDER_BASE_H +#define MOVEIT_TASK_CONSTRUCTOR_CORE_PLACE_PROVIDER_BASE_H +namespace moveit { +namespace task_constructor { +namespace stages { +class PlaceProviderBase : public GeneratePose +{ +public: + PlaceProviderBase(const std::string& name = "place pose"); + + void setObject(const std::string& object) { setProperty("object", object); } + +protected: + void onNewSolution(const SolutionBase& s) override; +}; +}}} +#include +#endif // MOVEIT_TASK_CONSTRUCTOR_CORE_PLACE_PROVIDER_BASE_H diff --git a/core/include/moveit/task_constructor/tasks/pick_place_task.h b/core/include/moveit/task_constructor/tasks/pick_place_task.h new file mode 100644 index 000000000..7eefe7f75 --- /dev/null +++ b/core/include/moveit/task_constructor/tasks/pick_place_task.h @@ -0,0 +1,136 @@ +/********************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019 PickNik LLC. + * 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 the copyright holder 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 HOLDER 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. + *********************************************************************/ + +/* Author: Henning Kayser, Simon Goldstein, Artur Karoly + Desc: MoveIt Task Constructor Task for the PickPlace Capability +*/ + +// ROS +#include + +// MoveIt +#include +#include +#include + +// MTC +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include + +#pragma once + +namespace moveit { +namespace task_constructor { +namespace tasks { + +using GraspProviderPluginLoader = pluginlib::ClassLoader; +using PlaceProviderPluginLoader = pluginlib::ClassLoader; +using namespace moveit::task_constructor; + +class PickPlaceTask +{ +public: + struct Parameters + { + short task_type_; + + // planning group properties + std::string arm_group_name_; + std::string eef_name_; + std::string hand_group_name_; + std::string hand_frame_; + + // object + surface + std::vector support_surfaces_; + std::string object_name_; + + // Plugins + std::string grasp_provider_plugin_name_; + std::string place_provider_plugin_name_; + + // Pick metrics + std::vector grasps_; + Eigen::Isometry3d grasp_frame_transform_; + + // Place metrics + std::vector place_locations_; + }; + + PickPlaceTask(const std::string& task_name); + ~PickPlaceTask() = default; + + bool init(const Parameters& parameters); + + bool plan(); + bool preempt(); + void getSolutionMsg(moveit_task_constructor_msgs::Solution& solution); + +private: + TaskPtr task_; + const std::string task_name_; + Stage* current_state_stage_; + Stage* attach_object_stage_; + + std::unique_ptr grasp_provider_class_loader_; + std::unique_ptr place_provider_class_loader_; +}; + +} // namespace tasks +} // namespace task_constructor +} // namespace moveit diff --git a/core/motion_planning_stages_plugin_description.xml b/core/motion_planning_stages_plugin_description.xml index 75af7442d..3919688c9 100644 --- a/core/motion_planning_stages_plugin_description.xml +++ b/core/motion_planning_stages_plugin_description.xml @@ -7,3 +7,37 @@ + + + + + Default Grasp provider stage plugin implementing the functionality of the GenerateGraspPose stage + + + + + + Grasp Provider plugin for setting a set of grasp poses + + + + + + Default Place provider stage plugin implementing the functionality of the GeneratePlacePose stage + + + + + + Place Provider plugin for setting a set of place poses + + + diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index 263c50332..13bb1994f 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -42,6 +42,7 @@ target_include_directories(${PROJECT_NAME} add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) add_subdirectory(stages) +add_subdirectory(tasks) install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 857a4977c..94a492c2c 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -17,7 +17,7 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/move_relative.h ${PROJECT_INCLUDE}/stages/simple_grasp.h - ${PROJECT_INCLUDE}/stages/pick.h + ${PROJECT_INCLUDE}/stages/pick_place.h modify_planning_scene.cpp fix_collision_objects.cpp @@ -25,6 +25,10 @@ add_library(${PROJECT_NAME}_stages current_state.cpp fixed_state.cpp fixed_cartesian_poses.cpp + grasp_provider_base.cpp + ${PROJECT_INCLUDE}/stages/grasp_provider_base.h + place_provider_base.cpp + ${PROJECT_INCLUDE}/stages/place_provider_base.h generate_pose.cpp generate_grasp_pose.cpp generate_place_pose.cpp @@ -37,10 +41,24 @@ add_library(${PROJECT_NAME}_stages move_relative.cpp simple_grasp.cpp - pick.cpp + pick_place.cpp ) target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES}) install(TARGETS ${PROJECT_NAME}_stages ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + +add_library(${PROJECT_NAME}_providers + ${PROJECT_INCLUDE}/stages/grasp_provider.h + ${PROJECT_INCLUDE}/stages/place_provider.h + ${PROJECT_INCLUDE}/stages/place_provider_base.h + ${PROJECT_INCLUDE}/stages/grasp_provider_base.h + grasp_provider.cpp + place_provider.cpp +) +target_link_libraries(${PROJECT_NAME}_providers ${PROJECT_NAME} ${PROJECT_NAME}_stages ${catkin_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME}_providers + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) diff --git a/core/src/stages/grasp_provider.cpp b/core/src/stages/grasp_provider.cpp new file mode 100644 index 000000000..3f2359b42 --- /dev/null +++ b/core/src/stages/grasp_provider.cpp @@ -0,0 +1,310 @@ +/********************************************************************* + * 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: Michael Goerner, Artur Karoly */ + +#include +#include +#include +#include + +#include + +#include +#include + +#include + +#include + +namespace moveit { +namespace task_constructor { +namespace stages { + +// ------------------- +// GraspProviderDefault +// ------------------- + +GraspProviderDefault::GraspProviderDefault(const std::string& name) : GraspProviderBase(name){ + auto& p = properties(); + p.declare("angle_delta", 0.1, "angular steps (rad)"); +} + +void GraspProviderDefault::init(const core::RobotModelConstPtr& robot_model) { + InitStageException errors; + try { + GraspProviderBase::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 object + props.get("object"); + // 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 { + if (props.get>("grasps").empty()) + errors.push_back(*this, "no grasp provided"); + else { + const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef); + moveit_msgs::Grasp grasp_ = props.get>("grasps")[0]; + if (grasp_.pre_grasp_approach.min_distance > grasp_.pre_grasp_approach.desired_distance) + errors.push_back(*this, "min approach distance is greater than desired distance in Grasp message's pre_grasp_approach field"); + if (grasp_.post_grasp_retreat.min_distance > grasp_.post_grasp_retreat.desired_distance) + errors.push_back(*this, "min approach distance is greater than desired distance in Grasp message's post_grasp_retreat field"); + if (grasp_.pre_grasp_posture.points[0].positions.size() != jmg->getActiveJointModels().size()) + errors.push_back(*this, "incorrect number of joints in Grasp message's pre_grasp_posture field"); + else { + for (unsigned i : boost::counting_range(size_t(0), grasp_.pre_grasp_posture.points[0].positions.size())){ + const moveit::core::JointModel::Bounds* bounds = jmg->getActiveJointModelsBounds()[i]; + for (auto bound : *bounds) { + if (bound.position_bounded_) { + if ((grasp_.pre_grasp_posture.points[0].positions[i] < bound.min_position_) || (grasp_.pre_grasp_posture.points[0].positions[i] > bound.max_position_)) + errors.push_back(*this, "joint value out of bounds in Grasp message's pre_grasp_posture field"); + } + } + } + } + if (grasp_.grasp_posture.points[0].positions.size() != jmg->getActiveJointModels().size()) + errors.push_back(*this, "incorrect number of joints in Grasp message's grasp_posture field"); + else { + for (unsigned i : boost::counting_range(size_t(0), grasp_.pre_grasp_posture.points[0].positions.size())){ + const moveit::core::JointModel::Bounds* bounds = jmg->getActiveJointModelsBounds()[i]; + for (auto bound : *bounds) { + if (bound.position_bounded_) { + if ((grasp_.grasp_posture.points[0].positions[i] < bound.min_position_) || (grasp_.grasp_posture.points[0].positions[i] > bound.max_position_)) + errors.push_back(*this, "joint value out of bounds in Grasp message's grasp_posture field"); + } + } + } + } + } + } + + if (errors) + throw errors; +} + +void GraspProviderDefault::compute() { + if (upstream_solutions_.empty()) + return; + planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff(); + + // set end effector pose + const auto& props = properties(); + const std::vector& grasps_ = props.get>("grasps"); + const std::string& eef = props.get("eef"); + const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef); + + geometry_msgs::PoseStamped target_pose_msg; + 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) { + // rotate object pose about z-axis + Eigen::Isometry3d target_pose(Eigen::AngleAxisd(current_angle, Eigen::Vector3d::UnitZ())); + current_angle += props.get("angle_delta"); + + InterfaceState state(scene); + tf::poseEigenToMsg(target_pose, target_pose_msg.pose); + state.properties().set("target_pose", target_pose_msg); + state.properties().set("approach_direction", grasps_[0].pre_grasp_approach.direction); + state.properties().set("approach_min_dist", static_cast(grasps_[0].pre_grasp_approach.min_distance)); + state.properties().set("approach_max_dist", static_cast(grasps_[0].pre_grasp_approach.desired_distance)); + + state.properties().set("retreat_direction", grasps_[0].post_grasp_retreat.direction); + state.properties().set("retreat_min_dist", static_cast(grasps_[0].post_grasp_retreat.min_distance)); + state.properties().set("retreat_max_dist", static_cast(grasps_[0].post_grasp_retreat.desired_distance)); + + std::map hand_open_pose; + std::map hand_close_pose; + + std::vector hand_joint_names = grasps_[0].pre_grasp_posture.joint_names; + std::vector open_pose = grasps_[0].pre_grasp_posture.points[0].positions; + std::vector close_pose = grasps_[0].grasp_posture.points[0].positions; + + std::transform(hand_joint_names.begin(), hand_joint_names.end(), open_pose.begin(), std::inserter(hand_open_pose, hand_open_pose.end()), std::make_pair); + std::transform(hand_joint_names.begin(), hand_joint_names.end(), close_pose.begin(), std::inserter(hand_close_pose, hand_close_pose.end()), std::make_pair); + + state.properties().set("hand_open_pose", hand_open_pose); + state.properties().set("hand_close_pose", hand_close_pose); + + SubTrajectory trajectory; + trajectory.setCost(0.0); + trajectory.setComment(std::to_string(current_angle)); + + // add frame at target pose + rviz_marker_tools::appendFrame(trajectory.markers(), target_pose_msg, 0.1, "grasp frame"); + + spawn(std::move(state), std::move(trajectory)); + } +} + + +// ------------------- +// GraspProviderFixedPoses +// ------------------- + + +GraspProviderFixedPoses::GraspProviderFixedPoses(const std::string& name) : GraspProviderBase(name){} + +void GraspProviderFixedPoses::init(const core::RobotModelConstPtr& robot_model) { + InitStageException errors; + try { + GraspProviderBase::init(robot_model); + } catch (InitStageException& e) { + errors.append(e); + } + + const auto& props = properties(); + + // check availability of object + props.get("object"); + // 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 { + if (props.get>("grasps").empty()) + errors.push_back(*this, "no grasp provided"); + else { + const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef); + std::vector grasps_ = props.get>("grasps"); + for (moveit_msgs::Grasp grasp_ : grasps_) { + if (grasp_.pre_grasp_approach.min_distance > grasp_.pre_grasp_approach.desired_distance) + errors.push_back(*this, "min approach distance is greater than desired distance in Grasp message's pre_grasp_approach field"); + if (grasp_.post_grasp_retreat.min_distance > grasp_.post_grasp_retreat.desired_distance) + errors.push_back(*this, "min approach distance is greater than desired distance in Grasp message's post_grasp_retreat field"); + if (grasp_.pre_grasp_posture.points[0].positions.size() != jmg->getActiveJointModels().size()) + errors.push_back(*this, "incorrect number of joints in Grasp message's pre_grasp_posture field"); + else { + for (unsigned i : boost::counting_range(size_t(0), grasp_.pre_grasp_posture.points[0].positions.size())){ + const moveit::core::JointModel::Bounds* bounds = jmg->getActiveJointModelsBounds()[i]; + for (auto bound : *bounds) { + if (bound.position_bounded_) { + if ((grasp_.pre_grasp_posture.points[0].positions[i] < bound.min_position_) || (grasp_.pre_grasp_posture.points[0].positions[i] > bound.max_position_)) + errors.push_back(*this, "joint value out of bounds in Grasp message's pre_grasp_posture field"); + } + } + } + } + if (grasp_.grasp_posture.points[0].positions.size() != jmg->getActiveJointModels().size()) + errors.push_back(*this, "incorrect number of joints in Grasp message's grasp_posture field"); + else { + for (unsigned i : boost::counting_range(size_t(0), grasp_.pre_grasp_posture.points[0].positions.size())){ + const moveit::core::JointModel::Bounds* bounds = jmg->getActiveJointModelsBounds()[i]; + for (auto bound : *bounds) { + if (bound.position_bounded_) { + if ((grasp_.grasp_posture.points[0].positions[i] < bound.min_position_) || (grasp_.grasp_posture.points[0].positions[i] > bound.max_position_)) + errors.push_back(*this, "joint value out of bounds in Grasp message's grasp_posture field"); + } + } + } + } + } + } + } + + if (errors) + throw errors; +} + +void GraspProviderFixedPoses::compute() { + if (upstream_solutions_.empty()) + return; + + planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff(); + + const std::vector& grasps_ = properties().get>("grasps"); + + for (moveit_msgs::Grasp grasp_ : grasps_){ + + geometry_msgs::PoseStamped target_pose = grasp_.grasp_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; + } + + InterfaceState state(scene); + state.properties().set("target_pose", target_pose); + state.properties().set("approach_direction", grasp_.pre_grasp_approach.direction); + state.properties().set("approach_min_dist", static_cast(grasp_.pre_grasp_approach.min_distance)); + state.properties().set("approach_max_dist", static_cast(grasp_.pre_grasp_approach.desired_distance)); + + state.properties().set("retreat_direction", grasp_.post_grasp_retreat.direction); + state.properties().set("retreat_min_dist", static_cast(grasp_.post_grasp_retreat.min_distance)); + state.properties().set("retreat_max_dist", static_cast(grasp_.post_grasp_retreat.desired_distance)); + + std::map hand_open_pose; + std::map hand_close_pose; + + std::vector hand_joint_names = grasp_.pre_grasp_posture.joint_names; + std::vector open_pose = grasp_.pre_grasp_posture.points[0].positions; + std::vector close_pose = grasp_.grasp_posture.points[0].positions; + + std::transform(hand_joint_names.begin(), hand_joint_names.end(), open_pose.begin(), std::inserter(hand_open_pose, hand_open_pose.end()), std::make_pair); + std::transform(hand_joint_names.begin(), hand_joint_names.end(), close_pose.begin(), std::inserter(hand_close_pose, hand_close_pose.end()), std::make_pair); + + state.properties().set("hand_open_pose", hand_open_pose); + state.properties().set("hand_close_pose", hand_close_pose); + + SubTrajectory trajectory; + trajectory.setCost(0.0); + + rviz_marker_tools::appendFrame(trajectory.markers(), target_pose, 0.1, "grasp frame"); + + spawn(std::move(state), std::move(trajectory)); + } +} + + + +} // namespace stages +} // namespace task_constructor +} // namespace moveit + +/// register plugin +#include +PLUGINLIB_EXPORT_CLASS(moveit::task_constructor::stages::GraspProviderDefault, moveit::task_constructor::stages::GraspProviderBase) +PLUGINLIB_EXPORT_CLASS(moveit::task_constructor::stages::GraspProviderFixedPoses, moveit::task_constructor::stages::GraspProviderBase) diff --git a/core/src/stages/grasp_provider_base.cpp b/core/src/stages/grasp_provider_base.cpp new file mode 100644 index 000000000..aa73931e1 --- /dev/null +++ b/core/src/stages/grasp_provider_base.cpp @@ -0,0 +1,101 @@ +/********************************************************************* + * 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: Artur Karoly, Jafar Abdi */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace moveit { +namespace task_constructor { +namespace stages { +GraspProviderBase::GraspProviderBase(const std::string& name) : GeneratePose(name) { + auto& p = properties(); + p.declare("eef", "name of end-effector"); + p.declare("object"); + + p.declare>("grasps", "list of Grasp messages"); +} +void GraspProviderBase::init(const std::shared_ptr& robot_model) { + InitStageException errors; + try { + GeneratePose::init(robot_model); + } catch (InitStageException& e) { + errors.append(e); + } + + const auto& props = properties(); + + // check availability of object + props.get("object"); + // check availability of eef + const std::string& eef = props.get("eef"); + if (!robot_model->hasEndEffector(eef)) + errors.push_back(*this, "unknown end effector: " + eef); + + if (errors) + throw errors; +} +void GraspProviderBase::onNewSolution(const SolutionBase& s) { + std::shared_ptr scene = s.end()->scene(); + + const auto& props = properties(); + const std::string& object = props.get("object"); + if (!scene->knowsFrameTransform(object)) { + const std::string msg = "object '" + object + "' not in scene"; + if (storeFailures()) { + InterfaceState state(scene); + SubTrajectory solution; + solution.markAsFailure(); + solution.setComment(msg); + spawn(std::move(state), std::move(solution)); + } else + ROS_WARN_STREAM_NAMED("GraspProviderBase", msg); + return; + } + + upstream_solutions_.push(&s); +} +} // namespace stages +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp deleted file mode 100644 index d289f2d4f..000000000 --- a/core/src/stages/pick.cpp +++ /dev/null @@ -1,103 +0,0 @@ -#include - -#include - -#include -#include - -#include - -namespace moveit { -namespace task_constructor { -namespace stages { - -PickPlaceBase::PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& name, bool forward) - : SerialContainer(name) { - PropertyMap& p = properties(); - p.declare("object", "name of object to grasp"); - p.declare("eef", "end effector name"); - p.declare("eef_frame", "name of end effector frame"); - - // internal properties (cannot be marked as such yet) - p.declare("eef_group", "JMG of eef"); - 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 init_ik_frame = [](const PropertyMap& other) -> boost::any { - geometry_msgs::PoseStamped pose; - const boost::any& frame = other.get("eef_frame"); - if (frame.empty()) - return boost::any(); - - pose.header.frame_id = boost::any_cast(frame); - pose.pose.orientation.w = 1.0; - return pose; - }; - - { - 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("ik_frame").configureInitFrom(Stage::PARENT, init_ik_frame); - p.set("marker_ns", std::string(forward ? "approach" : "retract")); - approach_stage_ = approach.get(); - insert(std::move(approach), insertion_position); - } - - grasp_stage_ = grasp_stage.get(); - grasp_stage->properties().configureInitFrom(Stage::PARENT, { "eef", "object" }); - insert(std::move(grasp_stage), insertion_position); - - { - 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("ik_frame").configureInitFrom(Stage::PARENT, init_ik_frame); - p.set("marker_ns", std::string(forward ? "lift" : "place")); - lift_stage_ = lift.get(); - insert(std::move(lift), insertion_position); - } -} - -void PickPlaceBase::init(const moveit::core::RobotModelConstPtr& robot_model) { - // inherit properties from parent - PropertyMap* p = &properties(); - p->performInitFrom(Stage::PARENT, parent()->properties()); - - // 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); - - // propagate my properties to children (and do standard init) - SerialContainer::init(robot_model); -} - -void PickPlaceBase::setApproachRetract(const geometry_msgs::TwistStamped& motion, double min_distance, - double max_distance) { - auto& p = approach_stage_->properties(); - p.set("direction", motion); - p.set("min_distance", min_distance); - p.set("max_distance", max_distance); -} - -void PickPlaceBase::setLiftPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { - auto& p = lift_stage_->properties(); - p.set("direction", motion); - p.set("min_distance", min_distance); - p.set("max_distance", max_distance); -} - -void PickPlaceBase::setLiftPlace(const std::map& joints) { - auto& p = lift_stage_->properties(); - p.set("joints", joints); -} -} // namespace stages -} // namespace task_constructor -} // namespace moveit diff --git a/core/src/stages/pick_place.cpp b/core/src/stages/pick_place.cpp new file mode 100644 index 000000000..d73c57cf2 --- /dev/null +++ b/core/src/stages/pick_place.cpp @@ -0,0 +1,242 @@ +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include "moveit/task_constructor/stages/grasp_provider_base.h" +#include "moveit/task_constructor/stages/place_provider_base.h" + +namespace moveit { +namespace task_constructor { +namespace stages { + +template +PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provider_stage_plugin_name, bool is_pick, pluginlib::ClassLoader* class_loader) + : SerialContainer(name) { + PropertyMap& p = properties(); + p.declare("object", "name of object to grasp/place"); + p.declare("eef", "end effector name"); + p.declare("ik_frame", "frame to be moved towards goal pose"); + p.declare>("support_surfaces", {}, "Name of support surfaces"); + + // internal properties (cannot be marked as such yet) + p.declare("eef_group", "JMG of eef"); + p.declare("eef_parent_group", "JMG of eef's parent"); + + cartesian_solver_ = std::make_shared(); + sampling_planner_ = std::make_shared(); + + is_pick_ = is_pick; + + provider_stage_plugin_name_ = provider_stage_plugin_name; + + { + auto move_there = std::make_unique(is_pick_ ? "approach object" : "lower object", cartesian_solver_); + PropertyMap& p = move_there->properties(); + p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); + p.property("ik_frame").configureInitFrom(Stage::PARENT, "ik_frame"); + p.set("marker_ns", std::string(is_pick_ ? "approach" : "place")); + p.property("direction").configureInitFrom(Stage::INTERFACE, "approach_direction"); + p.property("min_distance").configureInitFrom(Stage::INTERFACE, "approach_min_dist"); + p.property("max_distance").configureInitFrom(Stage::INTERFACE, "approach_max_dist"); + move_there_stage_ = move_there.get(); + insert(std::move(move_there)); + } + + if (provider_stage_plugin_name_ == "") + { + if (is_pick_) + provider_stage_plugin_name_ = "moveit_task_constructor/GraspProviderDefault"; + else + provider_stage_plugin_name_ = "moveit_task_constructor/PlaceProviderDefault"; + ROS_WARN_STREAM("The given name of the provider stage plugin is an empty string, using the default plugin (" << provider_stage_plugin_name_ << ") instead!"); + } + + try + { + std::unique_ptr wrapper; + if (is_pick_) { + std::unique_ptr provider_stage_plugin(class_loader->createUnmanagedInstance(provider_stage_plugin_name_)); + provider_stage_plugin->properties().configureInitFrom(Stage::PARENT); + provider_stage_plugin->properties().set("marker_ns", "grasp_pose"); + provider_plugin_stage_ = provider_stage_plugin.get(); + + wrapper = std::make_unique("grasp pose IK", std::move(provider_stage_plugin)); + } + else { + std::unique_ptr provider_stage_plugin(class_loader->createUnmanagedInstance(provider_stage_plugin_name_)); + provider_stage_plugin->properties().configureInitFrom(Stage::PARENT); + provider_stage_plugin->properties().set("marker_ns", "place_pose"); + provider_plugin_stage_ = provider_stage_plugin.get(); + + wrapper = std::make_unique("place pose IK", std::move(provider_stage_plugin)); + } + + properties().exposeTo(wrapper->properties(), {"object"}); + wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group", "ik_frame", "object"}); + wrapper->setForwardedProperties({"approach_direction", "approach_min_dist", "approach_max_dist", "retreat_direction", "retreat_min_dist", "retreat_max_dist", "hand_open_pose", "hand_close_pose"}); + wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); + compute_ik_stage_ = wrapper.get(); + insert(std::move(wrapper)); + + } + catch(pluginlib::PluginlibException& ex) + { + ROS_ERROR("The provider stage plugin failed to load. Error: %s", ex.what()); + } + + { + auto set_collision_object_hand = std::make_unique(is_pick_ ? "allow collision (hand,object)" : "forbid collision (hand,object)"); + set_collision_object_hand->setForwardedProperties({"retreat_direction", "retreat_min_dist", "retreat_max_dist", "hand_open_pose", "hand_close_pose"}); + set_collision_object_hand_stage_ = set_collision_object_hand.get(); + insert(std::move(set_collision_object_hand)); + } + + { + auto open_close_hand = std::make_unique(is_pick_ ? "close hand" : "open hand", sampling_planner_); + PropertyMap& p = open_close_hand->properties(); + p.property("group").configureInitFrom(Stage::PARENT, "eef_group"); + p.property("goal").configureInitFrom(Stage::INTERFACE, is_pick_ ? "hand_close_pose" : "hand_open_pose"); + open_close_hand->setForwardedProperties({"retreat_direction", "retreat_min_dist", "retreat_max_dist"}); + if (is_pick_) + insert(std::move(open_close_hand)); + else + insert(std::move(open_close_hand), -2); + } + + { + auto attach_detach = std::make_unique(is_pick_ ? "attach object" : "detach object"); + attach_detach->setForwardedProperties({"retreat_direction", "retreat_min_dist", "retreat_max_dist"}); + attach_detach_stage_ = attach_detach.get(); + insert(std::move(attach_detach)); + } + + if (is_pick_) { + auto set_collision_object_support = std::make_unique("allow collision (object,support)"); + set_collision_object_support->setForwardedProperties({"retreat_direction", "retreat_min_dist", "retreat_max_dist"}); + allow_collision_object_support_stage_ = set_collision_object_support.get(); + insert(std::move(set_collision_object_support)); + } + + { + auto move_back = std::make_unique(is_pick_ ? "lift object" : "retract", cartesian_solver_); + PropertyMap& p = move_back->properties(); + p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); + p.property("ik_frame").configureInitFrom(Stage::PARENT, "ik_frame"); + p.set("marker_ns", std::string(is_pick_ ? "lift" : "retract")); + p.property("direction").configureInitFrom(Stage::INTERFACE, "retreat_direction"); + p.property("min_distance").configureInitFrom(Stage::INTERFACE, "retreat_min_dist"); + p.property("max_distance").configureInitFrom(Stage::INTERFACE, "retreat_max_dist"); + move_back_stage_ = move_back.get(); + insert(std::move(move_back)); + } + + if (is_pick_) { + auto set_collision_object_support = std::make_unique("forbid collision (object,support)"); + forbid_collision_object_support_stage_ = set_collision_object_support.get(); + insert(std::move(set_collision_object_support)); + } +} + +template class PickPlaceBase; +template class PickPlaceBase; + +template +void PickPlaceBase::init(const moveit::core::RobotModelConstPtr& robot_model) { + // inherit properties from parent + PropertyMap* p = &properties(); + p->performInitFrom(Stage::PARENT, parent()->properties()); + + // 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); + + set_collision_object_hand_stage_->allowCollisions(properties().get("object"), + jmg->getLinkModelNamesWithCollisionGeometry(), is_pick_); + + const std::string& object = properties().get("object"); + const geometry_msgs::PoseStamped& ik_frame = properties().get("ik_frame"); + const std::vector& support_surfaces = properties().get>("support_surfaces"); + if (is_pick_) { + attach_detach_stage_->attachObject(object, ik_frame.header.frame_id); + allow_collision_object_support_stage_->allowCollisions({ object }, support_surfaces, true); + forbid_collision_object_support_stage_->allowCollisions({ object }, support_surfaces, false); + } + else + attach_detach_stage_->detachObject(object, ik_frame.header.frame_id); + + // propagate my properties to children (and do standard init) + SerialContainer::init(robot_model); +} + + +/// ------------------------- +/// setters of own properties + +template +void PickPlaceBase::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) { + geometry_msgs::PoseStamped pose_msg; + pose_msg.header.frame_id = link; + tf::poseEigenToMsg(pose, pose_msg.pose); + setIKFrame(pose_msg); +} + + +/// ------------------------- +/// setters of substage properties + +/// IK computation + +template +void PickPlaceBase::setMaxIKSolutions(const uint32_t& max_ik_solutions) { + auto& p = compute_ik_stage_->properties(); + p.set("max_ik_solutions", max_ik_solutions); +} + +template +void PickPlaceBase::setMinIKSolutionDistance(const double& min_ik_solution_distance) { + auto& p = compute_ik_stage_->properties(); + p.set("min_solution_distance", min_ik_solution_distance); +} + +template +void PickPlaceBase::setIgnoreIKCollisions(const bool& ignore_ik_collisions) { + auto& p = compute_ik_stage_->properties(); + p.set("ignore_collisions", ignore_ik_collisions); +} + + +/// ------------------------- +/// setters of pick properties + +void Pick::setMonitoredStage(Stage* monitored) { + provider_plugin_stage_->setMonitoredStage(monitored); +} + + +/// ------------------------- +/// setters of place properties + +void Place::setMonitoredStage(Stage* monitored) { + provider_plugin_stage_->setMonitoredStage(monitored); +} + +} // namespace stages +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/stages/place_provider.cpp b/core/src/stages/place_provider.cpp new file mode 100644 index 000000000..45c0f060c --- /dev/null +++ b/core/src/stages/place_provider.cpp @@ -0,0 +1,211 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, 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: Robert Haschke, Artur Karoly */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +namespace moveit { +namespace task_constructor { +namespace stages { + +// ------------------- +// PlaceProviderDefault +// ------------------- + +void PlaceProviderDefault::compute() { + if (upstream_solutions_.empty()) + return; + + const SolutionBase& s = *upstream_solutions_.pop(); + planning_scene::PlanningSceneConstPtr scene = s.end()->scene()->diff(); + const moveit::core::RobotState& robot_state = scene->getCurrentState(); + const auto& props = properties(); + + const moveit::core::AttachedBody* object = robot_state.getAttachedBody(props.get("object")); + // current object_pose w.r.t. planning frame + const Eigen::Isometry3d& orig_object_pose = object->getGlobalCollisionBodyTransforms()[0]; + + const std::vector& place_locations_ = props.get>("place_locations"); + const moveit_msgs::PlaceLocation& place_location_ =place_locations_[0]; + const geometry_msgs::PoseStamped& pose_msg = place_location_.place_pose; + Eigen::Isometry3d target_pose; + tf::poseMsgToEigen(pose_msg.pose, target_pose); + // target pose w.r.t. planning frame + scene->getTransforms().transformPose(pose_msg.header.frame_id, target_pose, target_pose); + + const geometry_msgs::PoseStamped& ik_frame_msg = props.get("ik_frame"); + Eigen::Isometry3d ik_frame; + tf::poseMsgToEigen(ik_frame_msg.pose, ik_frame); + ik_frame = robot_state.getGlobalLinkTransform(ik_frame_msg.header.frame_id) * ik_frame; + Eigen::Isometry3d object_to_ik = orig_object_pose.inverse() * ik_frame; + + // spawn the nominal target object pose, considering flip about z and rotations about z-axis + auto spawner = [&s, &scene, &object_to_ik, &place_location_, this](const Eigen::Isometry3d& nominal, uint z_flips, + uint z_rotations = 10) { + for (uint flip = 0; flip < z_flips; ++flip) { + // flip about object's x-axis + Eigen::Isometry3d object = nominal * Eigen::AngleAxisd(flip * M_PI, Eigen::Vector3d::UnitX()); + for (uint i = 0; i < z_rotations; ++i) { + // rotate object at target pose about world's z-axis + Eigen::Vector3d pos = object.translation(); + object.pretranslate(-pos) + .prerotate(Eigen::AngleAxisd(i * 2. * M_PI / z_rotations, Eigen::Vector3d::UnitZ())) + .pretranslate(pos); + + // target ik_frame's pose w.r.t. planning frame + geometry_msgs::PoseStamped target_pose_msg; + target_pose_msg.header.frame_id = scene->getPlanningFrame(); + tf::poseEigenToMsg(object * object_to_ik, target_pose_msg.pose); + + InterfaceState state(scene); + forwardProperties(*s.end(), state); // forward properties from inner solutions + state.properties().set("target_pose", target_pose_msg); + state.properties().set("approach_direction", place_location_.pre_place_approach.direction); + state.properties().set("approach_min_dist", static_cast(place_location_.pre_place_approach.min_distance)); + state.properties().set("approach_max_dist", static_cast(place_location_.pre_place_approach.desired_distance)); + + state.properties().set("retreat_direction", place_location_.post_place_retreat.direction); + state.properties().set("retreat_min_dist", static_cast(place_location_.post_place_retreat.min_distance)); + state.properties().set("retreat_max_dist", static_cast(place_location_.post_place_retreat.desired_distance)); + + std::map hand_open_pose; + + std::vector hand_joint_names = place_location_.post_place_posture.joint_names; + std::vector open_pose = place_location_.post_place_posture.points[0].positions; + + std::transform(hand_joint_names.begin(), hand_joint_names.end(), open_pose.begin(), std::inserter(hand_open_pose, hand_open_pose.end()), std::make_pair); + + // TODO(karolyartur): Raise exception if the sizes do not match + + state.properties().set("hand_open_pose", hand_open_pose); + + SubTrajectory trajectory; + trajectory.setCost(0.0); + rviz_marker_tools::appendFrame(trajectory.markers(), target_pose_msg, 0.1, "place frame"); + + spawn(std::move(state), std::move(trajectory)); + } + } + }; + + if (object->getShapes().size() == 1) { + switch (object->getShapes()[0]->type) { + case shapes::CYLINDER: + spawner(target_pose, 2); + return; + + case shapes::BOX: { // consider 180/90 degree rotations about z axis + const double* dims = static_cast(*object->getShapes()[0]).size; + spawner(target_pose, 2, (std::abs(dims[0] - dims[1]) < 1e-5) ? 4 : 2); + return; + } + case shapes::SPHERE: // keep original orientation and rotate about world's z + target_pose.linear() = orig_object_pose.linear(); + spawner(target_pose, 1); + return; + default: + break; + } + } + + // any other case: only try given target pose + spawner(target_pose, 1, 1); +} + +// ------------------- +// PlaceProviderDefault +// ------------------- + +void PlaceProviderFixedPoses::compute(){ + if (upstream_solutions_.empty()) + return; + + const SolutionBase& s = *upstream_solutions_.pop(); + planning_scene::PlanningSceneConstPtr scene = s.end()->scene()->diff(); + + const auto& props = properties(); + + const std::vector& place_locations_ = props.get>("place_locations"); + for (moveit_msgs::PlaceLocation place_location_ : place_locations_){ + + InterfaceState state(scene); + state.properties().set("target_pose", place_location_.place_pose); + state.properties().set("approach_direction", place_location_.pre_place_approach.direction); + state.properties().set("approach_min_dist", static_cast(place_location_.pre_place_approach.min_distance)); + state.properties().set("approach_max_dist", static_cast(place_location_.pre_place_approach.desired_distance)); + + state.properties().set("retreat_direction", place_location_.post_place_retreat.direction); + state.properties().set("retreat_min_dist", static_cast(place_location_.post_place_retreat.min_distance)); + state.properties().set("retreat_max_dist", static_cast(place_location_.post_place_retreat.desired_distance)); + + std::map hand_open_pose; + + std::vector hand_joint_names = place_location_.post_place_posture.joint_names; + std::vector open_pose = place_location_.post_place_posture.points[0].positions; + + std::transform(hand_joint_names.begin(), hand_joint_names.end(), open_pose.begin(), std::inserter(hand_open_pose, hand_open_pose.end()), std::make_pair); + + // TODO(karolyartur): Raise exception if the sizes do not match + + state.properties().set("hand_open_pose", hand_open_pose); + + SubTrajectory trajectory; + trajectory.setCost(0.0); + rviz_marker_tools::appendFrame(trajectory.markers(), place_location_.place_pose, 0.1, "place frame"); + + spawn(std::move(state), std::move(trajectory)); + } + +} + +} // namespace stages +} // namespace task_constructor +} // namespace moveit + +/// register plugin +#include +PLUGINLIB_EXPORT_CLASS(moveit::task_constructor::stages::PlaceProviderDefault, moveit::task_constructor::stages::PlaceProviderBase) +PLUGINLIB_EXPORT_CLASS(moveit::task_constructor::stages::PlaceProviderFixedPoses, moveit::task_constructor::stages::PlaceProviderBase) diff --git a/core/src/stages/place_provider_base.cpp b/core/src/stages/place_provider_base.cpp new file mode 100644 index 000000000..a08bbcd52 --- /dev/null +++ b/core/src/stages/place_provider_base.cpp @@ -0,0 +1,86 @@ +/********************************************************************* + * 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: Artur Karoly, Jafar Abdi */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "moveit/task_constructor/stages/place_provider_base.h" + +#include + +namespace moveit { +namespace task_constructor { +namespace stages { +PlaceProviderBase::PlaceProviderBase(const std::string& name) : GeneratePose(name) { + auto& p = properties(); + p.declare("object"); + p.declare<::geometry_msgs::PoseStamped_>>("ik_frame"); + p.declare>("place_locations"); +} +void PlaceProviderBase::onNewSolution(const SolutionBase& s) { + std::shared_ptr scene = s.end()->scene(); + + const auto& props = properties(); + const std::string& object = props.get("object"); + std::string msg; + if (!scene->getCurrentState().hasAttachedBody(object)) + msg = "'" + object + "' is not an attached object"; + if (scene->getCurrentState().getAttachedBody(object)->getFixedTransforms().empty()) + msg = "'" + object + "' has no associated shapes"; + if (!msg.empty()) { + if (storeFailures()) { + InterfaceState state(scene); + SubTrajectory solution; + solution.markAsFailure(); + solution.setComment(msg); + spawn(std::move(state), std::move(solution)); + } else + ROS_WARN_STREAM_NAMED("PlaceProviderBase", msg); + return; + } + + upstream_solutions_.push(&s); +} +} // namespace stages +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/stages/predicate_filter.cpp b/core/src/stages/predicate_filter.cpp index d64b2cb81..89650946d 100644 --- a/core/src/stages/predicate_filter.cpp +++ b/core/src/stages/predicate_filter.cpp @@ -85,10 +85,17 @@ void PredicateFilter::onNewSolution(const SolutionBase& s) { std::string comment = s.comment(); double cost = s.cost(); - if (!props.get("ignore_filter") && !props.get("predicate")(s, comment)) - cost = std::numeric_limits::infinity(); - - liftSolution(s, cost, comment); + if (!props.get("ignore_filter") && !props.get("predicate")(s, comment)) { + planning_scene::PlanningScenePtr scene = s.start()->scene()->diff(); + SubTrajectory solution; + solution.markAsFailure(); + solution.setComment(comment); + solution.setCost(std::numeric_limits::infinity()); + InterfaceState state(scene); + spawn(std::move(state), std::move(solution)); + } else { + liftSolution(s, cost, comment); + } } } // namespace stages } // namespace task_constructor diff --git a/core/src/tasks/CMakeLists.txt b/core/src/tasks/CMakeLists.txt new file mode 100644 index 000000000..254080742 --- /dev/null +++ b/core/src/tasks/CMakeLists.txt @@ -0,0 +1,9 @@ +add_library(${PROJECT_NAME}_tasks + ${PROJECT_INCLUDE}/tasks/pick_place_task.h + pick_place_task.cpp +) +target_link_libraries(${PROJECT_NAME}_tasks ${PROJECT_NAME} ${PROJECT_NAME}_stages ${catkin_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME}_tasks + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) diff --git a/core/src/tasks/pick_place_task.cpp b/core/src/tasks/pick_place_task.cpp new file mode 100644 index 000000000..5fc3e94e1 --- /dev/null +++ b/core/src/tasks/pick_place_task.cpp @@ -0,0 +1,281 @@ +/********************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019 PickNik LLC. + * 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 the copyright holder 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 HOLDER 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. + *********************************************************************/ + +/* Author: Henning Kayser, Simon Goldstein, Artur Karoly + Desc: MoveIt Task Constructor Task for the PickPlace Capability +*/ + +#include +#include +#include + +namespace moveit { +namespace task_constructor { +namespace tasks { + +constexpr char LOGNAME[] = "pick_place_task"; +PickPlaceTask::PickPlaceTask(const std::string& task_name) + : task_name_(task_name) { + grasp_provider_class_loader_ = std::make_unique("moveit_task_constructor_core", "moveit::task_constructor::stages::GraspProviderBase"); + place_provider_class_loader_ = std::make_unique("moveit_task_constructor_core", "moveit::task_constructor::stages::PlaceProviderBase"); + task_.reset(); + task_.reset(new moveit::task_constructor::Task(task_name_)); + task_->reset(); + task_->loadRobotModel(); + current_state_stage_ = nullptr; + attach_object_stage_ = nullptr; + } + +bool grasp_pose_is_defined(const geometry_msgs::PoseStamped& grasp_pose) +{ + if (grasp_pose.header.frame_id != "") + return true; + return false; +} + +bool PickPlaceTask::init(const Parameters& parameters) +{ + ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); + if(task_){ + task_->clear(); + task_->loadRobotModel(); + } + Task& t = *task_; + + // Sampling planner + // TODO(henningkayser): Setup and parameterize alternative planners + auto sampling_planner = std::make_shared(); + sampling_planner->setProperty("goal_joint_tolerance", 1e-5); + + // Cartesian planner + auto cartesian_planner = std::make_shared(); + cartesian_planner->setMaxVelocityScaling(1.0); + cartesian_planner->setMaxAccelerationScaling(1.0); + cartesian_planner->setStepSize(.01); + + // Set task properties + t.setProperty("group", parameters.arm_group_name_); + t.setProperty("eef", parameters.eef_name_); + t.setProperty("hand", parameters.hand_group_name_); + t.setProperty("ik_frame", parameters.hand_frame_); + + /**************************************************** + * * + * Current State * + * * + ***************************************************/ + { + auto _current_state = std::make_unique("current state"); + _current_state->setTimeout(10); + + // Verify that object is not attached for picking and if object is attached for placing + auto applicability_filter = + std::make_unique("applicability test", std::move(_current_state)); + applicability_filter->setPredicate([&](const SolutionBase& s, std::string& comment) { + s.start()->scene()->printKnownObjects(std::cout); + + if (parameters.task_type_ == moveit_task_constructor_msgs::PlanPickPlaceGoal::PICK_ONLY || + parameters.task_type_ == moveit_task_constructor_msgs::PlanPickPlaceGoal::PICK_AND_PLACE) + { + if (s.start()->scene()->getCurrentState().hasAttachedBody(parameters.object_name_)) + { + comment = "object with id '" + parameters.object_name_ + "' is already attached and cannot be picked"; + return false; + } + } + else if (parameters.task_type_ == moveit_task_constructor_msgs::PlanPickPlaceGoal::PLACE_ONLY) + { + if (!s.start()->scene()->getCurrentState().hasAttachedBody(parameters.object_name_)) + { + comment = "object with id '" + parameters.object_name_ + "' is not attached, so it cannot be placed"; + return false; + } + } + return true; + }); + + current_state_stage_ = applicability_filter.get(); + t.add(std::move(applicability_filter)); + } + + if (parameters.task_type_ == moveit_task_constructor_msgs::PlanPickPlaceGoal::PICK_ONLY || + parameters.task_type_ == moveit_task_constructor_msgs::PlanPickPlaceGoal::PICK_AND_PLACE) + { + + /**************************************************** + * * + * Move to Pick * + * * + ***************************************************/ + { // Move-to pre-grasp + auto stage = std::make_unique( + "move to pick", stages::Connect::GroupPlannerVector{ { parameters.arm_group_name_, sampling_planner } }); + stage->setTimeout(5.0); + stage->properties().configureInitFrom(Stage::PARENT); + t.add(std::move(stage)); + } + + /**************************************************** + * * + * Pick Object * + * * + ***************************************************/ + { // Decide which grasp provider to use + std::string grasp_provider_plugin; + if (parameters.grasp_provider_plugin_name_ == ""){ + if (parameters.grasps_.empty()){ + ROS_ERROR_STREAM("No grasp provider plugin and no grasps are specified for stage 'Pick' \n" + "Either set the grasp provider plugin, or provide at least one Grasp in which at least the approach and retreat fields and gripper poses are specified"); + grasp_provider_plugin = "moveit_task_constructor/GraspProviderDefault"; + } else if (parameters.grasps_.size() == 1) { + if (!grasp_pose_is_defined(parameters.grasps_[0].grasp_pose)){ + grasp_provider_plugin = "moveit_task_constructor/GraspProviderDefault"; + } else { + grasp_provider_plugin = "moveit_task_constructor/GraspProviderFixedPoses"; + } + } else { + grasp_provider_plugin = "moveit_task_constructor/GraspProviderFixedPoses"; + } + } else { + grasp_provider_plugin = parameters.grasp_provider_plugin_name_; + } + + // Create the Pick container + auto stage = std::make_unique("Pick object", grasp_provider_plugin, grasp_provider_class_loader_.get()); + stage->properties().property("eef_group").configureInitFrom(Stage::PARENT, "hand"); + stage->properties().property("eef_parent_group").configureInitFrom(Stage::PARENT, "group"); + stage->setObject(parameters.object_name_); + stage->setEndEffector(parameters.eef_name_); + stage->setSupportSurfaces(parameters.support_surfaces_); + stage->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_); + stage->setMaxIKSolutions(8); + stage->ProviderPlugin()->properties().set("grasps", parameters.grasps_); + stage->setMonitoredStage(current_state_stage_); + if (grasp_provider_plugin == "moveit_task_constructor/GraspProviderDefault") { + stage->ProviderPlugin()->properties().set("angle_delta", M_PI / 12); + } else if (grasp_provider_plugin != "moveit_task_constructor/GraspProviderFixedPoses") { + // TODO(karolyartur): Set plugin-specific properties systematically + } + attach_object_stage_ = stage->attachStage(); + t.add(std::move(stage)); + } + } + + if (parameters.task_type_ == moveit_task_constructor_msgs::PlanPickPlaceGoal::PLACE_ONLY || + parameters.task_type_ == moveit_task_constructor_msgs::PlanPickPlaceGoal::PICK_AND_PLACE) + { + + /****************************************************** + * * + * Move to Place * + * * + *****************************************************/ + { + auto stage = std::make_unique( + "move to place", stages::Connect::GroupPlannerVector{ { parameters.arm_group_name_, sampling_planner } }); + stage->setTimeout(5.0); + stage->properties().configureInitFrom(Stage::PARENT); + t.add(std::move(stage)); + } + + /****************************************************** + * * + * Place Object * + * * + *****************************************************/ + { // Decide which place provider to use + std::string place_provider_plugin; + if (parameters.place_provider_plugin_name_ == ""){ + if (parameters.place_locations_.empty()){ + ROS_ERROR_STREAM("No place provider plugin and no place locations are specified for stage 'Place' \n" + "Either set the place provider plugin, or provide at least one PlaceLocation"); + return false; + } else if(parameters.place_locations_.size()==1){ + ROS_WARN_STREAM("No place provider plugin and only one place location specified in stage 'Place' \n" + "Assuming place provider plugin to be moveit_task_constructor/PlaceProviderDefault"); + place_provider_plugin = "moveit_task_constructor/PlaceProviderDefault"; + } else { + place_provider_plugin = "moveit_task_constructor/PlaceProviderFixedPoses"; + } + } else { + place_provider_plugin = parameters.place_provider_plugin_name_; + } + + // Create the Place container + auto stage = std::make_unique("Place object", place_provider_plugin, place_provider_class_loader_.get()); + stage->properties().property("eef_group").configureInitFrom(Stage::PARENT, "hand"); + stage->properties().property("eef_parent_group").configureInitFrom(Stage::PARENT, "group"); + stage->setObject(parameters.object_name_); + stage->setEndEffector(parameters.eef_name_); + stage->setSupportSurfaces(parameters.support_surfaces_); + stage->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_); + stage->setMaxIKSolutions(8); + if (parameters.task_type_ == moveit_task_constructor_msgs::PlanPickPlaceGoal::PLACE_ONLY) + stage->setMonitoredStage(current_state_stage_); + if (parameters.task_type_ == moveit_task_constructor_msgs::PlanPickPlaceGoal::PICK_AND_PLACE) + stage->setMonitoredStage(attach_object_stage_); + stage->ProviderPlugin()->properties().set("place_locations", parameters.place_locations_); + if (place_provider_plugin != "moveit_task_constructor/PlaceProviderDefault" || place_provider_plugin != "moveit_task_constructor/PlaceProviderFixedPoses") { + // TODO(karolyartur): Set plugin-specific properties systematically + } + t.add(std::move(stage)); + } + } +} + +bool PickPlaceTask::plan() { + ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions"); + try { + task_->plan(1); // TODO(karolyartur): parameterize + } catch (InitStageException& e) { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e); + return false; + } + if (task_->numSolutions() == 0) { + ROS_ERROR_NAMED(LOGNAME, "Planning failed"); + return false; + } + return true; +} + +bool PickPlaceTask::preempt() { + task_->preempt(); + return true; +} + +void PickPlaceTask::getSolutionMsg(moveit_task_constructor_msgs::Solution& solution) { + task_->solutions().front()->fillMessage(solution); +} + +} // namespace tasks +} // namespace task_constructor +} // namespace moveit diff --git a/core/test/pick_pr2.cpp b/core/test/pick_pr2.cpp index ac1706320..06c4b5ac1 100644 --- a/core/test/pick_pr2.cpp +++ b/core/test/pick_pr2.cpp @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include #include diff --git a/core/test/pick_ur5.cpp b/core/test/pick_ur5.cpp index 6c2808ec8..7ea227c86 100644 --- a/core/test/pick_ur5.cpp +++ b/core/test/pick_ur5.cpp @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include #include diff --git a/msgs/CMakeLists.txt b/msgs/CMakeLists.txt index b72f07873..2ee5c82ac 100644 --- a/msgs/CMakeLists.txt +++ b/msgs/CMakeLists.txt @@ -27,6 +27,7 @@ add_service_files(DIRECTORY srv FILES add_action_files(DIRECTORY action FILES ExecuteTaskSolution.action + PlanPickPlace.action ) generate_messages(DEPENDENCIES ${MSG_DEPS}) diff --git a/msgs/action/PlanPickPlace.action b/msgs/action/PlanPickPlace.action new file mode 100644 index 000000000..8681f5aca --- /dev/null +++ b/msgs/action/PlanPickPlace.action @@ -0,0 +1,40 @@ +# By default, the task type is PICK_ONLY +byte PICK_ONLY=0 +byte PLACE_ONLY=1 +byte PICK_AND_PLACE=2 +byte task_type + +# Planning group properties +string arm_group_name +string hand_group_name + +# Robot model and links +string eef_name # The name of the end effector as in the srdf +string hand_frame # Will be used as the endEffectorFrame + +# Object and surfaces +# During the pick and place operation, the object is allowed to collide with the support_surfaces +string object_id # ID of the CollisionObject to pick/place +string[] support_surfaces + +# Picking +# grasp_provider_plugin_name defaults to "moveit_task_constructor/GraspProviderDefault" if only one Grasp is provided in grasps without setting its grasp_pose field +# If the grasp_pose field is set, or there are multiple Grasps provided in grasps, grasp_provider_plugin_name defaults to "moveit_task_constructor/GraspProviderFixedPoses" +string grasp_provider_plugin_name # Name of the Grasp Provider plugin to load +moveit_msgs/Grasp[] grasps # List of Grasps (The approach and retreat movements and hand poses are all used and required for each Grasp) +geometry_msgs/Pose grasp_frame_transform # Additional transformation to apply to the endEffectorFrame when computing the IK + +# Placing +# place_provider_plugin_name defaults to "moveit_task_constructor/PlaceProviderDefault" if only one PlaceLocation is provided in place_locations +# Otherwise place_provider_plugin_name defaults to "moveit_task_constructor/PlaceProviderFixedPoses" +string place_provider_plugin_name # Name of the Place Provider plugin to load +moveit_msgs/PlaceLocation[] place_locations # List of PlaceLocations (The approach and retreat movements and hand poses are all used and required for each PlaceLocation) + +--- + +bool success +Solution solution + +--- + +string feedback diff --git a/msgs/notes_from_moveit_msgs.action b/msgs/notes_from_moveit_msgs.action new file mode 100644 index 000000000..f924b58fc --- /dev/null +++ b/msgs/notes_from_moveit_msgs.action @@ -0,0 +1,95 @@ +### +### This file is a note "merging" the Pickup and Place actions from moveit_msgs. +### Fields indented 0 times are only in the "Pickup" action. +### Fields indented 1 times are in both "Pickup" and "Place". +### Fields indented 2 times are only in the "Place" action. +### This should help get an overview which information should be included +### in a replacement PlanPickPlace action in moveit_task_constructor. + +### Other messages to consider: +### Grasp, PlaceLocation +### GraspPlanning, PlanningOptions + +# An action for picking or placing an object + +# The name of the object to pick up (as known in the planning scene) +string target_name + + # which group should be used to plan for pickup + string group_name + + # the name of the attached object to place + string attached_object_name + +# which end-effector to be used for pickup (ideally descending from the group above) +string end_effector + +# a list of possible grasps to be used. At least one grasp must be filled in +Grasp[] possible_grasps + + # a list of possible transformations for placing the object + PlaceLocation[] place_locations + + # if the user prefers setting the eef pose (same as in pick) rather than + # the location of the object, this flag should be set to true + bool place_eef + + # the name that the support surface (e.g. table) has in the collision map + # can be left empty if no name is available + string support_surface_name + + # whether collisions between the gripper and the support surface should be acceptable + # during move from pre-grasp to grasp and during lift. Collisions when moving to the + # pre-grasp location are still not allowed even if this is set to true. + bool allow_gripper_support_collision + +# The names of the links the object to be attached is allowed to touch; +# If this is left empty, it defaults to the links in the used end-effector +string[] attached_object_touch_links + +# Optionally notify the pick action that it should approach the object further, +# as much as possible (this minimizing the distance to the object before the grasp) +# along the approach direction; Note: this option changes the grasping poses +# supplied in possible_grasps[] such that they are closer to the object when possible. +bool minimize_object_distance + + # Optional constraints to be imposed on every point in the motion plan + Constraints path_constraints + + # The name of the motion planner to use. If no name is specified, + # a default motion planner will be used + string planner_id + + # an optional list of obstacles that we have semantic information about + # and that can be touched/pushed/moved in the course of grasping; + # CAREFUL: If the object name 'all' is used, collisions with all objects are disabled during the approach & lift. + string[] allowed_touch_objects + + # The maximum amount of time the motion planner is allowed to plan for + float64 allowed_planning_time + + # Planning options + PlanningOptions planning_options + +--- + + # The overall result of the pickup attempt + MoveItErrorCodes error_code + + # The full starting state of the robot at the start of the trajectory + RobotState trajectory_start + + # The trajectory that moved group produced for execution + RobotTrajectory[] trajectory_stages + + string[] trajectory_descriptions + +# The performed grasp, if attempt was successful +Grasp grasp + + # The successful place location, if any + PlaceLocation place_location + + # The amount of time in seconds it took to complete the plan + float64 planning_time + --- \ No newline at end of file