From 95e7256c4576d1e7192e1e3c3a0cd49bc6fd2de7 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 20 Nov 2019 16:43:08 +0100 Subject: [PATCH 1/4] Add action message template --- msgs/CMakeLists.txt | 1 + msgs/action/PlanPickPlace.action | 16 ++++++++++++++++ 2 files changed, 17 insertions(+) create mode 100644 msgs/action/PlanPickPlace.action diff --git a/msgs/CMakeLists.txt b/msgs/CMakeLists.txt index 07ed05951..c49dcd9a6 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..5acfdb21a --- /dev/null +++ b/msgs/action/PlanPickPlace.action @@ -0,0 +1,16 @@ +# goal +string object_id +string end_effector + +moveit_msgs/PlaceLocation place_location + +--- + +# result +moveit_msgs/MoveItErrorCodes error_code +Solution solution + +--- + +# feedback +string feedback From bf78d46715d61e5478304d1dd66e6550d46b98dd Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 20 Nov 2019 16:43:29 +0100 Subject: [PATCH 2/4] Add PickPlaceTask to core package --- .../task_constructor/tasks/pick_place_task.h | 129 +++++ core/src/CMakeLists.txt | 1 + core/src/tasks/CMakeLists.txt | 9 + core/src/tasks/pick_place_task.cpp | 458 ++++++++++++++++++ 4 files changed, 597 insertions(+) create mode 100644 core/include/moveit/task_constructor/tasks/pick_place_task.h create mode 100644 core/src/tasks/CMakeLists.txt create mode 100644 core/src/tasks/pick_place_task.cpp 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..71a0a32d0 --- /dev/null +++ b/core/include/moveit/task_constructor/tasks/pick_place_task.h @@ -0,0 +1,129 @@ +/********************************************************************* + * 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 + Desc: A demo to show MoveIt Task Constructor in action +*/ + +// ROS +#include + +// MoveIt +#include +#include +#include + +// MTC +#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 namespace moveit::task_constructor; + +class PickPlaceTask +{ +public: + PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh); + ~PickPlaceTask() = default; + + void loadParameters(); + + void init(); + + bool plan(); + + bool execute(); + +private: + ros::NodeHandle nh_; + + std::string task_name_; + moveit::task_constructor::TaskPtr task_; + + // 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_reference_frame_; + std::string surface_link_; + std::string object_name_; + std::string world_frame_; + std::vector object_dimensions_; + + // Predefined pose targets + std::string hand_open_pose_; + std::string hand_close_pose_; + std::string arm_home_pose_; + + // Execution + actionlib::SimpleActionClient execute_; + + // Pick metrics + Eigen::Isometry3d grasp_frame_transform_; + double approach_object_min_dist_; + double approach_object_max_dist_; + double lift_object_min_dist_; + double lift_object_max_dist_; + + // Place metrics + geometry_msgs::Pose place_pose_; + double place_surface_offset_; +}; + +} // namespace tasks +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index cf8b6ad1f..b9e9823a9 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -41,6 +41,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/tasks/CMakeLists.txt b/core/src/tasks/CMakeLists.txt new file mode 100644 index 000000000..ef0d55285 --- /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} ${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..99b52aa9e --- /dev/null +++ b/core/src/tasks/pick_place_task.cpp @@ -0,0 +1,458 @@ +/********************************************************************* + * 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 + Desc: A demo to show MoveIt Task Constructor in action +*/ + +#include +#include + +namespace moveit { +namespace task_constructor { +namespace tasks { + +constexpr char LOGNAME[] = "pick_place_task"; +PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh) + : nh_(nh), task_name_(task_name), execute_("execute_task_solution", true) {} + +void PickPlaceTask::loadParameters() { + /**************************************************** + * * + * Load Parameters * + * * + ***************************************************/ + ROS_INFO_NAMED(LOGNAME, "Loading task parameters"); + ros::NodeHandle pnh("~"); + + // Planning group properties + size_t errors = 0; + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_group_name", arm_group_name_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_group_name", hand_group_name_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "eef_name", eef_name_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_frame", hand_frame_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "world_frame", world_frame_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "grasp_frame_transform", grasp_frame_transform_); + + // Predefined pose targets + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_open_pose", hand_open_pose_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_close_pose", hand_close_pose_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_home_pose", arm_home_pose_); + + // Target object + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "surface_link", surface_link_); + support_surfaces_ = { surface_link_ }; + + // Pick/Place metrics + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_min_dist", approach_object_min_dist_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_max_dist", approach_object_max_dist_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_min_dist", lift_object_min_dist_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_max_dist", lift_object_max_dist_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_surface_offset", place_surface_offset_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_pose", place_pose_); + rosparam_shortcuts::shutdownIfError(LOGNAME, errors); +} + +void PickPlaceTask::init() { + ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); + const std::string object = "object"; + + // Reset ROS introspection before constructing the new object + // TODO(henningkayser): verify this is a bug, fix if possible + task_.reset(); + task_.reset(new moveit::task_constructor::Task()); + + Task& t = *task_; + t.stages()->setName(task_name_); + t.loadRobotModel(); + + // Sampling planner + 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", arm_group_name_); + t.setProperty("eef", eef_name_); + t.setProperty("hand", hand_group_name_); + t.setProperty("hand_grasping_frame", hand_frame_); + t.setProperty("ik_frame", hand_frame_); + + /**************************************************** + * * + * Current State * + * * + ***************************************************/ + Stage* current_state = nullptr; // Forward current_state on to grasp pose generator + { + auto _current_state = std::make_unique("current state"); + + // Verify that object is not attachd + auto applicability_filter = + std::make_unique("applicability test", std::move(_current_state)); + applicability_filter->setPredicate([object](const SolutionBase& s, std::string& comment) { + if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) { + comment = "object with id '" + object + "' is already attached and cannot be picked"; + return false; + } + return true; + }); + + current_state = applicability_filter.get(); + t.add(std::move(applicability_filter)); + } + + /**************************************************** + * * + * Open Hand * + * * + ***************************************************/ + { // Open Hand + auto stage = std::make_unique("open hand", sampling_planner); + stage->setGroup(hand_group_name_); + stage->setGoal(hand_open_pose_); + t.add(std::move(stage)); + } + + /**************************************************** + * * + * Move to Pick * + * * + ***************************************************/ + { // Move-to pre-grasp + auto stage = std::make_unique( + "move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); + stage->setTimeout(5.0); + stage->properties().configureInitFrom(Stage::PARENT); + t.add(std::move(stage)); + } + + /**************************************************** + * * + * Pick Object * + * * + ***************************************************/ + Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator + { + auto grasp = std::make_unique("pick object"); + t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" }); + grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" }); + + /**************************************************** + ---- * Approach Object * + ***************************************************/ + { + auto stage = std::make_unique("approach object", cartesian_planner); + stage->properties().set("marker_ns", "approach_object"); + stage->properties().set("link", hand_frame_); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_); + + // Set hand forward direction + geometry_msgs::Vector3Stamped vec; + vec.header.frame_id = hand_frame_; + vec.vector.z = 1.0; + stage->setDirection(vec); + grasp->insert(std::move(stage)); + } + + /**************************************************** + ---- * Generate Grasp Pose * + ***************************************************/ + { + // Sample grasp pose + auto stage = std::make_unique("generate grasp pose"); + stage->properties().configureInitFrom(Stage::PARENT); + stage->properties().set("marker_ns", "grasp_pose"); + stage->setPreGraspPose(hand_open_pose_); + stage->setObject(object); + stage->setAngleDelta(M_PI / 12); + stage->setMonitoredStage(current_state); // Hook into current state + + // Compute IK + auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); + wrapper->setMaxIKSolutions(8); + wrapper->setMinSolutionDistance(1.0); + wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); + wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); + wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); + grasp->insert(std::move(wrapper)); + } + + /**************************************************** + ---- * Allow Collision (hand object) * + ***************************************************/ + { + auto stage = std::make_unique("allow collision (hand,object)"); + stage->allowCollisions( + object, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), + true); + grasp->insert(std::move(stage)); + } + + /**************************************************** + ---- * Close Hand * + ***************************************************/ + { + auto stage = std::make_unique("close hand", sampling_planner); + stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_group_name_); + stage->setGoal(hand_close_pose_); + grasp->insert(std::move(stage)); + } + + /**************************************************** + .... * Attach Object * + ***************************************************/ + { + auto stage = std::make_unique("attach object"); + stage->attachObject(object, hand_frame_); + attach_object_stage = stage.get(); + grasp->insert(std::move(stage)); + } + + /**************************************************** + .... * Allow collision (object support) * + ***************************************************/ + { + auto stage = std::make_unique("allow collision (object,support)"); + stage->allowCollisions({ object }, support_surfaces_, true); + grasp->insert(std::move(stage)); + } + + /**************************************************** + .... * Lift object * + ***************************************************/ + { + auto stage = std::make_unique("lift object", cartesian_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(lift_object_min_dist_, lift_object_max_dist_); + stage->setIKFrame(hand_frame_); + stage->properties().set("marker_ns", "lift_object"); + + // Set upward direction + geometry_msgs::Vector3Stamped vec; + vec.header.frame_id = world_frame_; + vec.vector.z = 1.0; + stage->setDirection(vec); + grasp->insert(std::move(stage)); + } + + /**************************************************** + .... * Forbid collision (object support) * + ***************************************************/ + { + auto stage = std::make_unique("forbid collision (object,surface)"); + stage->allowCollisions({ object }, support_surfaces_, false); + grasp->insert(std::move(stage)); + } + + // Add grasp container to task + t.add(std::move(grasp)); + } + + /****************************************************** + * * + * Move to Place * + * * + *****************************************************/ + { + auto stage = std::make_unique( + "move to place", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); + stage->setTimeout(5.0); + stage->properties().configureInitFrom(Stage::PARENT); + t.add(std::move(stage)); + } + + /****************************************************** + * * + * Place Object * + * * + *****************************************************/ + { + auto place = std::make_unique("place object"); + t.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); + place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" }); + + /****************************************************** + ---- * Lower Object * + *****************************************************/ + { + auto stage = std::make_unique("lower object", cartesian_planner); + stage->properties().set("marker_ns", "lower_object"); + stage->properties().set("link", hand_frame_); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(.03, .13); + + // Set downward direction + geometry_msgs::Vector3Stamped vec; + vec.header.frame_id = world_frame_; + vec.vector.z = -1.0; + stage->setDirection(vec); + place->insert(std::move(stage)); + } + + /****************************************************** + ---- * Generate Place Pose * + *****************************************************/ + { + // Generate Place Pose + auto stage = std::make_unique("generate place pose"); + stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" }); + stage->properties().set("marker_ns", "place_pose"); + stage->setObject(object); + + // Set target pose + geometry_msgs::PoseStamped p; + p.header.frame_id = object_reference_frame_; + p.pose = place_pose_; + p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_; + stage->setPose(p); + stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage + + // Compute IK + auto wrapper = std::make_unique("place pose IK", std::move(stage)); + wrapper->setMaxIKSolutions(2); + wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); + wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); + wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); + place->insert(std::move(wrapper)); + } + + /****************************************************** + ---- * Open Hand * + *****************************************************/ + { + auto stage = std::make_unique("open hand", sampling_planner); + stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_group_name_); + stage->setGoal(hand_open_pose_); + place->insert(std::move(stage)); + } + + /****************************************************** + ---- * Forbid collision (hand, object) * + *****************************************************/ + { + auto stage = std::make_unique("forbid collision (hand,object)"); + stage->allowCollisions( + object_name_, + t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), false); + place->insert(std::move(stage)); + } + + /****************************************************** + ---- * Detach Object * + *****************************************************/ + { + auto stage = std::make_unique("detach object"); + stage->detachObject(object_name_, hand_frame_); + place->insert(std::move(stage)); + } + + /****************************************************** + ---- * Retreat Motion * + *****************************************************/ + { + auto stage = std::make_unique("retreat after place", cartesian_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(.12, .25); + stage->setIKFrame(hand_frame_); + stage->properties().set("marker_ns", "retreat"); + geometry_msgs::Vector3Stamped vec; + vec.header.frame_id = hand_frame_; + vec.vector.z = -1.0; + stage->setDirection(vec); + place->insert(std::move(stage)); + } + + // Add place container to task + t.add(std::move(place)); + } + + /****************************************************** + * * + * Move to Home * + * * + *****************************************************/ + { + auto stage = std::make_unique("move home", sampling_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setGoal(arm_home_pose_); + stage->restrictDirection(stages::MoveTo::FORWARD); + t.add(std::move(stage)); + } +} + +bool PickPlaceTask::plan() { + ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions"); + ros::NodeHandle pnh("~"); + int planning_attempts = pnh.param("planning_attempts", 10); + + try { + task_->plan(planning_attempts); + } 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::execute() { + ROS_INFO_NAMED(LOGNAME, "Executing solution trajectory"); + moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal; + task_->solutions().front()->fillMessage(execute_goal.solution); + execute_.sendGoal(execute_goal); + execute_.waitForResult(); + moveit_msgs::MoveItErrorCodes execute_result = execute_.getResult()->error_code; + + if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed and returned: " << execute_.getState().toString()); + return false; + } + + return true; +} + +} // namespace tasks +} // namespace task_constructor +} // namespace moveit From 916d26db25c540e61ed6b3d667417cf8b60a96db Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 20 Nov 2019 16:43:52 +0100 Subject: [PATCH 3/4] Create empty plan_pick_place_capability --- capabilities/CMakeLists.txt | 3 + .../capabilities_plugin_description.xml | 6 ++ capabilities/package.xml | 1 + .../src/plan_pick_place_capability.cpp | 74 +++++++++++++++++++ capabilities/src/plan_pick_place_capability.h | 71 ++++++++++++++++++ 5 files changed, 155 insertions(+) create mode 100644 capabilities/src/plan_pick_place_capability.cpp create mode 100644 capabilities/src/plan_pick_place_capability.h diff --git a/capabilities/CMakeLists.txt b/capabilities/CMakeLists.txt index df46f9ff2..99a1ab58d 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 ) @@ -16,6 +17,7 @@ catkin_package( LIBRARIES $PROJECT_NAME} CATKIN_DEPENDS moveit_task_constructor_msgs + moveit_task_constructor_core std_msgs ) @@ -25,6 +27,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..0f0ffd07e --- /dev/null +++ b/capabilities/src/plan_pick_place_capability.cpp @@ -0,0 +1,74 @@ +/********************************************************************* + * 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 */ + +#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(); + ros::NodeHandle nh("~"); + pick_place_task_ = std::make_unique("", nh); +} + +void PlanPickPlaceCapability::goalCallback( + const moveit_task_constructor_msgs::PlanPickPlaceGoalConstPtr& goal) { + moveit_task_constructor_msgs::PlanPickPlaceResult result; + + // initialize task + // run plan + // return result +} + +void PlanPickPlaceCapability::preemptCallback() { + // TODO(henningkayser): abort planning +} + +} // 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 From 7f3bdaa607d114f662d591c0ca4de405536ec56a Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 20 Nov 2019 17:32:48 +0100 Subject: [PATCH 4/4] Add parameter struct --- .../src/plan_pick_place_capability.cpp | 10 +- .../task_constructor/tasks/pick_place_task.h | 83 +-- core/src/tasks/pick_place_task.cpp | 705 ++++++++---------- 3 files changed, 372 insertions(+), 426 deletions(-) diff --git a/capabilities/src/plan_pick_place_capability.cpp b/capabilities/src/plan_pick_place_capability.cpp index 0f0ffd07e..4ecdced34 100644 --- a/capabilities/src/plan_pick_place_capability.cpp +++ b/capabilities/src/plan_pick_place_capability.cpp @@ -51,17 +51,21 @@ void PlanPickPlaceCapability::initialize() { std::bind(&PlanPickPlaceCapability::goalCallback, this, std::placeholders::_1), false)); as_->registerPreemptCallback(std::bind(&PlanPickPlaceCapability::preemptCallback, this)); as_->start(); - ros::NodeHandle nh("~"); - pick_place_task_ = std::make_unique("", nh); + 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; + // TODO: fill parameters + PickPlaceTask::Parameters parameters; + // initialize task + pick_place_task_->init(parameters); // run plan - // return result + pick_place_task_->plan(); + // retrieve and return result } void PlanPickPlaceCapability::preemptCallback() { diff --git a/core/include/moveit/task_constructor/tasks/pick_place_task.h b/core/include/moveit/task_constructor/tasks/pick_place_task.h index 71a0a32d0..5a50ef380 100644 --- a/core/include/moveit/task_constructor/tasks/pick_place_task.h +++ b/core/include/moveit/task_constructor/tasks/pick_place_task.h @@ -73,55 +73,54 @@ using namespace moveit::task_constructor; class PickPlaceTask { public: - PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh); + struct Parameters + { + // 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_reference_frame_; + std::string surface_link_; + std::string object_name_; + std::string world_frame_; + std::string grasp_frame_; + std::vector object_dimensions_; + + // Predefined pose targets + std::string hand_open_pose_; + std::string hand_close_pose_; + std::string arm_home_pose_; + + // Pick metrics + Eigen::Isometry3d grasp_frame_transform_; + double approach_object_min_dist_; + double approach_object_max_dist_; + double lift_object_min_dist_; + double lift_object_max_dist_; + double place_object_min_dist_; + double place_object_max_dist_; + double retreat_object_min_dist_; + double retreat_object_max_dist_; + + // Place metrics + geometry_msgs::PoseStamped place_pose_; + double place_surface_offset_; + }; + + PickPlaceTask(const std::string& task_name); ~PickPlaceTask() = default; - void loadParameters(); - - void init(); + void init(const Parameters& parameters); bool plan(); - bool execute(); - private: - ros::NodeHandle nh_; - - std::string task_name_; moveit::task_constructor::TaskPtr task_; - - // 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_reference_frame_; - std::string surface_link_; - std::string object_name_; - std::string world_frame_; - std::vector object_dimensions_; - - // Predefined pose targets - std::string hand_open_pose_; - std::string hand_close_pose_; - std::string arm_home_pose_; - - // Execution - actionlib::SimpleActionClient execute_; - - // Pick metrics - Eigen::Isometry3d grasp_frame_transform_; - double approach_object_min_dist_; - double approach_object_max_dist_; - double lift_object_min_dist_; - double lift_object_max_dist_; - - // Place metrics - geometry_msgs::Pose place_pose_; - double place_surface_offset_; + const std::string task_name_; }; } // namespace tasks diff --git a/core/src/tasks/pick_place_task.cpp b/core/src/tasks/pick_place_task.cpp index 99b52aa9e..4400ab47d 100644 --- a/core/src/tasks/pick_place_task.cpp +++ b/core/src/tasks/pick_place_task.cpp @@ -42,390 +42,349 @@ namespace task_constructor { namespace tasks { constexpr char LOGNAME[] = "pick_place_task"; -PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh) - : nh_(nh), task_name_(task_name), execute_("execute_task_solution", true) {} - -void PickPlaceTask::loadParameters() { - /**************************************************** - * * - * Load Parameters * - * * - ***************************************************/ - ROS_INFO_NAMED(LOGNAME, "Loading task parameters"); - ros::NodeHandle pnh("~"); - - // Planning group properties - size_t errors = 0; - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_group_name", arm_group_name_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_group_name", hand_group_name_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "eef_name", eef_name_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_frame", hand_frame_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "world_frame", world_frame_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "grasp_frame_transform", grasp_frame_transform_); - - // Predefined pose targets - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_open_pose", hand_open_pose_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_close_pose", hand_close_pose_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_home_pose", arm_home_pose_); - - // Target object - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "surface_link", surface_link_); - support_surfaces_ = { surface_link_ }; - - // Pick/Place metrics - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_min_dist", approach_object_min_dist_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_max_dist", approach_object_max_dist_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_min_dist", lift_object_min_dist_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_max_dist", lift_object_max_dist_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_surface_offset", place_surface_offset_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_pose", place_pose_); - rosparam_shortcuts::shutdownIfError(LOGNAME, errors); -} - -void PickPlaceTask::init() { - ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); - const std::string object = "object"; - - // Reset ROS introspection before constructing the new object - // TODO(henningkayser): verify this is a bug, fix if possible - task_.reset(); - task_.reset(new moveit::task_constructor::Task()); - - Task& t = *task_; - t.stages()->setName(task_name_); - t.loadRobotModel(); - - // Sampling planner - 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", arm_group_name_); - t.setProperty("eef", eef_name_); - t.setProperty("hand", hand_group_name_); - t.setProperty("hand_grasping_frame", hand_frame_); - t.setProperty("ik_frame", hand_frame_); - - /**************************************************** - * * - * Current State * - * * - ***************************************************/ - Stage* current_state = nullptr; // Forward current_state on to grasp pose generator - { - auto _current_state = std::make_unique("current state"); - - // Verify that object is not attachd - auto applicability_filter = - std::make_unique("applicability test", std::move(_current_state)); - applicability_filter->setPredicate([object](const SolutionBase& s, std::string& comment) { - if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) { - comment = "object with id '" + object + "' is already attached and cannot be picked"; - return false; - } - return true; - }); - - current_state = applicability_filter.get(); - t.add(std::move(applicability_filter)); - } - - /**************************************************** - * * - * Open Hand * - * * - ***************************************************/ - { // Open Hand - auto stage = std::make_unique("open hand", sampling_planner); - stage->setGroup(hand_group_name_); - stage->setGoal(hand_open_pose_); - t.add(std::move(stage)); - } - - /**************************************************** - * * - * Move to Pick * - * * - ***************************************************/ - { // Move-to pre-grasp - auto stage = std::make_unique( - "move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); - stage->setTimeout(5.0); - stage->properties().configureInitFrom(Stage::PARENT); - t.add(std::move(stage)); - } - - /**************************************************** - * * - * Pick Object * - * * - ***************************************************/ - Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator - { - auto grasp = std::make_unique("pick object"); - t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" }); - grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" }); - - /**************************************************** +PickPlaceTask::PickPlaceTask(const std::string& task_name) + : task_name_(task_name) {} + +void PickPlaceTask::init(const Parameters& parameters) +{ + ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); + // Reset ROS introspection before constructing the new object + // TODO(henningkayser): verify this is a bug, fix if possible + task_.reset(); + task_.reset(new moveit::task_constructor::Task(task_name_)); + Task& t = *task_; + t.loadRobotModel(); + + // 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 * + * * + ***************************************************/ + Stage* current_state = nullptr; // Forward current_state on to grasp pose generator + { + auto _current_state = std::make_unique("current state"); + _current_state->setTimeout(10); + + // Verify that object is not attachd + 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 (s.start()->scene()->getCurrentState().hasAttachedBody(parameters.object_name_)) + { + comment = "object with id '" + parameters.object_name_ + "' is already attached and cannot be picked"; + return false; + } + return true; + }); + + current_state = applicability_filter.get(); + t.add(std::move(applicability_filter)); + } + + /**************************************************** + * * + * Open Hand * + * * + ***************************************************/ + { // Open Hand + auto stage = std::make_unique("open hand", sampling_planner); + stage->setGroup(parameters.hand_group_name_); + stage->setGoal(parameters.hand_open_pose_); + t.add(std::move(stage)); + } + + /**************************************************** + * * + * 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 * + * * + ***************************************************/ + Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator + { + auto grasp = std::make_unique("pick object"); + t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" }); + grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" }); + + /**************************************************** ---- * Approach Object * - ***************************************************/ - { - auto stage = std::make_unique("approach object", cartesian_planner); - stage->properties().set("marker_ns", "approach_object"); - stage->properties().set("link", hand_frame_); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_); - - // Set hand forward direction - geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = hand_frame_; - vec.vector.z = 1.0; - stage->setDirection(vec); - grasp->insert(std::move(stage)); - } - - /**************************************************** + ***************************************************/ + { + auto stage = std::make_unique("approach object", cartesian_planner); + stage->properties().set("marker_ns", "approach_object"); + stage->properties().set("link", parameters.hand_frame_); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(parameters.approach_object_min_dist_, parameters.approach_object_max_dist_); + + // Set hand forward direction + geometry_msgs::Vector3Stamped vec; + vec.header.frame_id = parameters.object_name_; + vec.vector.z = -1.0; + stage->setDirection(vec); + grasp->insert(std::move(stage)); + } + + /**************************************************** ---- * Generate Grasp Pose * - ***************************************************/ - { - // Sample grasp pose - auto stage = std::make_unique("generate grasp pose"); - stage->properties().configureInitFrom(Stage::PARENT); - stage->properties().set("marker_ns", "grasp_pose"); - stage->setPreGraspPose(hand_open_pose_); - stage->setObject(object); - stage->setAngleDelta(M_PI / 12); - stage->setMonitoredStage(current_state); // Hook into current state - - // Compute IK - auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); - wrapper->setMaxIKSolutions(8); - wrapper->setMinSolutionDistance(1.0); - wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); - wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); - wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); - grasp->insert(std::move(wrapper)); - } - - /**************************************************** + ***************************************************/ + { + // Sample grasp pose + auto stage = std::make_unique("generate grasp pose"); + stage->properties().configureInitFrom(Stage::PARENT); + stage->properties().set("marker_ns", "grasp_pose"); + stage->setPreGraspPose(parameters.hand_open_pose_); + stage->setObject(parameters.object_name_); + stage->setAngleDelta(M_PI / 12); + stage->setMonitoredStage(current_state); // Hook into current state + + // Compute IK + auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); + wrapper->setMaxIKSolutions(2); + wrapper->setMinSolutionDistance(1.0); + wrapper->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_); + //wrapper->setIgnoreCollisions(true); + wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); + wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); + grasp->insert(std::move(wrapper)); + } + + /**************************************************** ---- * Allow Collision (hand object) * - ***************************************************/ - { - auto stage = std::make_unique("allow collision (hand,object)"); - stage->allowCollisions( - object, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), - true); - grasp->insert(std::move(stage)); - } - - /**************************************************** + ***************************************************/ + { + auto stage = std::make_unique("allow collision (hand,object)"); + stage->allowCollisions( + parameters.object_name_, + t.getRobotModel()->getJointModelGroup(parameters.hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), + true); + grasp->insert(std::move(stage)); + } + + /**************************************************** ---- * Close Hand * - ***************************************************/ - { - auto stage = std::make_unique("close hand", sampling_planner); - stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_group_name_); - stage->setGoal(hand_close_pose_); - grasp->insert(std::move(stage)); - } - - /**************************************************** + ***************************************************/ + { + auto stage = std::make_unique("close hand", sampling_planner); + stage->properties().property("group").configureInitFrom(Stage::PARENT, parameters.hand_group_name_); + stage->setGoal(parameters.hand_close_pose_); + grasp->insert(std::move(stage)); + } + + /**************************************************** .... * Attach Object * - ***************************************************/ - { - auto stage = std::make_unique("attach object"); - stage->attachObject(object, hand_frame_); - attach_object_stage = stage.get(); - grasp->insert(std::move(stage)); - } - - /**************************************************** + ***************************************************/ + { + auto stage = std::make_unique("attach object"); + stage->attachObject(parameters.object_name_, parameters.hand_frame_); + attach_object_stage = stage.get(); + grasp->insert(std::move(stage)); + } + + /**************************************************** .... * Allow collision (object support) * - ***************************************************/ - { - auto stage = std::make_unique("allow collision (object,support)"); - stage->allowCollisions({ object }, support_surfaces_, true); - grasp->insert(std::move(stage)); - } - - /**************************************************** + ***************************************************/ + { + auto stage = std::make_unique("allow collision (object,support)"); + stage->allowCollisions({ parameters.object_name_ }, "source_container", true); // TODO(henningkayser): parameterize + grasp->insert(std::move(stage)); + } + + /**************************************************** .... * Lift object * - ***************************************************/ - { - auto stage = std::make_unique("lift object", cartesian_planner); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setMinMaxDistance(lift_object_min_dist_, lift_object_max_dist_); - stage->setIKFrame(hand_frame_); - stage->properties().set("marker_ns", "lift_object"); - - // Set upward direction - geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = world_frame_; - vec.vector.z = 1.0; - stage->setDirection(vec); - grasp->insert(std::move(stage)); - } - - /**************************************************** + ***************************************************/ + { + auto stage = std::make_unique("lift object", cartesian_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(parameters.lift_object_min_dist_, parameters.lift_object_max_dist_); + stage->setIKFrame(parameters.hand_frame_); + stage->properties().set("marker_ns", "lift_object"); + + // Set upward direction + geometry_msgs::Vector3Stamped vec; + vec.header.frame_id = parameters.world_frame_; + vec.vector.z = 1.0; + stage->setDirection(vec); + grasp->insert(std::move(stage)); + } + + /**************************************************** .... * Forbid collision (object support) * - ***************************************************/ - { - auto stage = std::make_unique("forbid collision (object,surface)"); - stage->allowCollisions({ object }, support_surfaces_, false); - grasp->insert(std::move(stage)); - } - - // Add grasp container to task - t.add(std::move(grasp)); - } - - /****************************************************** - * * - * Move to Place * - * * - *****************************************************/ - { - auto stage = std::make_unique( - "move to place", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); - stage->setTimeout(5.0); - stage->properties().configureInitFrom(Stage::PARENT); - t.add(std::move(stage)); - } - - /****************************************************** - * * - * Place Object * - * * - *****************************************************/ - { - auto place = std::make_unique("place object"); - t.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); - place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" }); - - /****************************************************** + ***************************************************/ + { + auto stage = std::make_unique("forbid collision (object,surface)"); + stage->allowCollisions({ parameters.object_name_ }, "source_container", false); // TODO(henningkayser): parameterize + grasp->insert(std::move(stage)); + } + + // Add grasp container to task + t.add(std::move(grasp)); + } + + /****************************************************** + * * + * 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 * + * * + *****************************************************/ + { + auto place = std::make_unique("place object"); + t.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); + place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" }); + + /****************************************************** ---- * Lower Object * - *****************************************************/ - { - auto stage = std::make_unique("lower object", cartesian_planner); - stage->properties().set("marker_ns", "lower_object"); - stage->properties().set("link", hand_frame_); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setMinMaxDistance(.03, .13); - - // Set downward direction - geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = world_frame_; - vec.vector.z = -1.0; - stage->setDirection(vec); - place->insert(std::move(stage)); - } - - /****************************************************** + *****************************************************/ + { + auto stage = std::make_unique("lower object", cartesian_planner); + stage->properties().set("marker_ns", "lower_object"); + stage->properties().set("link", parameters.hand_frame_); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(parameters.place_object_min_dist_, parameters.place_object_max_dist_); + + // Set downward direction + geometry_msgs::Vector3Stamped vec; + vec.header.frame_id = parameters.world_frame_; + vec.vector.z = -1.0; + stage->setDirection(vec); + place->insert(std::move(stage)); + } + + /****************************************************** ---- * Generate Place Pose * - *****************************************************/ - { - // Generate Place Pose - auto stage = std::make_unique("generate place pose"); - stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" }); - stage->properties().set("marker_ns", "place_pose"); - stage->setObject(object); - - // Set target pose - geometry_msgs::PoseStamped p; - p.header.frame_id = object_reference_frame_; - p.pose = place_pose_; - p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_; - stage->setPose(p); - stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage - - // Compute IK - auto wrapper = std::make_unique("place pose IK", std::move(stage)); - wrapper->setMaxIKSolutions(2); - wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); - wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); - wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); - place->insert(std::move(wrapper)); - } - - /****************************************************** + *****************************************************/ + { + // Generate Place Pose + auto stage = std::make_unique("generate place pose"); + stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" }); + stage->properties().set("marker_ns", "place_pose"); + stage->setObject(parameters.object_name_); + stage->setPose(parameters.place_pose_); + // Hook into attach_object_stage which allows us to use the attached object as IK frame + stage->setMonitoredStage(attach_object_stage); + + // Compute IK + auto wrapper = std::make_unique("place pose IK", std::move(stage)); + wrapper->setMaxIKSolutions(2); + wrapper->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_); + // TODO(henningkayser): Enable collisions + wrapper->setIgnoreCollisions(true); + wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); + wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); + place->insert(std::move(wrapper)); + } + + /****************************************************** ---- * Open Hand * - *****************************************************/ - { - auto stage = std::make_unique("open hand", sampling_planner); - stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_group_name_); - stage->setGoal(hand_open_pose_); - place->insert(std::move(stage)); - } - - /****************************************************** + *****************************************************/ + { + auto stage = std::make_unique("open hand", sampling_planner); + stage->properties().property("group").configureInitFrom(Stage::PARENT, parameters.hand_group_name_); + stage->setGoal(parameters.hand_open_pose_); + place->insert(std::move(stage)); + } + + /****************************************************** ---- * Forbid collision (hand, object) * - *****************************************************/ - { - auto stage = std::make_unique("forbid collision (hand,object)"); - stage->allowCollisions( - object_name_, - t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), false); - place->insert(std::move(stage)); - } - - /****************************************************** + *****************************************************/ + { + // TODO(henningkayser): Forbid collision after retreat? + auto stage = std::make_unique("forbid collision (hand,object)"); + stage->allowCollisions( + parameters.object_name_, + t.getRobotModel()->getJointModelGroup(parameters.hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), + false); + place->insert(std::move(stage)); + } + + /****************************************************** ---- * Detach Object * - *****************************************************/ - { - auto stage = std::make_unique("detach object"); - stage->detachObject(object_name_, hand_frame_); - place->insert(std::move(stage)); - } - - /****************************************************** + *****************************************************/ + { + auto stage = std::make_unique("detach object"); + stage->detachObject(parameters.object_name_, parameters.hand_frame_); + place->insert(std::move(stage)); + } + + /****************************************************** ---- * Retreat Motion * - *****************************************************/ - { - auto stage = std::make_unique("retreat after place", cartesian_planner); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setMinMaxDistance(.12, .25); - stage->setIKFrame(hand_frame_); - stage->properties().set("marker_ns", "retreat"); - geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = hand_frame_; - vec.vector.z = -1.0; - stage->setDirection(vec); - place->insert(std::move(stage)); - } - - // Add place container to task - t.add(std::move(place)); - } - - /****************************************************** - * * - * Move to Home * - * * - *****************************************************/ - { - auto stage = std::make_unique("move home", sampling_planner); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setGoal(arm_home_pose_); - stage->restrictDirection(stages::MoveTo::FORWARD); - t.add(std::move(stage)); - } + *****************************************************/ + { + // TODO(henningkayser): Do we need this if items are dropped? + auto stage = std::make_unique("retreat after place", cartesian_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(parameters.retreat_object_min_dist_, parameters.retreat_object_max_dist_); + stage->setIKFrame(parameters.hand_frame_); + stage->properties().set("marker_ns", "retreat"); + geometry_msgs::Vector3Stamped vec; + vec.header.frame_id = parameters.hand_frame_; + vec.vector.z = -1.0; + stage->setDirection(vec); + place->insert(std::move(stage)); + } + + // Add place container to task + t.add(std::move(place)); + } + + /****************************************************** + * * + * Move to Home * + * * + *****************************************************/ + { + auto stage = std::make_unique("move home", sampling_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setGoal(parameters.arm_home_pose_); + stage->restrictDirection(stages::MoveTo::FORWARD); + t.add(std::move(stage)); + } } bool PickPlaceTask::plan() { ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions"); - ros::NodeHandle pnh("~"); - int planning_attempts = pnh.param("planning_attempts", 10); - try { - task_->plan(planning_attempts); + task_->plan(10); // TODO: parameterize } catch (InitStageException& e) { ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e); return false; @@ -437,22 +396,6 @@ bool PickPlaceTask::plan() { return true; } -bool PickPlaceTask::execute() { - ROS_INFO_NAMED(LOGNAME, "Executing solution trajectory"); - moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal; - task_->solutions().front()->fillMessage(execute_goal.solution); - execute_.sendGoal(execute_goal); - execute_.waitForResult(); - moveit_msgs::MoveItErrorCodes execute_result = execute_.getResult()->error_code; - - if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed and returned: " << execute_.getState().toString()); - return false; - } - - return true; -} - } // namespace tasks } // namespace task_constructor } // namespace moveit