From ad6eb48d28f189c728533def8afec6b28c9d2b27 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 20 Nov 2019 16:43:08 +0100 Subject: [PATCH 01/23] 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 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..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 30079e1dcf4f6cf6e6b3d20efe7efe0ad2259c26 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 20 Nov 2019 16:43:29 +0100 Subject: [PATCH 02/23] 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 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/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 638a253816f0a1558dbaad065894adcadba23d53 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 20 Nov 2019 16:43:52 +0100 Subject: [PATCH 03/23] 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 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..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 52d8d217fcae4795589575e18cc2cf029152a6a9 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 20 Nov 2019 17:32:48 +0100 Subject: [PATCH 04/23] 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 From 733401cc15e37a86f9f906a0a58327453e5753f3 Mon Sep 17 00:00:00 2001 From: Artur Istvan Karoly Date: Thu, 7 Jan 2021 09:58:05 +0100 Subject: [PATCH 05/23] Add GraspProvider and PlaceProvider plugins --- .../task_constructor/stages/grasp_provider.h | 82 +++++++ .../task_constructor/stages/place_provider.h | 71 +++++++ ...ion_planning_stages_plugin_description.xml | 16 ++ core/src/stages/CMakeLists.txt | 4 + core/src/stages/grasp_provider.cpp | 201 ++++++++++++++++++ core/src/stages/place_provider.cpp | 176 +++++++++++++++ 6 files changed, 550 insertions(+) create mode 100644 core/include/moveit/task_constructor/stages/grasp_provider.h create mode 100644 core/include/moveit/task_constructor/stages/place_provider.h create mode 100644 core/src/stages/grasp_provider.cpp create mode 100644 core/src/stages/place_provider.cpp 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..39eb32a48 --- /dev/null +++ b/core/include/moveit/task_constructor/stages/grasp_provider.h @@ -0,0 +1,82 @@ +/********************************************************************* + * 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: Base class for grasp provider plaugins and default plugin +*/ + +#pragma once + +#include + +namespace moveit { +namespace task_constructor { +namespace stages { + +/// Base class for Grasp Provider Plugins + +class GraspProviderBase : public GeneratePose +{ +public: + GraspProviderBase(const std::string& name = "grasp provider"); + + void init(const core::RobotModelConstPtr& 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; +}; + + +/// 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; +}; + +} // 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..44de1066c --- /dev/null +++ b/core/include/moveit/task_constructor/stages/place_provider.h @@ -0,0 +1,71 @@ +/********************************************************************* + * 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: Base class for place provider plaugins and default plugin +*/ + +#pragma once + +#include + +namespace moveit { +namespace task_constructor { +namespace stages { + +/// Base class for Place Provider Plugins + +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; +}; + +/// 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; +}; +} // namespace stages +} // 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..4cd0540bd 100644 --- a/core/motion_planning_stages_plugin_description.xml +++ b/core/motion_planning_stages_plugin_description.xml @@ -6,4 +6,20 @@ Use the current state of the robot (when starting planning) as a target. + + + + Default Grasp provider stage plugin implementing the functionality of the GenerateGraspPose stage + + + + + + Default Place provider stage plugin implementing the functionality of the GeneratePlacePose stage + + diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 857a4977c..82962d80f 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -8,6 +8,8 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/generate_pose.h ${PROJECT_INCLUDE}/stages/generate_grasp_pose.h ${PROJECT_INCLUDE}/stages/generate_place_pose.h + ${PROJECT_INCLUDE}/stages/grasp_provider.h + ${PROJECT_INCLUDE}/stages/place_provider.h ${PROJECT_INCLUDE}/stages/compute_ik.h ${PROJECT_INCLUDE}/stages/passthrough.h ${PROJECT_INCLUDE}/stages/predicate_filter.h @@ -28,6 +30,8 @@ add_library(${PROJECT_NAME}_stages generate_pose.cpp generate_grasp_pose.cpp generate_place_pose.cpp + grasp_provider.cpp + place_provider.cpp compute_ik.cpp passthrough.cpp predicate_filter.cpp diff --git a/core/src/stages/grasp_provider.cpp b/core/src/stages/grasp_provider.cpp new file mode 100644 index 000000000..278100301 --- /dev/null +++ b/core/src/stages/grasp_provider.cpp @@ -0,0 +1,201 @@ +/********************************************************************* + * 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 + +namespace moveit { +namespace task_constructor { +namespace stages { + +// ------------------- +// GraspProviderBase +// ------------------- + +GraspProviderBase::GraspProviderBase(const std::string& name) : GeneratePose(name) { + auto& p = properties(); + p.declare("eef", "name of end-effector"); + p.declare("object"); + + p.declare("pregrasp", "pregrasp posture"); + p.declare("grasp", "grasp posture"); +} + +void GraspProviderBase::init(const core::RobotModelConstPtr& 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); + else { + // check availability of eef pose + const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef); + const std::string& name = props.get("pregrasp"); + std::map m; + if (!jmg->getVariableDefaultPositions(name, m)) + errors.push_back(*this, "unknown end effector pose: " + name); + } + + if (errors) + throw errors; +} + +void GraspProviderBase::onNewSolution(const SolutionBase& s) { + planning_scene::PlanningSceneConstPtr 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); +} + + +// ------------------- +// 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 { + // check availability of eef pose + const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef); + const std::string& name = props.get("pregrasp"); + std::map m; + if (!jmg->getVariableDefaultPositions(name, m)) + errors.push_back(*this, "unknown end effector pose: " + name); + } + + if (errors) + throw errors; +} + +void 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::string& eef = props.get("eef"); + const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef); + + robot_state::RobotState& robot_state = scene->getCurrentStateNonConst(); + robot_state.setToDefaultValues(jmg, props.get("pregrasp")); + + 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); + props.exposeTo(state.properties(), { "pregrasp", "grasp" }); + + 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)); + } +} + +} // namespace stages +} // namespace task_constructor +} // namespace moveit + +/// register plugin +#include +PLUGINLIB_EXPORT_CLASS(moveit::task_constructor::stages::GraspProviderDefault, moveit::task_constructor::stages::GraspProviderBase) diff --git a/core/src/stages/place_provider.cpp b/core/src/stages/place_provider.cpp new file mode 100644 index 000000000..7e370a4a7 --- /dev/null +++ b/core/src/stages/place_provider.cpp @@ -0,0 +1,176 @@ +/********************************************************************* + * 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 + +namespace moveit { +namespace task_constructor { +namespace stages { + +// ------------------- +// PlaceProviderBase +// ------------------- + +PlaceProviderBase::PlaceProviderBase(const std::string& name) : GeneratePose(name) { + auto& p = properties(); + p.declare("object"); + p.declare("ik_frame"); +} + +void PlaceProviderBase::onNewSolution(const SolutionBase& s) { + planning_scene::PlanningSceneConstPtr 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); +} + +// ------------------- +// 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 geometry_msgs::PoseStamped& pose_msg = props.get("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, 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); + + 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); +} +} // namespace stages +} // namespace task_constructor +} // namespace moveit + +/// register plugin +#include +PLUGINLIB_EXPORT_CLASS(moveit::task_constructor::stages::PlaceProviderDefault, moveit::task_constructor::stages::PlaceProviderBase) From 2f4fb16e99ac1dde4ecc409322c4ebeed16a484d Mon Sep 17 00:00:00 2001 From: Artur Istvan Karoly Date: Thu, 7 Jan 2021 10:00:26 +0100 Subject: [PATCH 06/23] Update pickplace container --- .../moveit/task_constructor/stages/pick.h | 149 +++++++++-- core/src/stages/pick.cpp | 243 +++++++++++++++--- 2 files changed, 333 insertions(+), 59 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/pick.h b/core/include/moveit/task_constructor/stages/pick.h index 34d9f6ec1..e6be83f9e 100644 --- a/core/include/moveit/task_constructor/stages/pick.h +++ b/core/include/moveit/task_constructor/stages/pick.h @@ -32,19 +32,27 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Robert Haschke */ +/* Authors: Robert Haschke, Artur Karoly */ #pragma once +#include #include #include +#include +#include +#include #include +#include +#include +#include namespace moveit { namespace task_constructor { namespace solvers { MOVEIT_CLASS_FORWARD(CartesianPath) +MOVEIT_CLASS_FORWARD(PipelinePlanner) } namespace stages { @@ -53,73 +61,166 @@ namespace stages { * * Picking consist of the following sub stages: * - linearly approaching the object along an approach direction/twist - * - "grasp" end effector posture + * - 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 - * - * 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 { + + bool is_pick_; + solvers::CartesianPathPtr cartesian_solver_; - Stage* grasp_stage_ = nullptr; - Stage* approach_stage_ = nullptr; - Stage* lift_stage_ = nullptr; + solvers::PipelinePlannerPtr sampling_planner_; + + Stage* move_there_stage_ = nullptr; + Stage* compute_ik_stage_ = nullptr; + moveit::task_constructor::stages::ModifyPlanningScene* set_collision_object_hand_stage_ = nullptr; + moveit::task_constructor::stages::ModifyPlanningScene* allow_collision_object_support_stage_ = nullptr; + moveit::task_constructor::stages::ModifyPlanningScene* forbid_collision_object_support_stage_ = nullptr; + Stage* move_back_stage_ = nullptr; + + std::string provider_stage_plugin_name_; + + pluginlib::ClassLoader grasp_provider_class_loader_; + pluginlib::ClassLoader place_provider_class_loader_; + +protected: + moveit::task_constructor::stages::GraspProviderBase* grasp_stage_ = nullptr; + moveit::task_constructor::stages::PlaceProviderBase* place_stage_ = nullptr; + moveit::task_constructor::stages::ModifyPlanningScene* attach_detach_stage_ = nullptr; public: - PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& name, bool forward); + PickPlaceBase(const std::string& name, const std::string& provider_stage_plugin_name, bool is_pick); 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 setObject(const std::string& object) { properties().set("object", object); } + 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); + } - solvers::CartesianPathPtr cartesianSolver() { return cartesian_solver_; } + /// 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 + + /// approach / place + void setApproachPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance); + void setApproachPlace(const geometry_msgs::Vector3Stamped& direction, double min_distance, double max_distance); + void setApproachPlace(const std::map& joints); + + /// 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); + + /// lift / retract + void setLiftRetract(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance); + void setLiftRetract(const geometry_msgs::Vector3Stamped& direction, double min_distance, double max_distance); + void setLiftRetract(const std::map& joints); - void setApproachRetract(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance); + /// ------------------------- + /// getters, for further configuration - void setLiftPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance); - void setLiftPlace(const std::map& joints); + 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 + moveit::task_constructor::Stage* GraspProviderPlugin() {return grasp_stage_;} }; /// 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) {} + Pick(const std::string& name = "pick", const std::string& provider_stage_plugin_name = "moveit_task_constructor/GraspProviderDefault") + : PickPlaceBase(name, provider_stage_plugin_name, true) {} + + void setMonitoredStage(Stage* monitored); void setApproachMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { - setApproachRetract(motion, min_distance, max_distance); + setApproachPlace(motion, min_distance, max_distance); } + void setApproachMotion(const geometry_msgs::Vector3Stamped& direction, double min_distance, double max_distance) { + setApproachPlace(direction, min_distance, max_distance); + } + + void setApproachMotion(const std::map& joints) { setApproachPlace(joints); } + void setLiftMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { - setLiftPlace(motion, min_distance, max_distance); + setLiftRetract(motion, min_distance, max_distance); } - void setLiftMotion(const std::map& joints) { setLiftPlace(joints); } + + void setLiftMotion(const geometry_msgs::Vector3Stamped& direction, double min_distance, double max_distance) { + setLiftRetract(direction, min_distance, max_distance); + } + + void setLiftMotion(const std::map& joints) { setLiftRetract(joints); } + + moveit::task_constructor::Stage* attachStage() {return attach_detach_stage_;} }; /// 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) {} + Place(const std::string& name = "place", const std::string& provider_stage_plugin_name = "moveit_task_constructor/PlaceProviderDefault") + : PickPlaceBase(name, provider_stage_plugin_name, false) {} + + void setMonitoredStage(Stage* monitored); + + void setPlacePose(const geometry_msgs::PoseStamped& pose); void setRetractMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { - setApproachRetract(motion, min_distance, max_distance); + setLiftRetract(motion, min_distance, max_distance); + } + + void setRetractMotion(const geometry_msgs::Vector3Stamped& direction, double min_distance, double max_distance) { + setLiftRetract(direction, min_distance, max_distance); } + void setRetractMotion(const std::map& joints) { setLiftRetract(joints); } + void setPlaceMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { - setLiftPlace(motion, min_distance, max_distance); + setApproachPlace(motion, min_distance, max_distance); + } + void setPlaceMotion(const geometry_msgs::Vector3Stamped& direction, double min_distance, double max_distance) { + setApproachPlace(direction, min_distance, max_distance); } - void setPlaceMotion(const std::map& joints) { setLiftPlace(joints); } + + void setPlaceMotion(const std::map& joints) { setApproachPlace(joints); } + + moveit::task_constructor::Stage* detachStage() {return attach_detach_stage_;} }; } // namespace stages } // namespace task_constructor diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index d289f2d4f..fe11df0b5 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -1,63 +1,143 @@ #include #include +#include #include #include +#include +#include +#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) { +PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provider_stage_plugin_name, bool is_pick) + : SerialContainer(name), + grasp_provider_class_loader_("moveit_task_constructor_core", "moveit::task_constructor::stages::GraspProviderBase"), + place_provider_class_loader_("moveit_task_constructor_core", "moveit::task_constructor::stages::PlaceProviderBase") { PropertyMap& p = properties(); - p.declare("object", "name of object to grasp"); + p.declare("object", "name of object to grasp/place"); p.declare("eef", "end effector name"); - p.declare("eef_frame", "name of end effector frame"); + p.declare("ik_frame", "frame to be moved towards goal pose"); + p.declare("eef_group_open_pose", "Name of open pose of eef_group"); + p.declare("eef_group_close_pose", "Name of close pose of eef_group"); + 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(); - int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order + sampling_planner_ = std::make_shared(); - 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(); + is_pick_ = is_pick; - pose.header.frame_id = boost::any_cast(frame); - pose.pose.orientation.w = 1.0; - return pose; - }; + provider_stage_plugin_name_ = provider_stage_plugin_name; { - auto approach = std::make_unique(forward ? "approach object" : "retract", cartesian_solver_); - PropertyMap& p = approach->properties(); + 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, init_ik_frame); - p.set("marker_ns", std::string(forward ? "approach" : "retract")); - approach_stage_ = approach.get(); - insert(std::move(approach), insertion_position); + p.property("ik_frame").configureInitFrom(Stage::PARENT, "ik_frame"); + p.set("marker_ns", std::string(is_pick_ ? "approach" : "place")); + 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(grasp_provider_class_loader_.createUnmanagedInstance(provider_stage_plugin_name_)); + provider_stage_plugin->properties().configureInitFrom(Stage::PARENT); + provider_stage_plugin->properties().property("pregrasp").configureInitFrom(Stage::PARENT, "eef_group_open_pose"); + provider_stage_plugin->properties().property("grasp").configureInitFrom(Stage::PARENT, "eef_group_close_pose"); + provider_stage_plugin->properties().set("marker_ns", "grasp_pose"); + grasp_stage_ = provider_stage_plugin.get(); + + wrapper = std::make_unique("grasp pose IK", std::move(provider_stage_plugin)); + } + else { + std::unique_ptr provider_stage_plugin(place_provider_class_loader_.createUnmanagedInstance(provider_stage_plugin_name_)); + provider_stage_plugin->properties().configureInitFrom(Stage::PARENT); + provider_stage_plugin->properties().set("marker_ns", "place_pose"); + place_stage_ = provider_stage_plugin.get(); + + wrapper = std::make_unique("place pose IK", std::move(provider_stage_plugin)); + } + + properties().exposeTo(wrapper->properties(), {"object", "eef_group_open_pose", "eef_group_close_pose"}); + wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group", "ik_frame", "object", "eef_group_open_pose", "eef_group_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_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::PARENT, is_pick_ ? "eef_group_close_pose" : "eef_group_open_pose"); + if (is_pick_) + insert(std::move(open_close_hand)); + else + insert(std::move(open_close_hand), -2); } - grasp_stage_ = grasp_stage.get(); - grasp_stage->properties().configureInitFrom(Stage::PARENT, { "eef", "object" }); - insert(std::move(grasp_stage), insertion_position); + { + auto attach_detach = std::make_unique(is_pick_ ? "attach object" : "detach object"); + 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)"); + allow_collision_object_support_stage_ = set_collision_object_support.get(); + insert(std::move(set_collision_object_support)); + } { - auto lift = std::make_unique(forward ? "lift object" : "place object", cartesian_solver_); - PropertyMap& p = lift->properties(); + 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, init_ik_frame); - p.set("marker_ns", std::string(forward ? "lift" : "place")); - lift_stage_ = lift.get(); - insert(std::move(lift), insertion_position); + p.property("ik_frame").configureInitFrom(Stage::PARENT, "ik_frame"); + p.set("marker_ns", std::string(is_pick_ ? "lift" : "retract")); + 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)); } } @@ -75,29 +155,122 @@ void PickPlaceBase::init(const moveit::core::RobotModelConstPtr& robot_model) { 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); } -void PickPlaceBase::setApproachRetract(const geometry_msgs::TwistStamped& motion, double min_distance, + +/// ------------------------- +/// setters of own properties + +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 + + +/// approach / place + +void PickPlaceBase::setApproachPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { - auto& p = approach_stage_->properties(); + auto& p = move_there_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(); +void PickPlaceBase::setApproachPlace(const geometry_msgs::Vector3Stamped& direction, double min_distance, + double max_distance) { + auto& p = move_there_stage_->properties(); + p.set("direction", direction); + p.set("min_distance", min_distance); + p.set("max_distance", max_distance); +} + +void PickPlaceBase::setApproachPlace(const std::map& joints) { + auto& p = move_there_stage_->properties(); + p.set("joints", joints); +} + +/// IK computation + +void PickPlaceBase::setMaxIKSolutions(const uint32_t& max_ik_solutions) { + auto& p = compute_ik_stage_->properties(); + p.set("max_ik_solutions", max_ik_solutions); +} + +void PickPlaceBase::setMinIKSolutionDistance(const double& min_ik_solution_distance) { + auto& p = compute_ik_stage_->properties(); + p.set("min_solution_distance", min_ik_solution_distance); +} + +void PickPlaceBase::setIgnoreIKCollisions(const bool& ignore_ik_collisions) { + auto& p = compute_ik_stage_->properties(); + p.set("ignore_collisions", ignore_ik_collisions); +} + + +/// lift / retract + +void PickPlaceBase::setLiftRetract(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { + auto& p = move_back_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(); +void PickPlaceBase::setLiftRetract(const geometry_msgs::Vector3Stamped& direction, double min_distance, double max_distance) { + auto& p = move_back_stage_->properties(); + p.set("direction", direction); + p.set("min_distance", min_distance); + p.set("max_distance", max_distance); +} + +void PickPlaceBase::setLiftRetract(const std::map& joints) { + auto& p = move_back_stage_->properties(); p.set("joints", joints); } + + +/// ------------------------- +/// setters of pick properties + +void Pick::setMonitoredStage(Stage* monitored) { + grasp_stage_->setMonitoredStage(monitored); +} + + +/// ------------------------- +/// setters of place properties + +void Place::setMonitoredStage(Stage* monitored) { + place_stage_->setMonitoredStage(monitored); +} + +void Place::setPlacePose(const geometry_msgs::PoseStamped& pose) { + place_stage_->setPose(pose); +} + } // namespace stages } // namespace task_constructor } // namespace moveit From 59c6514baaa9f57582e021bf53a14678bb8bf7ae Mon Sep 17 00:00:00 2001 From: Artur Istvan Karoly Date: Thu, 7 Jan 2021 18:28:19 +0100 Subject: [PATCH 07/23] Update capability --- .../src/plan_pick_place_capability.cpp | 19 ++ .../task_constructor/tasks/pick_place_task.h | 19 +- core/src/tasks/pick_place_task.cpp | 282 +++--------------- msgs/action/PlanPickPlace.action | 18 +- 4 files changed, 90 insertions(+), 248 deletions(-) diff --git a/capabilities/src/plan_pick_place_capability.cpp b/capabilities/src/plan_pick_place_capability.cpp index 4ecdced34..f337ef5e5 100644 --- a/capabilities/src/plan_pick_place_capability.cpp +++ b/capabilities/src/plan_pick_place_capability.cpp @@ -60,12 +60,31 @@ void PlanPickPlaceCapability::goalCallback( // TODO: fill parameters PickPlaceTask::Parameters parameters; + 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.grasp_provider_plugin_name_ = goal->grasp_provider_plugin_name; + tf::poseMsgToEigen(goal->grasp_frame_transform, parameters.grasp_frame_transform_); + parameters.hand_open_pose_ = "open"; + parameters.hand_close_pose_ = "close"; + parameters.approach_object_direction_ = goal->grasp.pre_grasp_approach.direction; + parameters.approach_object_min_dist_ = goal->grasp.pre_grasp_approach.min_distance; + parameters.approach_object_max_dist_ = goal->grasp.pre_grasp_approach.desired_distance; + parameters.lift_object_direction_ = goal->grasp.post_grasp_retreat.direction; + parameters.lift_object_min_dist_ = goal->grasp.post_grasp_retreat.min_distance; + parameters.lift_object_max_dist_ = goal->grasp.post_grasp_retreat.desired_distance; // initialize task pick_place_task_->init(parameters); + // run plan pick_place_task_->plan(); + // retrieve and return result + as_->setSucceeded(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 5a50ef380..af575c8bf 100644 --- a/core/include/moveit/task_constructor/tasks/pick_place_task.h +++ b/core/include/moveit/task_constructor/tasks/pick_place_task.h @@ -53,6 +53,7 @@ #include #include #include +#include #include #include #include @@ -62,6 +63,7 @@ #include #include +#include #pragma once @@ -83,22 +85,27 @@ class PickPlaceTask // object + surface std::vector support_surfaces_; - std::string object_reference_frame_; - std::string surface_link_; + // 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_; + // 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_; + // std::string arm_home_pose_; + + // Plugins + std::string grasp_provider_plugin_name_; // Pick metrics Eigen::Isometry3d grasp_frame_transform_; + geometry_msgs::Vector3Stamped approach_object_direction_; double approach_object_min_dist_; double approach_object_max_dist_; + geometry_msgs::Vector3Stamped lift_object_direction_; double lift_object_min_dist_; double lift_object_max_dist_; double place_object_min_dist_; diff --git a/core/src/tasks/pick_place_task.cpp b/core/src/tasks/pick_place_task.cpp index 4400ab47d..7eba36660 100644 --- a/core/src/tasks/pick_place_task.cpp +++ b/core/src/tasks/pick_place_task.cpp @@ -131,254 +131,54 @@ void PickPlaceTask::init(const Parameters& parameters) ***************************************************/ 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", 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(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( - 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, 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(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({ 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(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({ 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); + auto stage = std::make_unique("Pick object", parameters.grasp_provider_plugin_name_); + 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->setEndEffectorOpenClose(parameters.hand_open_pose_, parameters.hand_close_pose_); + stage->setSupportSurfaces(parameters.support_surfaces_); + stage->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_); + stage->GraspProviderPlugin()->properties().set("angle_delta", M_PI / 12); // Set plugin-specific properties + stage->setMonitoredStage(current_state); + stage->setApproachMotion(parameters.approach_object_direction_,parameters.approach_object_min_dist_, parameters.approach_object_max_dist_); + stage->setLiftMotion(parameters.lift_object_direction_, parameters.lift_object_min_dist_, parameters.lift_object_max_dist_); + attach_object_stage = stage->attachStage(); 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", 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(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, parameters.hand_group_name_); - stage->setGoal(parameters.hand_open_pose_); - place->insert(std::move(stage)); - } - - /****************************************************** - ---- * Forbid collision (hand, object) * - *****************************************************/ - { - // 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(parameters.object_name_, parameters.hand_frame_); - place->insert(std::move(stage)); - } - - /****************************************************** - ---- * Retreat Motion * - *****************************************************/ - { - // 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 * + * Move to Place * * * *****************************************************/ - { - 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)); - } + // { + // 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 * + // * * + // *****************************************************/ + + + // /****************************************************** + // * * + // * 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() { diff --git a/msgs/action/PlanPickPlace.action b/msgs/action/PlanPickPlace.action index 5acfdb21a..87865e7ec 100644 --- a/msgs/action/PlanPickPlace.action +++ b/msgs/action/PlanPickPlace.action @@ -1,7 +1,23 @@ # goal + +# planning group properties +string arm_group_name +string hand_group_name + +# robot model and links +string eef_name +string hand_frame + +# object and surfaces string object_id -string end_effector +string[] support_surfaces + +# picking +string grasp_provider_plugin_name +moveit_msgs/Grasp grasp +geometry_msgs/Pose grasp_frame_transform +# placing moveit_msgs/PlaceLocation place_location --- From ffd803694574c1273c9aa8c6d788a3d711533d8a Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Fri, 26 Mar 2021 18:53:13 +0900 Subject: [PATCH 08/23] Fix build errors (comment out test) --- core/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 7032b57dc..39ab4e9c9 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 ) @@ -43,7 +44,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) From dde84e6edc4213207ffb3b31cf0f852d49defe7f Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Fri, 26 Mar 2021 20:01:34 +0900 Subject: [PATCH 09/23] Typo, formatting --- capabilities/src/plan_pick_place_capability.cpp | 8 ++++---- core/src/tasks/pick_place_task.cpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/capabilities/src/plan_pick_place_capability.cpp b/capabilities/src/plan_pick_place_capability.cpp index f337ef5e5..9c81a7bef 100644 --- a/capabilities/src/plan_pick_place_capability.cpp +++ b/capabilities/src/plan_pick_place_capability.cpp @@ -45,7 +45,7 @@ namespace move_group { PlanPickPlaceCapability::PlanPickPlaceCapability() : MoveGroupCapability("PlanPickPlace") {} void PlanPickPlaceCapability::initialize() { - // configure the action server + // Configure the action server as_.reset(new actionlib::SimpleActionServer( root_node_handle_, "plan_pick_place", std::bind(&PlanPickPlaceCapability::goalCallback, this, std::placeholders::_1), false)); @@ -77,13 +77,13 @@ void PlanPickPlaceCapability::goalCallback( parameters.lift_object_min_dist_ = goal->grasp.post_grasp_retreat.min_distance; parameters.lift_object_max_dist_ = goal->grasp.post_grasp_retreat.desired_distance; - // initialize task + // Initialize task pick_place_task_->init(parameters); - // run plan + // Run plan pick_place_task_->plan(); - // retrieve and return result + // Retrieve and return result as_->setSucceeded(result); } diff --git a/core/src/tasks/pick_place_task.cpp b/core/src/tasks/pick_place_task.cpp index 7eba36660..f7eab5b16 100644 --- a/core/src/tasks/pick_place_task.cpp +++ b/core/src/tasks/pick_place_task.cpp @@ -82,7 +82,7 @@ void PickPlaceTask::init(const Parameters& parameters) auto _current_state = std::make_unique("current state"); _current_state->setTimeout(10); - // Verify that object is not attachd + // Verify that object is not attached auto applicability_filter = std::make_unique("applicability test", std::move(_current_state)); applicability_filter->setPredicate([&](const SolutionBase& s, std::string& comment) { From 27acd22767ad51f442c0b77871e96d911dd2c58f Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Fri, 26 Mar 2021 21:30:24 +0900 Subject: [PATCH 10/23] Fix linker error --- core/CMakeLists.txt | 1 + core/src/tasks/CMakeLists.txt | 1 + 2 files changed, 2 insertions(+) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 39ab4e9c9..a789dc917 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -19,6 +19,7 @@ catkin_package( LIBRARIES ${PROJECT_NAME} ${PROJECT_NAME}_stages + ${PROJECT_NAME}_tasks INCLUDE_DIRS include CATKIN_DEPENDS diff --git a/core/src/tasks/CMakeLists.txt b/core/src/tasks/CMakeLists.txt index ef0d55285..7dedaf092 100644 --- a/core/src/tasks/CMakeLists.txt +++ b/core/src/tasks/CMakeLists.txt @@ -3,6 +3,7 @@ add_library(${PROJECT_NAME}_tasks pick_place_task.cpp ) target_link_libraries(${PROJECT_NAME}_tasks ${PROJECT_NAME} ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME}_tasks ${PROJECT_NAME}_stages ${catkin_LIBRARIES}) install(TARGETS ${PROJECT_NAME}_tasks ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} From 2e8d588842667e5f556507e954ca165ea3b7ab03 Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Fri, 26 Mar 2021 23:53:24 +0900 Subject: [PATCH 11/23] Add note about action definition --- msgs/notes_from_moveit_msgs.action | 95 ++++++++++++++++++++++++++++++ 1 file changed, 95 insertions(+) create mode 100644 msgs/notes_from_moveit_msgs.action 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 From c71ab863d4e04d65d5be41baba3152e5af95c22e Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Sat, 27 Mar 2021 03:59:24 +0300 Subject: [PATCH 12/23] Seperate base from derived classes for pluginlibs --- .../task_constructor/stages/grasp_provider.h | 21 +---- .../stages/grasp_provider_base.h | 41 ++++++++++ .../moveit/task_constructor/stages/pick.h | 2 + .../task_constructor/stages/place_provider.h | 12 +-- .../stages/place_provider_base.h | 22 ++++++ ...ion_planning_stages_plugin_description.xml | 2 + core/src/stages/CMakeLists.txt | 22 +++++- core/src/stages/grasp_provider.cpp | 60 --------------- core/src/stages/grasp_provider_base.cpp | 76 +++++++++++++++++++ core/src/stages/pick.cpp | 2 + core/src/stages/place_provider.cpp | 31 -------- core/src/stages/place_provider_base.cpp | 50 ++++++++++++ core/src/tasks/CMakeLists.txt | 3 +- 13 files changed, 216 insertions(+), 128 deletions(-) create mode 100644 core/include/moveit/task_constructor/stages/grasp_provider_base.h create mode 100644 core/include/moveit/task_constructor/stages/place_provider_base.h create mode 100644 core/src/stages/grasp_provider_base.cpp create mode 100644 core/src/stages/place_provider_base.cpp diff --git a/core/include/moveit/task_constructor/stages/grasp_provider.h b/core/include/moveit/task_constructor/stages/grasp_provider.h index 39eb32a48..3728a465d 100644 --- a/core/include/moveit/task_constructor/stages/grasp_provider.h +++ b/core/include/moveit/task_constructor/stages/grasp_provider.h @@ -39,6 +39,7 @@ #pragma once #include +#include "grasp_provider_base.h" namespace moveit { namespace task_constructor { @@ -46,26 +47,6 @@ namespace stages { /// Base class for Grasp Provider Plugins -class GraspProviderBase : public GeneratePose -{ -public: - GraspProviderBase(const std::string& name = "grasp provider"); - - void init(const core::RobotModelConstPtr& 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; -}; - - /// Default Grasp Provider plugin implementing the functionality of the GenerateGraspPose stage class GraspProviderDefault : public GraspProviderBase 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..d71b151f6 --- /dev/null +++ b/core/include/moveit/task_constructor/stages/grasp_provider_base.h @@ -0,0 +1,41 @@ +// +// Created by jafar_abdi on 3/27/21. +// + +#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 index e6be83f9e..91d7a3425 100644 --- a/core/include/moveit/task_constructor/stages/pick.h +++ b/core/include/moveit/task_constructor/stages/pick.h @@ -46,6 +46,8 @@ #include #include #include +#include "grasp_provider_base.h" +#include "place_provider_base.h" namespace moveit { namespace task_constructor { diff --git a/core/include/moveit/task_constructor/stages/place_provider.h b/core/include/moveit/task_constructor/stages/place_provider.h index 44de1066c..f15d40d4f 100644 --- a/core/include/moveit/task_constructor/stages/place_provider.h +++ b/core/include/moveit/task_constructor/stages/place_provider.h @@ -39,6 +39,7 @@ #pragma once #include +#include "place_provider_base.h" namespace moveit { namespace task_constructor { @@ -46,17 +47,6 @@ namespace stages { /// Base class for Place Provider Plugins -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; -}; - /// Default Place Provider plugin implementing the functionality of the GeneratePlacePose stage class PlaceProviderDefault : public PlaceProviderBase 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..cd1090d93 --- /dev/null +++ b/core/include/moveit/task_constructor/stages/place_provider_base.h @@ -0,0 +1,22 @@ +// +// Created by jafar_abdi on 3/27/21. +// + +#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/motion_planning_stages_plugin_description.xml b/core/motion_planning_stages_plugin_description.xml index 4cd0540bd..606373a41 100644 --- a/core/motion_planning_stages_plugin_description.xml +++ b/core/motion_planning_stages_plugin_description.xml @@ -6,7 +6,9 @@ Use the current state of the robot (when starting planning) as a target. + + diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 82962d80f..247797b39 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -8,8 +8,6 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/generate_pose.h ${PROJECT_INCLUDE}/stages/generate_grasp_pose.h ${PROJECT_INCLUDE}/stages/generate_place_pose.h - ${PROJECT_INCLUDE}/stages/grasp_provider.h - ${PROJECT_INCLUDE}/stages/place_provider.h ${PROJECT_INCLUDE}/stages/compute_ik.h ${PROJECT_INCLUDE}/stages/passthrough.h ${PROJECT_INCLUDE}/stages/predicate_filter.h @@ -27,11 +25,13 @@ 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 - grasp_provider.cpp - place_provider.cpp compute_ik.cpp passthrough.cpp predicate_filter.cpp @@ -48,3 +48,17 @@ 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 index 278100301..71ec69a00 100644 --- a/core/src/stages/grasp_provider.cpp +++ b/core/src/stages/grasp_provider.cpp @@ -52,66 +52,6 @@ namespace stages { // GraspProviderBase // ------------------- -GraspProviderBase::GraspProviderBase(const std::string& name) : GeneratePose(name) { - auto& p = properties(); - p.declare("eef", "name of end-effector"); - p.declare("object"); - - p.declare("pregrasp", "pregrasp posture"); - p.declare("grasp", "grasp posture"); -} - -void GraspProviderBase::init(const core::RobotModelConstPtr& 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); - else { - // check availability of eef pose - const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef); - const std::string& name = props.get("pregrasp"); - std::map m; - if (!jmg->getVariableDefaultPositions(name, m)) - errors.push_back(*this, "unknown end effector pose: " + name); - } - - if (errors) - throw errors; -} - -void GraspProviderBase::onNewSolution(const SolutionBase& s) { - planning_scene::PlanningSceneConstPtr 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); -} - - // ------------------- // GraspProviderDefault // ------------------- diff --git a/core/src/stages/grasp_provider_base.cpp b/core/src/stages/grasp_provider_base.cpp new file mode 100644 index 000000000..2d5bac11c --- /dev/null +++ b/core/src/stages/grasp_provider_base.cpp @@ -0,0 +1,76 @@ +// +// Created by jafar_abdi on 3/27/21. +// + +#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("pregrasp", "pregrasp posture"); + p.declare("grasp", "grasp posture"); +} +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); + else { + // check availability of eef pose + const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef); + const std::string& name = props.get("pregrasp"); + std::map m; + if (!jmg->getVariableDefaultPositions(name, m)) + errors.push_back(*this, "unknown end effector pose: " + name); + } + + if (errors) + throw errors; +} +void 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 index fe11df0b5..e68da136d 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -14,6 +14,8 @@ #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 { diff --git a/core/src/stages/place_provider.cpp b/core/src/stages/place_provider.cpp index 7e370a4a7..c7072b304 100644 --- a/core/src/stages/place_provider.cpp +++ b/core/src/stages/place_provider.cpp @@ -53,37 +53,6 @@ namespace stages { // PlaceProviderBase // ------------------- -PlaceProviderBase::PlaceProviderBase(const std::string& name) : GeneratePose(name) { - auto& p = properties(); - p.declare("object"); - p.declare("ik_frame"); -} - -void PlaceProviderBase::onNewSolution(const SolutionBase& s) { - planning_scene::PlanningSceneConstPtr 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); -} - // ------------------- // PlaceProviderDefault // ------------------- diff --git a/core/src/stages/place_provider_base.cpp b/core/src/stages/place_provider_base.cpp new file mode 100644 index 000000000..067dcb169 --- /dev/null +++ b/core/src/stages/place_provider_base.cpp @@ -0,0 +1,50 @@ +// +// Created by jafar_abdi on 3/27/21. +// + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "moveit/task_constructor/stages/place_provider_base.h" +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"); +} +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/tasks/CMakeLists.txt b/core/src/tasks/CMakeLists.txt index 7dedaf092..254080742 100644 --- a/core/src/tasks/CMakeLists.txt +++ b/core/src/tasks/CMakeLists.txt @@ -2,8 +2,7 @@ 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}) -target_link_libraries(${PROJECT_NAME}_tasks ${PROJECT_NAME}_stages ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME}_tasks ${PROJECT_NAME} ${PROJECT_NAME}_stages ${catkin_LIBRARIES}) install(TARGETS ${PROJECT_NAME}_tasks ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} From 7f57c9dadcaa13300ba2f88ef7e3c1c63b6793a4 Mon Sep 17 00:00:00 2001 From: Artur Istvan Karoly Date: Mon, 29 Mar 2021 17:16:37 +0200 Subject: [PATCH 13/23] WIP: Fix bug at task clear --- .../task_constructor/tasks/pick_place_task.h | 2 ++ core/src/stages/pick.cpp | 3 ++- core/src/tasks/pick_place_task.cpp | 22 ++++++++++++------- 3 files changed, 18 insertions(+), 9 deletions(-) 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 af575c8bf..e117d637d 100644 --- a/core/include/moveit/task_constructor/tasks/pick_place_task.h +++ b/core/include/moveit/task_constructor/tasks/pick_place_task.h @@ -128,6 +128,8 @@ class PickPlaceTask private: moveit::task_constructor::TaskPtr task_; const std::string task_name_; + Stage* current_state_stage; + Stage* attach_object_stage; }; } // namespace tasks diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index e68da136d..91a7c634d 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -67,7 +67,8 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provide { std::unique_ptr wrapper; if (is_pick_) { - std::unique_ptr provider_stage_plugin(grasp_provider_class_loader_.createUnmanagedInstance(provider_stage_plugin_name_)); + // std::unique_ptr provider_stage_plugin(grasp_provider_class_loader_.createUnmanagedInstance(provider_stage_plugin_name_)); + auto provider_stage_plugin = grasp_provider_class_loader_.createUniqueInstance(provider_stage_plugin_name_); provider_stage_plugin->properties().configureInitFrom(Stage::PARENT); provider_stage_plugin->properties().property("pregrasp").configureInitFrom(Stage::PARENT, "eef_group_open_pose"); provider_stage_plugin->properties().property("grasp").configureInitFrom(Stage::PARENT, "eef_group_close_pose"); diff --git a/core/src/tasks/pick_place_task.cpp b/core/src/tasks/pick_place_task.cpp index f7eab5b16..0f76d27c9 100644 --- a/core/src/tasks/pick_place_task.cpp +++ b/core/src/tasks/pick_place_task.cpp @@ -43,17 +43,25 @@ namespace tasks { constexpr char LOGNAME[] = "pick_place_task"; PickPlaceTask::PickPlaceTask(const std::string& task_name) - : task_name_(task_name) {} + : task_name_(task_name) { + task_.reset(); + task_.reset(new moveit::task_constructor::Task(task_name_)); + task_->reset(); + task_->loadRobotModel(); + current_state_stage = nullptr; + attach_object_stage = nullptr; + } 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_)); + if(task_){ + task_->clear(); + task_->loadRobotModel(); + } Task& t = *task_; - t.loadRobotModel(); // Sampling planner // TODO(henningkayser): Setup and parameterize alternative planners @@ -77,7 +85,6 @@ void PickPlaceTask::init(const Parameters& parameters) * 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); @@ -95,7 +102,7 @@ void PickPlaceTask::init(const Parameters& parameters) return true; }); - current_state = applicability_filter.get(); + current_state_stage = applicability_filter.get(); t.add(std::move(applicability_filter)); } @@ -129,7 +136,6 @@ void PickPlaceTask::init(const Parameters& parameters) * Pick Object * * * ***************************************************/ - Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator { auto stage = std::make_unique("Pick object", parameters.grasp_provider_plugin_name_); stage->properties().property("eef_group").configureInitFrom(Stage::PARENT, "hand"); @@ -140,7 +146,7 @@ void PickPlaceTask::init(const Parameters& parameters) stage->setSupportSurfaces(parameters.support_surfaces_); stage->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_); stage->GraspProviderPlugin()->properties().set("angle_delta", M_PI / 12); // Set plugin-specific properties - stage->setMonitoredStage(current_state); + stage->setMonitoredStage(current_state_stage); stage->setApproachMotion(parameters.approach_object_direction_,parameters.approach_object_min_dist_, parameters.approach_object_max_dist_); stage->setLiftMotion(parameters.lift_object_direction_, parameters.lift_object_min_dist_, parameters.lift_object_max_dist_); attach_object_stage = stage->attachStage(); From ae12189ae885bdf3c2f15459115423c856e21914 Mon Sep 17 00:00:00 2001 From: Artur Istvan Karoly Date: Thu, 1 Apr 2021 21:08:40 +0200 Subject: [PATCH 14/23] Fix bug at task clear --- .../moveit/task_constructor/stages/pick.h | 26 ++++++++-------- .../task_constructor/tasks/pick_place_task.h | 18 +++++++++-- core/src/stages/pick.cpp | 30 +++++++++---------- core/src/tasks/pick_place_task.cpp | 14 +++++---- 4 files changed, 51 insertions(+), 37 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/pick.h b/core/include/moveit/task_constructor/stages/pick.h index 91d7a3425..b958bb3e3 100644 --- a/core/include/moveit/task_constructor/stages/pick.h +++ b/core/include/moveit/task_constructor/stages/pick.h @@ -85,23 +85,23 @@ class PickPlaceBase : public SerialContainer Stage* move_there_stage_ = nullptr; Stage* compute_ik_stage_ = nullptr; - moveit::task_constructor::stages::ModifyPlanningScene* set_collision_object_hand_stage_ = nullptr; - moveit::task_constructor::stages::ModifyPlanningScene* allow_collision_object_support_stage_ = nullptr; - moveit::task_constructor::stages::ModifyPlanningScene* forbid_collision_object_support_stage_ = nullptr; + ModifyPlanningScene* set_collision_object_hand_stage_ = nullptr; + ModifyPlanningScene* allow_collision_object_support_stage_ = nullptr; + ModifyPlanningScene* forbid_collision_object_support_stage_ = nullptr; Stage* move_back_stage_ = nullptr; std::string provider_stage_plugin_name_; - pluginlib::ClassLoader grasp_provider_class_loader_; - pluginlib::ClassLoader place_provider_class_loader_; + pluginlib::ClassLoader* grasp_provider_class_loader_; + pluginlib::ClassLoader* place_provider_class_loader_; protected: - moveit::task_constructor::stages::GraspProviderBase* grasp_stage_ = nullptr; - moveit::task_constructor::stages::PlaceProviderBase* place_stage_ = nullptr; - moveit::task_constructor::stages::ModifyPlanningScene* attach_detach_stage_ = nullptr; + GraspProviderBase* grasp_stage_ = nullptr; + PlaceProviderBase* place_stage_ = nullptr; + ModifyPlanningScene* attach_detach_stage_ = nullptr; public: - PickPlaceBase(const std::string& name, const std::string& provider_stage_plugin_name, bool is_pick); + PickPlaceBase(const std::string& name, const std::string& provider_stage_plugin_name, bool is_pick, pluginlib::ClassLoader* grasp_class_loader, pluginlib::ClassLoader* place_class_loader); void init(const moveit::core::RobotModelConstPtr& robot_model) override; @@ -164,8 +164,8 @@ class PickPlaceBase : public SerialContainer class Pick : public PickPlaceBase { public: - Pick(const std::string& name = "pick", const std::string& provider_stage_plugin_name = "moveit_task_constructor/GraspProviderDefault") - : PickPlaceBase(name, provider_stage_plugin_name, true) {} + 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, nullptr) {} void setMonitoredStage(Stage* monitored); @@ -196,8 +196,8 @@ class Pick : public PickPlaceBase class Place : public PickPlaceBase { public: - Place(const std::string& name = "place", const std::string& provider_stage_plugin_name = "moveit_task_constructor/PlaceProviderDefault") - : PickPlaceBase(name, provider_stage_plugin_name, false) {} + 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, nullptr, class_loader) {} void setMonitoredStage(Stage* monitored); 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 e117d637d..8ce196eab 100644 --- a/core/include/moveit/task_constructor/tasks/pick_place_task.h +++ b/core/include/moveit/task_constructor/tasks/pick_place_task.h @@ -54,6 +54,10 @@ #include #include #include + +#include +#include + #include #include #include @@ -65,11 +69,16 @@ #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 @@ -126,10 +135,13 @@ class PickPlaceTask bool plan(); private: - moveit::task_constructor::TaskPtr task_; + TaskPtr task_; const std::string task_name_; - Stage* current_state_stage; - Stage* attach_object_stage; + Stage* current_state_stage_; + Stage* attach_object_stage_; + + std::unique_ptr grasp_provider_class_loader_; + std::unique_ptr place_provider_class_loader_; }; } // namespace tasks diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index 91a7c634d..e779020d5 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -21,10 +21,8 @@ namespace moveit { namespace task_constructor { namespace stages { -PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provider_stage_plugin_name, bool is_pick) - : SerialContainer(name), - grasp_provider_class_loader_("moveit_task_constructor_core", "moveit::task_constructor::stages::GraspProviderBase"), - place_provider_class_loader_("moveit_task_constructor_core", "moveit::task_constructor::stages::PlaceProviderBase") { +PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provider_stage_plugin_name, bool is_pick, pluginlib::ClassLoader* grasp_class_loader, pluginlib::ClassLoader* place_class_loader) + : SerialContainer(name) { PropertyMap& p = properties(); p.declare("object", "name of object to grasp/place"); p.declare("eef", "end effector name"); @@ -37,6 +35,9 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provide p.declare("eef_group", "JMG of eef"); p.declare("eef_parent_group", "JMG of eef's parent"); + grasp_provider_class_loader_ = grasp_class_loader; + place_provider_class_loader_ = place_class_loader; + cartesian_solver_ = std::make_shared(); sampling_planner_ = std::make_shared(); @@ -65,25 +66,24 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provide try { - std::unique_ptr wrapper; + std::unique_ptr wrapper; if (is_pick_) { - // std::unique_ptr provider_stage_plugin(grasp_provider_class_loader_.createUnmanagedInstance(provider_stage_plugin_name_)); - auto provider_stage_plugin = grasp_provider_class_loader_.createUniqueInstance(provider_stage_plugin_name_); + std::unique_ptr provider_stage_plugin(grasp_provider_class_loader_->createUnmanagedInstance(provider_stage_plugin_name_)); provider_stage_plugin->properties().configureInitFrom(Stage::PARENT); provider_stage_plugin->properties().property("pregrasp").configureInitFrom(Stage::PARENT, "eef_group_open_pose"); provider_stage_plugin->properties().property("grasp").configureInitFrom(Stage::PARENT, "eef_group_close_pose"); provider_stage_plugin->properties().set("marker_ns", "grasp_pose"); grasp_stage_ = provider_stage_plugin.get(); - wrapper = std::make_unique("grasp pose IK", std::move(provider_stage_plugin)); + wrapper = std::make_unique("grasp pose IK", std::move(provider_stage_plugin)); } else { - std::unique_ptr provider_stage_plugin(place_provider_class_loader_.createUnmanagedInstance(provider_stage_plugin_name_)); + std::unique_ptr provider_stage_plugin(place_provider_class_loader_->createUnmanagedInstance(provider_stage_plugin_name_)); provider_stage_plugin->properties().configureInitFrom(Stage::PARENT); provider_stage_plugin->properties().set("marker_ns", "place_pose"); place_stage_ = provider_stage_plugin.get(); - wrapper = std::make_unique("place pose IK", std::move(provider_stage_plugin)); + wrapper = std::make_unique("place pose IK", std::move(provider_stage_plugin)); } properties().exposeTo(wrapper->properties(), {"object", "eef_group_open_pose", "eef_group_close_pose"}); @@ -99,13 +99,13 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provide } { - auto set_collision_object_hand = std::make_unique(is_pick_ ? "allow collision (hand,object)" : "forbid collision (hand,object)"); + auto set_collision_object_hand = std::make_unique(is_pick_ ? "allow collision (hand,object)" : "forbid collision (hand,object)"); 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_); + 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::PARENT, is_pick_ ? "eef_group_close_pose" : "eef_group_open_pose"); @@ -116,13 +116,13 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provide } { - auto attach_detach = std::make_unique(is_pick_ ? "attach object" : "detach object"); + auto attach_detach = std::make_unique(is_pick_ ? "attach object" : "detach object"); 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)"); + auto set_collision_object_support = std::make_unique("allow collision (object,support)"); allow_collision_object_support_stage_ = set_collision_object_support.get(); insert(std::move(set_collision_object_support)); } @@ -138,7 +138,7 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provide } if (is_pick_) { - auto set_collision_object_support = std::make_unique("forbid collision (object,support)"); + 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)); } diff --git a/core/src/tasks/pick_place_task.cpp b/core/src/tasks/pick_place_task.cpp index 0f76d27c9..9aeec9cec 100644 --- a/core/src/tasks/pick_place_task.cpp +++ b/core/src/tasks/pick_place_task.cpp @@ -44,12 +44,14 @@ 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; + current_state_stage_ = nullptr; + attach_object_stage_ = nullptr; } void PickPlaceTask::init(const Parameters& parameters) @@ -102,7 +104,7 @@ void PickPlaceTask::init(const Parameters& parameters) return true; }); - current_state_stage = applicability_filter.get(); + current_state_stage_ = applicability_filter.get(); t.add(std::move(applicability_filter)); } @@ -137,7 +139,7 @@ void PickPlaceTask::init(const Parameters& parameters) * * ***************************************************/ { - auto stage = std::make_unique("Pick object", parameters.grasp_provider_plugin_name_); + auto stage = std::make_unique("Pick object", parameters.grasp_provider_plugin_name_, 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_); @@ -146,10 +148,10 @@ void PickPlaceTask::init(const Parameters& parameters) stage->setSupportSurfaces(parameters.support_surfaces_); stage->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_); stage->GraspProviderPlugin()->properties().set("angle_delta", M_PI / 12); // Set plugin-specific properties - stage->setMonitoredStage(current_state_stage); + stage->setMonitoredStage(current_state_stage_); stage->setApproachMotion(parameters.approach_object_direction_,parameters.approach_object_min_dist_, parameters.approach_object_max_dist_); stage->setLiftMotion(parameters.lift_object_direction_, parameters.lift_object_min_dist_, parameters.lift_object_max_dist_); - attach_object_stage = stage->attachStage(); + attach_object_stage_ = stage->attachStage(); t.add(std::move(stage)); } From ff9ab714b76d9b1c499133d56100b10d2584a3cb Mon Sep 17 00:00:00 2001 From: Artur Istvan Karoly Date: Thu, 8 Apr 2021 18:47:24 +0200 Subject: [PATCH 15/23] Create template class PickPlaceBase --- .../moveit/task_constructor/stages/pick.h | 27 +++++---- core/src/stages/pick.cpp | 56 +++++++++++-------- core/src/tasks/pick_place_task.cpp | 2 +- 3 files changed, 48 insertions(+), 37 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/pick.h b/core/include/moveit/task_constructor/stages/pick.h index b958bb3e3..178637bf8 100644 --- a/core/include/moveit/task_constructor/stages/pick.h +++ b/core/include/moveit/task_constructor/stages/pick.h @@ -39,6 +39,8 @@ #include #include #include +#include +#include #include #include #include @@ -75,6 +77,7 @@ namespace stages { * - detach the object * - linearly retract end effector */ +template class PickPlaceBase : public SerialContainer { @@ -83,25 +86,21 @@ class PickPlaceBase : public SerialContainer solvers::CartesianPathPtr cartesian_solver_; solvers::PipelinePlannerPtr sampling_planner_; - Stage* move_there_stage_ = nullptr; - Stage* compute_ik_stage_ = nullptr; + 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; - Stage* move_back_stage_ = nullptr; + MoveRelative* move_back_stage_ = nullptr; std::string provider_stage_plugin_name_; - pluginlib::ClassLoader* grasp_provider_class_loader_; - pluginlib::ClassLoader* place_provider_class_loader_; - protected: - GraspProviderBase* grasp_stage_ = nullptr; - PlaceProviderBase* place_stage_ = nullptr; + 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* grasp_class_loader, pluginlib::ClassLoader* place_class_loader); + 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; @@ -157,15 +156,15 @@ class PickPlaceBase : public SerialContainer solvers::PipelinePlannerPtr samplingPlanner() {return sampling_planner_;} // Use this to retrieve a pointer to the GraspProviderPlugin object, to set its custom properties - moveit::task_constructor::Stage* GraspProviderPlugin() {return grasp_stage_;} + C* ProviderPlugin() {return provider_plugin_stage_;} }; /// specialization of PickPlaceBase to realize picking -class Pick : public PickPlaceBase +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, nullptr) {} + : PickPlaceBase(name, provider_stage_plugin_name, true, class_loader) {} void setMonitoredStage(Stage* monitored); @@ -193,11 +192,11 @@ class Pick : public PickPlaceBase }; /// specialization of PickPlaceBase to realize placing -class Place : public PickPlaceBase +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, nullptr, class_loader) {} + : PickPlaceBase(name, provider_stage_plugin_name, false, class_loader) {} void setMonitoredStage(Stage* monitored); diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index e779020d5..91e5e7614 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -21,7 +21,8 @@ namespace moveit { namespace task_constructor { namespace stages { -PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provider_stage_plugin_name, bool is_pick, pluginlib::ClassLoader* grasp_class_loader, pluginlib::ClassLoader* place_class_loader) +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"); @@ -35,9 +36,6 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provide p.declare("eef_group", "JMG of eef"); p.declare("eef_parent_group", "JMG of eef's parent"); - grasp_provider_class_loader_ = grasp_class_loader; - place_provider_class_loader_ = place_class_loader; - cartesian_solver_ = std::make_shared(); sampling_planner_ = std::make_shared(); @@ -68,20 +66,20 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provide { std::unique_ptr wrapper; if (is_pick_) { - std::unique_ptr provider_stage_plugin(grasp_provider_class_loader_->createUnmanagedInstance(provider_stage_plugin_name_)); + std::unique_ptr provider_stage_plugin(class_loader->createUnmanagedInstance(provider_stage_plugin_name_)); provider_stage_plugin->properties().configureInitFrom(Stage::PARENT); provider_stage_plugin->properties().property("pregrasp").configureInitFrom(Stage::PARENT, "eef_group_open_pose"); provider_stage_plugin->properties().property("grasp").configureInitFrom(Stage::PARENT, "eef_group_close_pose"); provider_stage_plugin->properties().set("marker_ns", "grasp_pose"); - grasp_stage_ = provider_stage_plugin.get(); + 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(place_provider_class_loader_->createUnmanagedInstance(provider_stage_plugin_name_)); + 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"); - place_stage_ = provider_stage_plugin.get(); + provider_plugin_stage_ = provider_stage_plugin.get(); wrapper = std::make_unique("place pose IK", std::move(provider_stage_plugin)); } @@ -144,7 +142,11 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& provide } } -void PickPlaceBase::init(const moveit::core::RobotModelConstPtr& robot_model) { +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()); @@ -180,7 +182,8 @@ void PickPlaceBase::init(const moveit::core::RobotModelConstPtr& robot_model) { /// ------------------------- /// setters of own properties -void PickPlaceBase::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) { +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); @@ -194,7 +197,8 @@ void PickPlaceBase::setIKFrame(const Eigen::Isometry3d& pose, const std::string& /// approach / place -void PickPlaceBase::setApproachPlace(const geometry_msgs::TwistStamped& motion, double min_distance, +template +void PickPlaceBase::setApproachPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { auto& p = move_there_stage_->properties(); p.set("direction", motion); @@ -202,7 +206,8 @@ void PickPlaceBase::setApproachPlace(const geometry_msgs::TwistStamped& motion, p.set("max_distance", max_distance); } -void PickPlaceBase::setApproachPlace(const geometry_msgs::Vector3Stamped& direction, double min_distance, +template +void PickPlaceBase::setApproachPlace(const geometry_msgs::Vector3Stamped& direction, double min_distance, double max_distance) { auto& p = move_there_stage_->properties(); p.set("direction", direction); @@ -210,24 +215,28 @@ void PickPlaceBase::setApproachPlace(const geometry_msgs::Vector3Stamped& direct p.set("max_distance", max_distance); } -void PickPlaceBase::setApproachPlace(const std::map& joints) { +template +void PickPlaceBase::setApproachPlace(const std::map& joints) { auto& p = move_there_stage_->properties(); p.set("joints", joints); } /// IK computation -void PickPlaceBase::setMaxIKSolutions(const uint32_t& max_ik_solutions) { +template +void PickPlaceBase::setMaxIKSolutions(const uint32_t& max_ik_solutions) { auto& p = compute_ik_stage_->properties(); p.set("max_ik_solutions", max_ik_solutions); } -void PickPlaceBase::setMinIKSolutionDistance(const double& min_ik_solution_distance) { +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); } -void PickPlaceBase::setIgnoreIKCollisions(const bool& ignore_ik_collisions) { +template +void PickPlaceBase::setIgnoreIKCollisions(const bool& ignore_ik_collisions) { auto& p = compute_ik_stage_->properties(); p.set("ignore_collisions", ignore_ik_collisions); } @@ -235,21 +244,24 @@ void PickPlaceBase::setIgnoreIKCollisions(const bool& ignore_ik_collisions) { /// lift / retract -void PickPlaceBase::setLiftRetract(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { +template +void PickPlaceBase::setLiftRetract(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { auto& p = move_back_stage_->properties(); p.set("direction", motion); p.set("min_distance", min_distance); p.set("max_distance", max_distance); } -void PickPlaceBase::setLiftRetract(const geometry_msgs::Vector3Stamped& direction, double min_distance, double max_distance) { +template +void PickPlaceBase::setLiftRetract(const geometry_msgs::Vector3Stamped& direction, double min_distance, double max_distance) { auto& p = move_back_stage_->properties(); p.set("direction", direction); p.set("min_distance", min_distance); p.set("max_distance", max_distance); } -void PickPlaceBase::setLiftRetract(const std::map& joints) { +template +void PickPlaceBase::setLiftRetract(const std::map& joints) { auto& p = move_back_stage_->properties(); p.set("joints", joints); } @@ -259,7 +271,7 @@ void PickPlaceBase::setLiftRetract(const std::map& joints) /// setters of pick properties void Pick::setMonitoredStage(Stage* monitored) { - grasp_stage_->setMonitoredStage(monitored); + provider_plugin_stage_->setMonitoredStage(monitored); } @@ -267,11 +279,11 @@ void Pick::setMonitoredStage(Stage* monitored) { /// setters of place properties void Place::setMonitoredStage(Stage* monitored) { - place_stage_->setMonitoredStage(monitored); + provider_plugin_stage_->setMonitoredStage(monitored); } void Place::setPlacePose(const geometry_msgs::PoseStamped& pose) { - place_stage_->setPose(pose); + provider_plugin_stage_->setPose(pose); } } // namespace stages diff --git a/core/src/tasks/pick_place_task.cpp b/core/src/tasks/pick_place_task.cpp index 9aeec9cec..e746baf58 100644 --- a/core/src/tasks/pick_place_task.cpp +++ b/core/src/tasks/pick_place_task.cpp @@ -147,7 +147,7 @@ void PickPlaceTask::init(const Parameters& parameters) stage->setEndEffectorOpenClose(parameters.hand_open_pose_, parameters.hand_close_pose_); stage->setSupportSurfaces(parameters.support_surfaces_); stage->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_); - stage->GraspProviderPlugin()->properties().set("angle_delta", M_PI / 12); // Set plugin-specific properties + stage->ProviderPlugin()->properties().set("angle_delta", M_PI / 12); // Set plugin-specific properties stage->setMonitoredStage(current_state_stage_); stage->setApproachMotion(parameters.approach_object_direction_,parameters.approach_object_min_dist_, parameters.approach_object_max_dist_); stage->setLiftMotion(parameters.lift_object_direction_, parameters.lift_object_min_dist_, parameters.lift_object_max_dist_); From 0c8b21bee326d782a61ee2cb8a239d4584811e23 Mon Sep 17 00:00:00 2001 From: Artur Istvan Karoly Date: Mon, 12 Apr 2021 12:18:17 +0200 Subject: [PATCH 16/23] Return solution properly --- capabilities/src/plan_pick_place_capability.cpp | 7 +++++-- .../task_constructor/tasks/pick_place_task.h | 1 + core/src/tasks/pick_place_task.cpp | 4 ++++ msgs/action/PlanPickPlace.action | 16 ++++++---------- 4 files changed, 16 insertions(+), 12 deletions(-) diff --git a/capabilities/src/plan_pick_place_capability.cpp b/capabilities/src/plan_pick_place_capability.cpp index 9c81a7bef..d5fc73622 100644 --- a/capabilities/src/plan_pick_place_capability.cpp +++ b/capabilities/src/plan_pick_place_capability.cpp @@ -80,8 +80,11 @@ void PlanPickPlaceCapability::goalCallback( // Initialize task pick_place_task_->init(parameters); - // Run plan - pick_place_task_->plan(); + // Compute plan + result.success = pick_place_task_->plan(); + if (result.success) { + pick_place_task_->getSolutionMsg(result.solution); + } // Retrieve and return result as_->setSucceeded(result); 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 8ce196eab..d58d3aee1 100644 --- a/core/include/moveit/task_constructor/tasks/pick_place_task.h +++ b/core/include/moveit/task_constructor/tasks/pick_place_task.h @@ -133,6 +133,7 @@ class PickPlaceTask void init(const Parameters& parameters); bool plan(); + void getSolutionMsg(moveit_task_constructor_msgs::Solution& solution); private: TaskPtr task_; diff --git a/core/src/tasks/pick_place_task.cpp b/core/src/tasks/pick_place_task.cpp index e746baf58..70228faed 100644 --- a/core/src/tasks/pick_place_task.cpp +++ b/core/src/tasks/pick_place_task.cpp @@ -204,6 +204,10 @@ bool PickPlaceTask::plan() { 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/msgs/action/PlanPickPlace.action b/msgs/action/PlanPickPlace.action index 87865e7ec..c16ee6ba5 100644 --- a/msgs/action/PlanPickPlace.action +++ b/msgs/action/PlanPickPlace.action @@ -1,32 +1,28 @@ -# goal - -# planning group properties +# Planning group properties string arm_group_name string hand_group_name -# robot model and links +# Robot model and links string eef_name string hand_frame -# object and surfaces +# Object and surfaces string object_id string[] support_surfaces -# picking +# Picking string grasp_provider_plugin_name moveit_msgs/Grasp grasp geometry_msgs/Pose grasp_frame_transform -# placing +# Placing moveit_msgs/PlaceLocation place_location --- -# result -moveit_msgs/MoveItErrorCodes error_code +bool success Solution solution --- -# feedback string feedback From 10fdb4e9439e086fca4f6bbd1bc4b04e80b5d3c6 Mon Sep 17 00:00:00 2001 From: Artur Istvan Karoly Date: Mon, 12 Apr 2021 18:19:32 +0200 Subject: [PATCH 17/23] Add Placing --- .../src/plan_pick_place_capability.cpp | 27 ++- .../task_constructor/tasks/pick_place_task.h | 15 +- core/src/tasks/pick_place_task.cpp | 186 ++++++++++-------- msgs/action/PlanPickPlace.action | 7 + 4 files changed, 143 insertions(+), 92 deletions(-) diff --git a/capabilities/src/plan_pick_place_capability.cpp b/capabilities/src/plan_pick_place_capability.cpp index d5fc73622..ee473b263 100644 --- a/capabilities/src/plan_pick_place_capability.cpp +++ b/capabilities/src/plan_pick_place_capability.cpp @@ -60,6 +60,7 @@ void PlanPickPlaceCapability::goalCallback( // TODO: 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; @@ -77,17 +78,27 @@ void PlanPickPlaceCapability::goalCallback( parameters.lift_object_min_dist_ = goal->grasp.post_grasp_retreat.min_distance; parameters.lift_object_max_dist_ = goal->grasp.post_grasp_retreat.desired_distance; - // Initialize task - pick_place_task_->init(parameters); + parameters.place_provider_plugin_name_ = goal->place_provider_plugin_name; + parameters.place_pose_ = goal->place_location.place_pose; + parameters.place_object_direction_ = goal->place_location.pre_place_approach.direction; + parameters.place_object_min_dist_ = goal->place_location.pre_place_approach.min_distance; + parameters.place_object_max_dist_ = goal->place_location.pre_place_approach.desired_distance; + parameters.retract_direction_ = goal->place_location.post_place_retreat.direction; + parameters.retract_min_dist_ = goal->place_location.post_place_retreat.min_distance; + parameters.retract_max_dist_ = goal->place_location.post_place_retreat.desired_distance; - // Compute plan - result.success = pick_place_task_->plan(); - if (result.success) { - pick_place_task_->getSolutionMsg(result.solution); + // 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); + as_->setSucceeded(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 d58d3aee1..45060dfec 100644 --- a/core/include/moveit/task_constructor/tasks/pick_place_task.h +++ b/core/include/moveit/task_constructor/tasks/pick_place_task.h @@ -86,6 +86,8 @@ class PickPlaceTask public: struct Parameters { + short task_type_; + // planning group properties std::string arm_group_name_; std::string eef_name_; @@ -108,6 +110,7 @@ class PickPlaceTask // Plugins std::string grasp_provider_plugin_name_; + std::string place_provider_plugin_name_; // Pick metrics Eigen::Isometry3d grasp_frame_transform_; @@ -117,20 +120,22 @@ class PickPlaceTask geometry_msgs::Vector3Stamped lift_object_direction_; 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_; + geometry_msgs::Vector3Stamped place_object_direction_; + double place_object_min_dist_; + double place_object_max_dist_; + geometry_msgs::Vector3Stamped retract_direction_; + double retract_min_dist_; + double retract_max_dist_; }; PickPlaceTask(const std::string& task_name); ~PickPlaceTask() = default; - void init(const Parameters& parameters); + bool init(const Parameters& parameters); bool plan(); void getSolutionMsg(moveit_task_constructor_msgs::Solution& solution); diff --git a/core/src/tasks/pick_place_task.cpp b/core/src/tasks/pick_place_task.cpp index 70228faed..c09723e93 100644 --- a/core/src/tasks/pick_place_task.cpp +++ b/core/src/tasks/pick_place_task.cpp @@ -54,7 +54,7 @@ PickPlaceTask::PickPlaceTask(const std::string& task_name) attach_object_stage_ = nullptr; } -void PickPlaceTask::init(const Parameters& parameters) +bool PickPlaceTask::init(const Parameters& parameters) { ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); // Reset ROS introspection before constructing the new object @@ -96,10 +96,23 @@ void PickPlaceTask::init(const Parameters& parameters) 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_)) + + if (parameters.task_type_ == moveit_task_constructor_msgs::PlanPickPlaceGoal::PICK_ONLY || + parameters.task_type_ == moveit_task_constructor_msgs::PlanPickPlaceGoal::PICK_AND_PLACE) { - comment = "object with id '" + parameters.object_name_ + "' is already attached and cannot be picked"; - return false; + 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; }); @@ -108,85 +121,100 @@ void PickPlaceTask::init(const Parameters& parameters) 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)); + if (parameters.task_type_ == moveit_task_constructor_msgs::PlanPickPlaceGoal::PICK_ONLY || + parameters.task_type_ == moveit_task_constructor_msgs::PlanPickPlaceGoal::PICK_AND_PLACE) + { + /**************************************************** + * * + * 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 * + * * + ***************************************************/ + { + auto stage = std::make_unique("Pick object", parameters.grasp_provider_plugin_name_, 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->setEndEffectorOpenClose(parameters.hand_open_pose_, parameters.hand_close_pose_); + stage->setSupportSurfaces(parameters.support_surfaces_); + stage->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_); + stage->ProviderPlugin()->properties().set("angle_delta", M_PI / 12); // Set plugin-specific properties + stage->setMonitoredStage(current_state_stage_); + stage->setApproachMotion(parameters.approach_object_direction_,parameters.approach_object_min_dist_, parameters.approach_object_max_dist_); + stage->setLiftMotion(parameters.lift_object_direction_, parameters.lift_object_min_dist_, parameters.lift_object_max_dist_); + attach_object_stage_ = stage->attachStage(); + t.add(std::move(stage)); + } } - /**************************************************** - * * - * Pick Object * - * * - ***************************************************/ + if (parameters.task_type_ == moveit_task_constructor_msgs::PlanPickPlaceGoal::PLACE_ONLY || + parameters.task_type_ == moveit_task_constructor_msgs::PlanPickPlaceGoal::PICK_AND_PLACE) { - auto stage = std::make_unique("Pick object", parameters.grasp_provider_plugin_name_, 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->setEndEffectorOpenClose(parameters.hand_open_pose_, parameters.hand_close_pose_); - stage->setSupportSurfaces(parameters.support_surfaces_); - stage->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_); - stage->ProviderPlugin()->properties().set("angle_delta", M_PI / 12); // Set plugin-specific properties - stage->setMonitoredStage(current_state_stage_); - stage->setApproachMotion(parameters.approach_object_direction_,parameters.approach_object_min_dist_, parameters.approach_object_max_dist_); - stage->setLiftMotion(parameters.lift_object_direction_, parameters.lift_object_min_dist_, parameters.lift_object_max_dist_); - attach_object_stage_ = stage->attachStage(); - t.add(std::move(stage)); - } - /****************************************************** - * * - * 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 * - // * * - // *****************************************************/ - - - // /****************************************************** - // * * - // * 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)); - // } + /****************************************************** + * * + * 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 * + * * + *****************************************************/ + Stage* detach_object_stage = nullptr; // Forward detach_object_stage to place pose generator + { + auto stage = std::make_unique("Place object", parameters.place_provider_plugin_name_, 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->setEndEffectorOpenClose(parameters.hand_open_pose_, parameters.hand_close_pose_); + stage->setSupportSurfaces(parameters.support_surfaces_); + stage->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_); + 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->setPlacePose(parameters.place_pose_); + stage->setPlaceMotion(parameters.place_object_direction_, parameters.place_object_min_dist_, parameters.place_object_max_dist_); + stage->setRetractMotion(parameters.retract_direction_, parameters.retract_min_dist_, parameters.retract_max_dist_); + detach_object_stage = stage->detachStage(); + t.add(std::move(stage)); + } + } } bool PickPlaceTask::plan() { diff --git a/msgs/action/PlanPickPlace.action b/msgs/action/PlanPickPlace.action index c16ee6ba5..b521a5099 100644 --- a/msgs/action/PlanPickPlace.action +++ b/msgs/action/PlanPickPlace.action @@ -1,3 +1,9 @@ +# By default, the task type is +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 @@ -16,6 +22,7 @@ moveit_msgs/Grasp grasp geometry_msgs/Pose grasp_frame_transform # Placing +string place_provider_plugin_name moveit_msgs/PlaceLocation place_location --- From 482973487e6b6b47aedb79958aeb49c387746c3b Mon Sep 17 00:00:00 2001 From: Artur Istvan Karoly Date: Wed, 14 Apr 2021 11:09:36 +0200 Subject: [PATCH 18/23] Add GraspProvider for fixed grasp pose --- .../src/plan_pick_place_capability.cpp | 1 + .../task_constructor/stages/grasp_provider.h | 13 ++++ .../task_constructor/tasks/pick_place_task.h | 1 + ...ion_planning_stages_plugin_description.xml | 8 ++ core/src/stages/grasp_provider.cpp | 76 ++++++++++++++++++- core/src/tasks/pick_place_task.cpp | 29 ++++++- 6 files changed, 122 insertions(+), 6 deletions(-) diff --git a/capabilities/src/plan_pick_place_capability.cpp b/capabilities/src/plan_pick_place_capability.cpp index ee473b263..8d22e77bb 100644 --- a/capabilities/src/plan_pick_place_capability.cpp +++ b/capabilities/src/plan_pick_place_capability.cpp @@ -67,6 +67,7 @@ void PlanPickPlaceCapability::goalCallback( parameters.hand_frame_ = goal->hand_frame; parameters.object_name_ = goal->object_id; parameters.support_surfaces_ = goal->support_surfaces; + parameters.grasp_pose_ = goal->grasp.grasp_pose; parameters.grasp_provider_plugin_name_ = goal->grasp_provider_plugin_name; tf::poseMsgToEigen(goal->grasp_frame_transform, parameters.grasp_frame_transform_); parameters.hand_open_pose_ = "open"; diff --git a/core/include/moveit/task_constructor/stages/grasp_provider.h b/core/include/moveit/task_constructor/stages/grasp_provider.h index 3728a465d..9ce5b7607 100644 --- a/core/include/moveit/task_constructor/stages/grasp_provider.h +++ b/core/include/moveit/task_constructor/stages/grasp_provider.h @@ -58,6 +58,19 @@ class GraspProviderDefault : public GraspProviderBase void compute() override; }; + +/// Grasp Provider plugin for setting a single grasp pose + +class GraspProviderFixedPose : public GraspProviderBase +{ +public: + GraspProviderFixedPose(const std::string& name = "set grasp pose"); + + 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/tasks/pick_place_task.h b/core/include/moveit/task_constructor/tasks/pick_place_task.h index 45060dfec..8e3aa65f8 100644 --- a/core/include/moveit/task_constructor/tasks/pick_place_task.h +++ b/core/include/moveit/task_constructor/tasks/pick_place_task.h @@ -113,6 +113,7 @@ class PickPlaceTask std::string place_provider_plugin_name_; // Pick metrics + geometry_msgs::PoseStamped grasp_pose_; Eigen::Isometry3d grasp_frame_transform_; geometry_msgs::Vector3Stamped approach_object_direction_; double approach_object_min_dist_; diff --git a/core/motion_planning_stages_plugin_description.xml b/core/motion_planning_stages_plugin_description.xml index 606373a41..93d2cf902 100644 --- a/core/motion_planning_stages_plugin_description.xml +++ b/core/motion_planning_stages_plugin_description.xml @@ -17,6 +17,14 @@ + + + Grasp Provider plugin for setting a single grasp pose + + + diff --git a/core/src/stages/grasp_provider.cpp b/core/src/stages/grasp_provider.cpp index 71ec69a00..ac6c96e79 100644 --- a/core/src/stages/grasp_provider.cpp +++ b/core/src/stages/grasp_provider.cpp @@ -48,10 +48,6 @@ namespace moveit { namespace task_constructor { namespace stages { -// ------------------- -// GraspProviderBase -// ------------------- - // ------------------- // GraspProviderDefault // ------------------- @@ -132,6 +128,77 @@ void GraspProviderDefault::compute() { } } + +// ------------------- +// GraspProviderFixedPose +// ------------------- + + +GraspProviderFixedPose::GraspProviderFixedPose(const std::string& name) : GraspProviderBase(name){ + auto& p = properties(); + p.declare("pose", geometry_msgs::PoseStamped(), "Grasp pose"); +} + +void GraspProviderFixedPose::init(const core::RobotModelConstPtr& robot_model) { + InitStageException errors; + try { + GraspProviderBase::init(robot_model); + } catch (InitStageException& e) { + errors.append(e); + } + + const auto& props = properties(); + + // check pose + geometry_msgs::PoseStamped target_pose = properties().get("pose"); + if (props.get("pose").header.frame_id == "") + errors.push_back(*this, "grasp pose header must be set"); + + // 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 { + // check availability of eef pose + const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef); + const std::string& name = props.get("pregrasp"); + std::map m; + if (!jmg->getVariableDefaultPositions(name, m)) + errors.push_back(*this, "unknown end effector pose: " + name); + } + + if (errors) + throw errors; +} + +void GraspProviderFixedPose::compute() { + if (upstream_solutions_.empty()) + return; + + planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff(); + geometry_msgs::PoseStamped target_pose = properties().get("pose"); + if (target_pose.header.frame_id.empty()) + target_pose.header.frame_id = scene->getPlanningFrame(); + else if (!scene->knowsFrameTransform(target_pose.header.frame_id)) { + ROS_WARN_NAMED("GeneratePose", "Unknown frame: '%s'", target_pose.header.frame_id.c_str()); + return; + } + + InterfaceState state(scene); + state.properties().set("target_pose", target_pose); + + SubTrajectory trajectory; + trajectory.setCost(0.0); + + rviz_marker_tools::appendFrame(trajectory.markers(), target_pose, 0.1, "grasp frame"); + + spawn(std::move(state), std::move(trajectory)); +} + + + } // namespace stages } // namespace task_constructor } // namespace moveit @@ -139,3 +206,4 @@ void GraspProviderDefault::compute() { /// register plugin #include PLUGINLIB_EXPORT_CLASS(moveit::task_constructor::stages::GraspProviderDefault, moveit::task_constructor::stages::GraspProviderBase) +PLUGINLIB_EXPORT_CLASS(moveit::task_constructor::stages::GraspProviderFixedPose, moveit::task_constructor::stages::GraspProviderBase) diff --git a/core/src/tasks/pick_place_task.cpp b/core/src/tasks/pick_place_task.cpp index c09723e93..24a180c90 100644 --- a/core/src/tasks/pick_place_task.cpp +++ b/core/src/tasks/pick_place_task.cpp @@ -36,6 +36,7 @@ #include #include +#include namespace moveit { namespace task_constructor { @@ -54,6 +55,13 @@ PickPlaceTask::PickPlaceTask(const std::string& task_name) 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"); @@ -155,7 +163,17 @@ bool PickPlaceTask::init(const Parameters& parameters) * * ***************************************************/ { - auto stage = std::make_unique("Pick object", parameters.grasp_provider_plugin_name_, grasp_provider_class_loader_.get()); + const bool grasp_pose_defined = grasp_pose_is_defined(parameters.grasp_pose_); + std::string grasp_provider_plugin; + if (grasp_pose_defined) + { + grasp_provider_plugin = "moveit_task_constructor/GraspProviderFixedPose"; + } + else + { + grasp_provider_plugin = parameters.grasp_provider_plugin_name_; + } + 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_); @@ -163,7 +181,14 @@ bool PickPlaceTask::init(const Parameters& parameters) stage->setEndEffectorOpenClose(parameters.hand_open_pose_, parameters.hand_close_pose_); stage->setSupportSurfaces(parameters.support_surfaces_); stage->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_); - stage->ProviderPlugin()->properties().set("angle_delta", M_PI / 12); // Set plugin-specific properties + if (grasp_pose_defined) + { + stage->ProviderPlugin()->properties().set("pose", parameters.grasp_pose_); + } + else // TODO: Go through properties systematically + { + stage->ProviderPlugin()->properties().set("angle_delta", M_PI / 12); // Set plugin-specific properties + } stage->setMonitoredStage(current_state_stage_); stage->setApproachMotion(parameters.approach_object_direction_,parameters.approach_object_min_dist_, parameters.approach_object_max_dist_); stage->setLiftMotion(parameters.lift_object_direction_, parameters.lift_object_min_dist_, parameters.lift_object_max_dist_); From eba5b096ed4c39951e874a5f21a17cdfec92f474 Mon Sep 17 00:00:00 2001 From: Artur Istvan Karoly Date: Thu, 22 Apr 2021 20:13:10 +0200 Subject: [PATCH 19/23] Make Pick container work with Grasp messages --- .../src/plan_pick_place_capability.cpp | 8 +- .../task_constructor/stages/grasp_provider.h | 4 +- .../task_constructor/tasks/pick_place_task.h | 2 + ...ion_planning_stages_plugin_description.xml | 4 +- core/src/stages/grasp_provider.cpp | 121 ++++++++++++------ core/src/stages/grasp_provider_base.cpp | 12 +- core/src/stages/pick.cpp | 21 ++- core/src/tasks/pick_place_task.cpp | 54 ++++---- msgs/action/PlanPickPlace.action | 2 +- 9 files changed, 129 insertions(+), 99 deletions(-) diff --git a/capabilities/src/plan_pick_place_capability.cpp b/capabilities/src/plan_pick_place_capability.cpp index 8d22e77bb..5dae14d03 100644 --- a/capabilities/src/plan_pick_place_capability.cpp +++ b/capabilities/src/plan_pick_place_capability.cpp @@ -67,17 +67,11 @@ void PlanPickPlaceCapability::goalCallback( parameters.hand_frame_ = goal->hand_frame; parameters.object_name_ = goal->object_id; parameters.support_surfaces_ = goal->support_surfaces; - parameters.grasp_pose_ = goal->grasp.grasp_pose; + 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.hand_open_pose_ = "open"; parameters.hand_close_pose_ = "close"; - parameters.approach_object_direction_ = goal->grasp.pre_grasp_approach.direction; - parameters.approach_object_min_dist_ = goal->grasp.pre_grasp_approach.min_distance; - parameters.approach_object_max_dist_ = goal->grasp.pre_grasp_approach.desired_distance; - parameters.lift_object_direction_ = goal->grasp.post_grasp_retreat.direction; - parameters.lift_object_min_dist_ = goal->grasp.post_grasp_retreat.min_distance; - parameters.lift_object_max_dist_ = goal->grasp.post_grasp_retreat.desired_distance; parameters.place_provider_plugin_name_ = goal->place_provider_plugin_name; parameters.place_pose_ = goal->place_location.place_pose; diff --git a/core/include/moveit/task_constructor/stages/grasp_provider.h b/core/include/moveit/task_constructor/stages/grasp_provider.h index 9ce5b7607..547f4c47c 100644 --- a/core/include/moveit/task_constructor/stages/grasp_provider.h +++ b/core/include/moveit/task_constructor/stages/grasp_provider.h @@ -61,10 +61,10 @@ class GraspProviderDefault : public GraspProviderBase /// Grasp Provider plugin for setting a single grasp pose -class GraspProviderFixedPose : public GraspProviderBase +class GraspProviderFixedPoses : public GraspProviderBase { public: - GraspProviderFixedPose(const std::string& name = "set grasp pose"); + GraspProviderFixedPoses(const std::string& name = "set grasp poses"); void init(const core::RobotModelConstPtr& robot_model) override; void compute() override; 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 8e3aa65f8..b3dcd1e35 100644 --- a/core/include/moveit/task_constructor/tasks/pick_place_task.h +++ b/core/include/moveit/task_constructor/tasks/pick_place_task.h @@ -68,6 +68,7 @@ #include #include +#include #include @@ -113,6 +114,7 @@ class PickPlaceTask std::string place_provider_plugin_name_; // Pick metrics + std::vector grasps_; geometry_msgs::PoseStamped grasp_pose_; Eigen::Isometry3d grasp_frame_transform_; geometry_msgs::Vector3Stamped approach_object_direction_; diff --git a/core/motion_planning_stages_plugin_description.xml b/core/motion_planning_stages_plugin_description.xml index 93d2cf902..6c3e0d2cb 100644 --- a/core/motion_planning_stages_plugin_description.xml +++ b/core/motion_planning_stages_plugin_description.xml @@ -17,8 +17,8 @@ - Grasp Provider plugin for setting a single grasp pose diff --git a/core/src/stages/grasp_provider.cpp b/core/src/stages/grasp_provider.cpp index ac6c96e79..21f4c3fd9 100644 --- a/core/src/stages/grasp_provider.cpp +++ b/core/src/stages/grasp_provider.cpp @@ -44,6 +44,8 @@ #include #include +#include + namespace moveit { namespace task_constructor { namespace stages { @@ -78,12 +80,9 @@ void GraspProviderDefault::init(const core::RobotModelConstPtr& robot_model) { if (!robot_model->hasEndEffector(eef)) errors.push_back(*this, "unknown end effector: " + eef); else { - // check availability of eef pose - const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef); - const std::string& name = props.get("pregrasp"); - std::map m; - if (!jmg->getVariableDefaultPositions(name, m)) - errors.push_back(*this, "unknown end effector pose: " + name); + // TODO: Validate eef_poses specified in grasps + if (props.get>("grasps").empty()) + errors.push_back(*this, "no grasp provided"); } if (errors) @@ -97,12 +96,10 @@ void GraspProviderDefault::compute() { // 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); - robot_state::RobotState& robot_state = scene->getCurrentStateNonConst(); - robot_state.setToDefaultValues(jmg, props.get("pregrasp")); - geometry_msgs::PoseStamped target_pose_msg; target_pose_msg.header.frame_id = props.get("object"); @@ -115,7 +112,30 @@ void GraspProviderDefault::compute() { InterfaceState state(scene); tf::poseEigenToMsg(target_pose, target_pose_msg.pose); state.properties().set("target_pose", target_pose_msg); - props.exposeTo(state.properties(), { "pregrasp", "grasp" }); + 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); + + // TODO: Raise exception if the sizes do not match + + state.properties().set("hand_open_pose", hand_open_pose); + state.properties().set("hand_close_pose", hand_close_pose); + + // props.exposeTo(state.properties(), { "pregrasp", "grasp" }); SubTrajectory trajectory; trajectory.setCost(0.0); @@ -130,16 +150,13 @@ void GraspProviderDefault::compute() { // ------------------- -// GraspProviderFixedPose +// GraspProviderFixedPoses // ------------------- -GraspProviderFixedPose::GraspProviderFixedPose(const std::string& name) : GraspProviderBase(name){ - auto& p = properties(); - p.declare("pose", geometry_msgs::PoseStamped(), "Grasp pose"); -} +GraspProviderFixedPoses::GraspProviderFixedPoses(const std::string& name) : GraspProviderBase(name){} -void GraspProviderFixedPose::init(const core::RobotModelConstPtr& robot_model) { +void GraspProviderFixedPoses::init(const core::RobotModelConstPtr& robot_model) { InitStageException errors; try { GraspProviderBase::init(robot_model); @@ -149,11 +166,6 @@ void GraspProviderFixedPose::init(const core::RobotModelConstPtr& robot_model) { const auto& props = properties(); - // check pose - geometry_msgs::PoseStamped target_pose = properties().get("pose"); - if (props.get("pose").header.frame_id == "") - errors.push_back(*this, "grasp pose header must be set"); - // check availability of object props.get("object"); // check availability of eef @@ -161,40 +173,65 @@ void GraspProviderFixedPose::init(const core::RobotModelConstPtr& robot_model) { if (!robot_model->hasEndEffector(eef)) errors.push_back(*this, "unknown end effector: " + eef); else { - // check availability of eef pose - const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef); - const std::string& name = props.get("pregrasp"); - std::map m; - if (!jmg->getVariableDefaultPositions(name, m)) - errors.push_back(*this, "unknown end effector pose: " + name); + // TODO: Validate eef_poses specified in grasps + if (props.get>("grasps").empty()) + errors.push_back(*this, "no grasp provided"); } if (errors) throw errors; } -void GraspProviderFixedPose::compute() { +void GraspProviderFixedPoses::compute() { if (upstream_solutions_.empty()) return; planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff(); - geometry_msgs::PoseStamped target_pose = properties().get("pose"); - if (target_pose.header.frame_id.empty()) - target_pose.header.frame_id = scene->getPlanningFrame(); - else if (!scene->knowsFrameTransform(target_pose.header.frame_id)) { - ROS_WARN_NAMED("GeneratePose", "Unknown frame: '%s'", target_pose.header.frame_id.c_str()); - return; - } - InterfaceState state(scene); - state.properties().set("target_pose", target_pose); + const std::vector& grasps_ = properties().get>("grasps"); - SubTrajectory trajectory; - trajectory.setCost(0.0); + for (moveit_msgs::Grasp grasp_ : grasps_){ - rviz_marker_tools::appendFrame(trajectory.markers(), target_pose, 0.1, "grasp frame"); + 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; + } - spawn(std::move(state), std::move(trajectory)); + 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); + + // TODO: Raise exception if the sizes do not match + + 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)); + } } @@ -206,4 +243,4 @@ void GraspProviderFixedPose::compute() { /// register plugin #include PLUGINLIB_EXPORT_CLASS(moveit::task_constructor::stages::GraspProviderDefault, moveit::task_constructor::stages::GraspProviderBase) -PLUGINLIB_EXPORT_CLASS(moveit::task_constructor::stages::GraspProviderFixedPose, 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 index 2d5bac11c..1961100eb 100644 --- a/core/src/stages/grasp_provider_base.cpp +++ b/core/src/stages/grasp_provider_base.cpp @@ -12,6 +12,8 @@ #include #include +#include + namespace moveit { namespace task_constructor { namespace stages { @@ -20,8 +22,7 @@ GraspProviderBase::GraspProviderBase(const std::string& name) : GeneratePose(nam p.declare("eef", "name of end-effector"); p.declare("object"); - p.declare("pregrasp", "pregrasp posture"); - p.declare("grasp", "grasp posture"); + p.declare>("grasps", "list of Grasp messages"); } void GraspProviderBase::init(const std::shared_ptr& robot_model) { InitStageException errors; @@ -40,12 +41,7 @@ void GraspProviderBase::init(const std::shared_ptrhasEndEffector(eef)) errors.push_back(*this, "unknown end effector: " + eef); else { - // check availability of eef pose - const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef); - const std::string& name = props.get("pregrasp"); - std::map m; - if (!jmg->getVariableDefaultPositions(name, m)) - errors.push_back(*this, "unknown end effector pose: " + name); + // TODO: Validate eef_poses specified in grasps } if (errors) diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index 91e5e7614..007c80d62 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -28,8 +28,6 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& prov 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("eef_group_open_pose", "Name of open pose of eef_group"); - p.declare("eef_group_close_pose", "Name of close pose of eef_group"); p.declare>("support_surfaces", {}, "Name of support surfaces"); // internal properties (cannot be marked as such yet) @@ -49,6 +47,9 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& prov 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)); } @@ -68,8 +69,6 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& prov 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().property("pregrasp").configureInitFrom(Stage::PARENT, "eef_group_open_pose"); - provider_stage_plugin->properties().property("grasp").configureInitFrom(Stage::PARENT, "eef_group_close_pose"); provider_stage_plugin->properties().set("marker_ns", "grasp_pose"); provider_plugin_stage_ = provider_stage_plugin.get(); @@ -84,8 +83,9 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& prov wrapper = std::make_unique("place pose IK", std::move(provider_stage_plugin)); } - properties().exposeTo(wrapper->properties(), {"object", "eef_group_open_pose", "eef_group_close_pose"}); - wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group", "ik_frame", "object", "eef_group_open_pose", "eef_group_close_pose"}); + 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)); @@ -98,6 +98,7 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& prov { 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)); } @@ -106,7 +107,8 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& prov 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::PARENT, is_pick_ ? "eef_group_close_pose" : "eef_group_open_pose"); + 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 @@ -115,12 +117,14 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& prov { 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)); } @@ -131,6 +135,9 @@ PickPlaceBase::PickPlaceBase(const std::string& name, const std::string& prov 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)); } diff --git a/core/src/tasks/pick_place_task.cpp b/core/src/tasks/pick_place_task.cpp index 24a180c90..52892064b 100644 --- a/core/src/tasks/pick_place_task.cpp +++ b/core/src/tasks/pick_place_task.cpp @@ -132,17 +132,6 @@ bool PickPlaceTask::init(const Parameters& parameters) if (parameters.task_type_ == moveit_task_constructor_msgs::PlanPickPlaceGoal::PICK_ONLY || parameters.task_type_ == moveit_task_constructor_msgs::PlanPickPlaceGoal::PICK_AND_PLACE) { - /**************************************************** - * * - * 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)); - } /**************************************************** * * @@ -162,36 +151,41 @@ bool PickPlaceTask::init(const Parameters& parameters) * Pick Object * * * ***************************************************/ - { - const bool grasp_pose_defined = grasp_pose_is_defined(parameters.grasp_pose_); + { // Decide which grasp provider to use std::string grasp_provider_plugin; - if (grasp_pose_defined) - { - grasp_provider_plugin = "moveit_task_constructor/GraspProviderFixedPose"; - } - else - { + 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 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->setEndEffectorOpenClose(parameters.hand_open_pose_, parameters.hand_close_pose_); stage->setSupportSurfaces(parameters.support_surfaces_); stage->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_); - if (grasp_pose_defined) - { - stage->ProviderPlugin()->properties().set("pose", parameters.grasp_pose_); - } - else // TODO: Go through properties systematically - { - stage->ProviderPlugin()->properties().set("angle_delta", M_PI / 12); // Set plugin-specific properties - } + stage->ProviderPlugin()->properties().set("grasps", parameters.grasps_); stage->setMonitoredStage(current_state_stage_); - stage->setApproachMotion(parameters.approach_object_direction_,parameters.approach_object_min_dist_, parameters.approach_object_max_dist_); - stage->setLiftMotion(parameters.lift_object_direction_, parameters.lift_object_min_dist_, parameters.lift_object_max_dist_); + 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: Set plugin-specific properties systematically + } attach_object_stage_ = stage->attachStage(); t.add(std::move(stage)); } diff --git a/msgs/action/PlanPickPlace.action b/msgs/action/PlanPickPlace.action index b521a5099..ca4d00a33 100644 --- a/msgs/action/PlanPickPlace.action +++ b/msgs/action/PlanPickPlace.action @@ -18,7 +18,7 @@ string[] support_surfaces # Picking string grasp_provider_plugin_name -moveit_msgs/Grasp grasp +moveit_msgs/Grasp[] grasps geometry_msgs/Pose grasp_frame_transform # Placing From 540eb8c2a9080a8d6376692b8a0900f1f1eddf1d Mon Sep 17 00:00:00 2001 From: Artur Istvan Karoly Date: Tue, 27 Apr 2021 10:35:25 +0200 Subject: [PATCH 20/23] Make Place container work with PlaceLocation messages --- .../src/plan_pick_place_capability.cpp | 10 +-- .../task_constructor/stages/grasp_provider.h | 4 +- .../task_constructor/stages/place_provider.h | 15 +++- .../task_constructor/tasks/pick_place_task.h | 27 +------ ...ion_planning_stages_plugin_description.xml | 10 ++- core/src/stages/grasp_provider.cpp | 8 +- core/src/stages/place_provider.cpp | 78 +++++++++++++++++-- core/src/stages/place_provider_base.cpp | 4 + core/src/tasks/pick_place_task.cpp | 40 +++++++--- msgs/action/PlanPickPlace.action | 2 +- 10 files changed, 135 insertions(+), 63 deletions(-) diff --git a/capabilities/src/plan_pick_place_capability.cpp b/capabilities/src/plan_pick_place_capability.cpp index 5dae14d03..9f25fafd2 100644 --- a/capabilities/src/plan_pick_place_capability.cpp +++ b/capabilities/src/plan_pick_place_capability.cpp @@ -70,17 +70,9 @@ void PlanPickPlaceCapability::goalCallback( 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.hand_open_pose_ = "open"; - parameters.hand_close_pose_ = "close"; parameters.place_provider_plugin_name_ = goal->place_provider_plugin_name; - parameters.place_pose_ = goal->place_location.place_pose; - parameters.place_object_direction_ = goal->place_location.pre_place_approach.direction; - parameters.place_object_min_dist_ = goal->place_location.pre_place_approach.min_distance; - parameters.place_object_max_dist_ = goal->place_location.pre_place_approach.desired_distance; - parameters.retract_direction_ = goal->place_location.post_place_retreat.direction; - parameters.retract_min_dist_ = goal->place_location.post_place_retreat.min_distance; - parameters.retract_max_dist_ = goal->place_location.post_place_retreat.desired_distance; + parameters.place_locations_ = goal->place_locations; // Initialize task and plan if (pick_place_task_->init(parameters)){ diff --git a/core/include/moveit/task_constructor/stages/grasp_provider.h b/core/include/moveit/task_constructor/stages/grasp_provider.h index 547f4c47c..067f0e5ca 100644 --- a/core/include/moveit/task_constructor/stages/grasp_provider.h +++ b/core/include/moveit/task_constructor/stages/grasp_provider.h @@ -33,7 +33,7 @@ *********************************************************************/ /* Authors: Michael Goerner, Artur Karoly - Desc: Base class for grasp provider plaugins and default plugin + Desc: Grasp provider plugins and default plugin */ #pragma once @@ -45,8 +45,6 @@ namespace moveit { namespace task_constructor { namespace stages { -/// Base class for Grasp Provider Plugins - /// Default Grasp Provider plugin implementing the functionality of the GenerateGraspPose stage class GraspProviderDefault : public GraspProviderBase diff --git a/core/include/moveit/task_constructor/stages/place_provider.h b/core/include/moveit/task_constructor/stages/place_provider.h index f15d40d4f..bfc6e1f8e 100644 --- a/core/include/moveit/task_constructor/stages/place_provider.h +++ b/core/include/moveit/task_constructor/stages/place_provider.h @@ -33,7 +33,7 @@ *********************************************************************/ /* Authors: Robert Haschke, Artur Karoly - Desc: Base class for place provider plaugins and default plugin + Desc: Place provider plaugins and default plugin */ #pragma once @@ -45,8 +45,6 @@ namespace moveit { namespace task_constructor { namespace stages { -/// Base class for Place Provider Plugins - /// Default Place Provider plugin implementing the functionality of the GeneratePlacePose stage class PlaceProviderDefault : public PlaceProviderBase @@ -56,6 +54,17 @@ class PlaceProviderDefault : public PlaceProviderBase 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/tasks/pick_place_task.h b/core/include/moveit/task_constructor/tasks/pick_place_task.h index b3dcd1e35..b964a51d2 100644 --- a/core/include/moveit/task_constructor/tasks/pick_place_task.h +++ b/core/include/moveit/task_constructor/tasks/pick_place_task.h @@ -69,6 +69,7 @@ #include #include #include +#include #include @@ -97,17 +98,7 @@ class PickPlaceTask // 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_; // Plugins std::string grasp_provider_plugin_name_; @@ -115,24 +106,10 @@ class PickPlaceTask // Pick metrics std::vector grasps_; - geometry_msgs::PoseStamped grasp_pose_; Eigen::Isometry3d grasp_frame_transform_; - geometry_msgs::Vector3Stamped approach_object_direction_; - double approach_object_min_dist_; - double approach_object_max_dist_; - geometry_msgs::Vector3Stamped lift_object_direction_; - double lift_object_min_dist_; - double lift_object_max_dist_; // Place metrics - geometry_msgs::PoseStamped place_pose_; - double place_surface_offset_; - geometry_msgs::Vector3Stamped place_object_direction_; - double place_object_min_dist_; - double place_object_max_dist_; - geometry_msgs::Vector3Stamped retract_direction_; - double retract_min_dist_; - double retract_max_dist_; + std::vector place_locations_; }; PickPlaceTask(const std::string& task_name); diff --git a/core/motion_planning_stages_plugin_description.xml b/core/motion_planning_stages_plugin_description.xml index 6c3e0d2cb..3919688c9 100644 --- a/core/motion_planning_stages_plugin_description.xml +++ b/core/motion_planning_stages_plugin_description.xml @@ -21,7 +21,7 @@ type="moveit::task_constructor::stages::GraspProviderFixedPoses" base_class_type="moveit::task_constructor::stages::GraspProviderBase"> - Grasp Provider plugin for setting a single grasp pose + Grasp Provider plugin for setting a set of grasp poses @@ -32,4 +32,12 @@ 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/stages/grasp_provider.cpp b/core/src/stages/grasp_provider.cpp index 21f4c3fd9..672ac6678 100644 --- a/core/src/stages/grasp_provider.cpp +++ b/core/src/stages/grasp_provider.cpp @@ -80,7 +80,7 @@ void GraspProviderDefault::init(const core::RobotModelConstPtr& robot_model) { if (!robot_model->hasEndEffector(eef)) errors.push_back(*this, "unknown end effector: " + eef); else { - // TODO: Validate eef_poses specified in grasps + // TODO(karolyartur): Validate eef_poses specified in grasps if (props.get>("grasps").empty()) errors.push_back(*this, "no grasp provided"); } @@ -130,7 +130,7 @@ void GraspProviderDefault::compute() { 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); - // TODO: Raise exception if the sizes do not match + // TODO(karolyartur): Raise exception if the sizes do not match state.properties().set("hand_open_pose", hand_open_pose); state.properties().set("hand_close_pose", hand_close_pose); @@ -173,7 +173,7 @@ void GraspProviderFixedPoses::init(const core::RobotModelConstPtr& robot_model) if (!robot_model->hasEndEffector(eef)) errors.push_back(*this, "unknown end effector: " + eef); else { - // TODO: Validate eef_poses specified in grasps + // TODO(karolyartur): Validate eef_poses specified in grasps if (props.get>("grasps").empty()) errors.push_back(*this, "no grasp provided"); } @@ -220,7 +220,7 @@ void GraspProviderFixedPoses::compute() { 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); - // TODO: Raise exception if the sizes do not match + // TODO(karolyartur): Raise exception if the sizes do not match state.properties().set("hand_open_pose", hand_open_pose); state.properties().set("hand_close_pose", hand_close_pose); diff --git a/core/src/stages/place_provider.cpp b/core/src/stages/place_provider.cpp index c7072b304..45c0f060c 100644 --- a/core/src/stages/place_provider.cpp +++ b/core/src/stages/place_provider.cpp @@ -45,14 +45,12 @@ #include #include +#include + namespace moveit { namespace task_constructor { namespace stages { -// ------------------- -// PlaceProviderBase -// ------------------- - // ------------------- // PlaceProviderDefault // ------------------- @@ -70,7 +68,9 @@ void PlaceProviderDefault::compute() { // current object_pose w.r.t. planning frame const Eigen::Isometry3d& orig_object_pose = object->getGlobalCollisionBodyTransforms()[0]; - const geometry_msgs::PoseStamped& pose_msg = props.get("pose"); + 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 @@ -83,7 +83,7 @@ void PlaceProviderDefault::compute() { 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, this](const Eigen::Isometry3d& nominal, uint z_flips, + 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 @@ -103,6 +103,24 @@ void PlaceProviderDefault::compute() { 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); @@ -136,6 +154,53 @@ void PlaceProviderDefault::compute() { // 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 @@ -143,3 +208,4 @@ void PlaceProviderDefault::compute() { /// 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 index 067dcb169..76ed37ffe 100644 --- a/core/src/stages/place_provider_base.cpp +++ b/core/src/stages/place_provider_base.cpp @@ -13,6 +13,9 @@ #include #include #include "moveit/task_constructor/stages/place_provider_base.h" + +#include + namespace moveit { namespace task_constructor { namespace stages { @@ -20,6 +23,7 @@ PlaceProviderBase::PlaceProviderBase(const std::string& name) : GeneratePose(nam 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(); diff --git a/core/src/tasks/pick_place_task.cpp b/core/src/tasks/pick_place_task.cpp index 52892064b..94059d673 100644 --- a/core/src/tasks/pick_place_task.cpp +++ b/core/src/tasks/pick_place_task.cpp @@ -156,7 +156,7 @@ bool PickPlaceTask::init(const Parameters& parameters) 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 are specified"); + "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)){ @@ -179,12 +179,13 @@ bool PickPlaceTask::init(const Parameters& parameters) 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: Set plugin-specific properties systematically + // TODO(karolyartur): Set plugin-specific properties systematically } attach_object_stage_ = stage->attachStage(); t.add(std::move(stage)); @@ -213,24 +214,41 @@ bool PickPlaceTask::init(const Parameters& parameters) * Place Object * * * *****************************************************/ - Stage* detach_object_stage = nullptr; // Forward detach_object_stage to place pose generator - { - auto stage = std::make_unique("Place object", parameters.place_provider_plugin_name_, place_provider_class_loader_.get()); + { // 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->setEndEffectorOpenClose(parameters.hand_open_pose_, parameters.hand_close_pose_); 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->setPlacePose(parameters.place_pose_); - stage->setPlaceMotion(parameters.place_object_direction_, parameters.place_object_min_dist_, parameters.place_object_max_dist_); - stage->setRetractMotion(parameters.retract_direction_, parameters.retract_min_dist_, parameters.retract_max_dist_); - detach_object_stage = stage->detachStage(); + 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)); } } @@ -239,7 +257,7 @@ bool PickPlaceTask::init(const Parameters& parameters) bool PickPlaceTask::plan() { ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions"); try { - task_->plan(10); // TODO: parameterize + task_->plan(1); // TODO(karolyartur): parameterize } catch (InitStageException& e) { ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e); return false; diff --git a/msgs/action/PlanPickPlace.action b/msgs/action/PlanPickPlace.action index ca4d00a33..9a7be907e 100644 --- a/msgs/action/PlanPickPlace.action +++ b/msgs/action/PlanPickPlace.action @@ -23,7 +23,7 @@ geometry_msgs/Pose grasp_frame_transform # Placing string place_provider_plugin_name -moveit_msgs/PlaceLocation place_location +moveit_msgs/PlaceLocation[] place_locations --- From 02d19e853a868dc4e8c86bc6da3701b72063574d Mon Sep 17 00:00:00 2001 From: Artur Istvan Karoly Date: Tue, 27 Apr 2021 16:09:17 +0200 Subject: [PATCH 21/23] Fix PredicateFilter Stage --- core/src/stages/predicate_filter.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) 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 From 4b32609bf8a3656b6e3f842afe4e03dc3b461fb6 Mon Sep 17 00:00:00 2001 From: Artur Istvan Karoly Date: Wed, 28 Apr 2021 18:54:20 +0200 Subject: [PATCH 22/23] Cleanup --- .../src/plan_pick_place_capability.cpp | 6 +- .../task_constructor/stages/grasp_provider.h | 2 +- .../stages/grasp_provider_base.h | 38 ++++++++- .../moveit/task_constructor/stages/pick.h | 51 ------------ .../stages/place_provider_base.h | 38 ++++++++- .../task_constructor/tasks/pick_place_task.h | 5 +- core/src/stages/grasp_provider.cpp | 80 +++++++++++++++++-- core/src/stages/grasp_provider_base.cpp | 41 ++++++++-- core/src/stages/pick.cpp | 56 ------------- core/src/stages/place_provider_base.cpp | 38 ++++++++- core/src/tasks/pick_place_task.cpp | 13 +-- 11 files changed, 227 insertions(+), 141 deletions(-) diff --git a/capabilities/src/plan_pick_place_capability.cpp b/capabilities/src/plan_pick_place_capability.cpp index 9f25fafd2..850bbd46d 100644 --- a/capabilities/src/plan_pick_place_capability.cpp +++ b/capabilities/src/plan_pick_place_capability.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Henning Kayser */ +/* Author: Henning Kayser, Artur Karoly*/ #include "plan_pick_place_capability.h" @@ -58,7 +58,7 @@ void PlanPickPlaceCapability::goalCallback( const moveit_task_constructor_msgs::PlanPickPlaceGoalConstPtr& goal) { moveit_task_constructor_msgs::PlanPickPlaceResult result; - // TODO: fill parameters + // Fill parameters PickPlaceTask::Parameters parameters; parameters.task_type_ = goal->task_type; parameters.arm_group_name_ = goal->arm_group_name; @@ -89,7 +89,7 @@ void PlanPickPlaceCapability::goalCallback( } void PlanPickPlaceCapability::preemptCallback() { - // TODO(henningkayser): abort planning + pick_place_task_->preempt(); } } // namespace move_group diff --git a/core/include/moveit/task_constructor/stages/grasp_provider.h b/core/include/moveit/task_constructor/stages/grasp_provider.h index 067f0e5ca..ed13165f3 100644 --- a/core/include/moveit/task_constructor/stages/grasp_provider.h +++ b/core/include/moveit/task_constructor/stages/grasp_provider.h @@ -57,7 +57,7 @@ class GraspProviderDefault : public GraspProviderBase }; -/// Grasp Provider plugin for setting a single grasp pose +/// Grasp Provider plugin for setting a set of grasp poses class GraspProviderFixedPoses : public GraspProviderBase { diff --git a/core/include/moveit/task_constructor/stages/grasp_provider_base.h b/core/include/moveit/task_constructor/stages/grasp_provider_base.h index d71b151f6..dfc831a3f 100644 --- a/core/include/moveit/task_constructor/stages/grasp_provider_base.h +++ b/core/include/moveit/task_constructor/stages/grasp_provider_base.h @@ -1,6 +1,38 @@ -// -// Created by jafar_abdi on 3/27/21. -// +/********************************************************************* + * 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 diff --git a/core/include/moveit/task_constructor/stages/pick.h b/core/include/moveit/task_constructor/stages/pick.h index 178637bf8..b192c2bb9 100644 --- a/core/include/moveit/task_constructor/stages/pick.h +++ b/core/include/moveit/task_constructor/stages/pick.h @@ -134,21 +134,11 @@ class PickPlaceBase : public SerialContainer /// ------------------------- /// setters of substage properties - /// approach / place - void setApproachPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance); - void setApproachPlace(const geometry_msgs::Vector3Stamped& direction, double min_distance, double max_distance); - void setApproachPlace(const std::map& joints); - /// 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); - /// lift / retract - void setLiftRetract(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance); - void setLiftRetract(const geometry_msgs::Vector3Stamped& direction, double min_distance, double max_distance); - void setLiftRetract(const std::map& joints); - /// ------------------------- /// getters, for further configuration @@ -168,26 +158,6 @@ class Pick : public PickPlaceBase void setMonitoredStage(Stage* monitored); - void setApproachMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { - setApproachPlace(motion, min_distance, max_distance); - } - - void setApproachMotion(const geometry_msgs::Vector3Stamped& direction, double min_distance, double max_distance) { - setApproachPlace(direction, min_distance, max_distance); - } - - void setApproachMotion(const std::map& joints) { setApproachPlace(joints); } - - void setLiftMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { - setLiftRetract(motion, min_distance, max_distance); - } - - void setLiftMotion(const geometry_msgs::Vector3Stamped& direction, double min_distance, double max_distance) { - setLiftRetract(direction, min_distance, max_distance); - } - - void setLiftMotion(const std::map& joints) { setLiftRetract(joints); } - moveit::task_constructor::Stage* attachStage() {return attach_detach_stage_;} }; @@ -200,27 +170,6 @@ class Place : public PickPlaceBase void setMonitoredStage(Stage* monitored); - void setPlacePose(const geometry_msgs::PoseStamped& pose); - - void setRetractMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { - setLiftRetract(motion, min_distance, max_distance); - } - - void setRetractMotion(const geometry_msgs::Vector3Stamped& direction, double min_distance, double max_distance) { - setLiftRetract(direction, min_distance, max_distance); - } - - void setRetractMotion(const std::map& joints) { setLiftRetract(joints); } - - void setPlaceMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { - setApproachPlace(motion, min_distance, max_distance); - } - void setPlaceMotion(const geometry_msgs::Vector3Stamped& direction, double min_distance, double max_distance) { - setApproachPlace(direction, min_distance, max_distance); - } - - void setPlaceMotion(const std::map& joints) { setApproachPlace(joints); } - moveit::task_constructor::Stage* detachStage() {return attach_detach_stage_;} }; } // namespace stages diff --git a/core/include/moveit/task_constructor/stages/place_provider_base.h b/core/include/moveit/task_constructor/stages/place_provider_base.h index cd1090d93..4bf6d2f13 100644 --- a/core/include/moveit/task_constructor/stages/place_provider_base.h +++ b/core/include/moveit/task_constructor/stages/place_provider_base.h @@ -1,6 +1,38 @@ -// -// Created by jafar_abdi on 3/27/21. -// +/********************************************************************* + * 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 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 b964a51d2..4dd69f23b 100644 --- a/core/include/moveit/task_constructor/tasks/pick_place_task.h +++ b/core/include/moveit/task_constructor/tasks/pick_place_task.h @@ -30,8 +30,8 @@ * 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 +/* Author: Henning Kayser, Simon Goldstein, Artur Karoly + Desc: MoveIt Task Constructor Task for the PickPlace Capability */ // ROS @@ -118,6 +118,7 @@ class PickPlaceTask bool init(const Parameters& parameters); bool plan(); + bool preempt(); void getSolutionMsg(moveit_task_constructor_msgs::Solution& solution); private: diff --git a/core/src/stages/grasp_provider.cpp b/core/src/stages/grasp_provider.cpp index 672ac6678..3f2359b42 100644 --- a/core/src/stages/grasp_provider.cpp +++ b/core/src/stages/grasp_provider.cpp @@ -46,6 +46,8 @@ #include +#include + namespace moveit { namespace task_constructor { namespace stages { @@ -80,9 +82,42 @@ void GraspProviderDefault::init(const core::RobotModelConstPtr& robot_model) { if (!robot_model->hasEndEffector(eef)) errors.push_back(*this, "unknown end effector: " + eef); else { - // TODO(karolyartur): Validate eef_poses specified in grasps 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) @@ -130,13 +165,9 @@ void GraspProviderDefault::compute() { 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); - // TODO(karolyartur): Raise exception if the sizes do not match - state.properties().set("hand_open_pose", hand_open_pose); state.properties().set("hand_close_pose", hand_close_pose); - // props.exposeTo(state.properties(), { "pregrasp", "grasp" }); - SubTrajectory trajectory; trajectory.setCost(0.0); trajectory.setComment(std::to_string(current_angle)); @@ -173,9 +204,44 @@ void GraspProviderFixedPoses::init(const core::RobotModelConstPtr& robot_model) if (!robot_model->hasEndEffector(eef)) errors.push_back(*this, "unknown end effector: " + eef); else { - // TODO(karolyartur): Validate eef_poses specified in grasps 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) @@ -220,8 +286,6 @@ void GraspProviderFixedPoses::compute() { 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); - // TODO(karolyartur): Raise exception if the sizes do not match - state.properties().set("hand_open_pose", hand_open_pose); state.properties().set("hand_close_pose", hand_close_pose); diff --git a/core/src/stages/grasp_provider_base.cpp b/core/src/stages/grasp_provider_base.cpp index 1961100eb..aa73931e1 100644 --- a/core/src/stages/grasp_provider_base.cpp +++ b/core/src/stages/grasp_provider_base.cpp @@ -1,6 +1,38 @@ -// -// Created by jafar_abdi on 3/27/21. -// +/********************************************************************* + * 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 @@ -40,9 +72,6 @@ void GraspProviderBase::init(const std::shared_ptr("eef"); if (!robot_model->hasEndEffector(eef)) errors.push_back(*this, "unknown end effector: " + eef); - else { - // TODO: Validate eef_poses specified in grasps - } if (errors) throw errors; diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index 007c80d62..69262f175 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -201,33 +201,6 @@ void PickPlaceBase::setIKFrame(const Eigen::Isometry3d& pose, const std::stri /// ------------------------- /// setters of substage properties - -/// approach / place - -template -void PickPlaceBase::setApproachPlace(const geometry_msgs::TwistStamped& motion, double min_distance, - double max_distance) { - auto& p = move_there_stage_->properties(); - p.set("direction", motion); - p.set("min_distance", min_distance); - p.set("max_distance", max_distance); -} - -template -void PickPlaceBase::setApproachPlace(const geometry_msgs::Vector3Stamped& direction, double min_distance, - double max_distance) { - auto& p = move_there_stage_->properties(); - p.set("direction", direction); - p.set("min_distance", min_distance); - p.set("max_distance", max_distance); -} - -template -void PickPlaceBase::setApproachPlace(const std::map& joints) { - auto& p = move_there_stage_->properties(); - p.set("joints", joints); -} - /// IK computation template @@ -249,31 +222,6 @@ void PickPlaceBase::setIgnoreIKCollisions(const bool& ignore_ik_collisions) { } -/// lift / retract - -template -void PickPlaceBase::setLiftRetract(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { - auto& p = move_back_stage_->properties(); - p.set("direction", motion); - p.set("min_distance", min_distance); - p.set("max_distance", max_distance); -} - -template -void PickPlaceBase::setLiftRetract(const geometry_msgs::Vector3Stamped& direction, double min_distance, double max_distance) { - auto& p = move_back_stage_->properties(); - p.set("direction", direction); - p.set("min_distance", min_distance); - p.set("max_distance", max_distance); -} - -template -void PickPlaceBase::setLiftRetract(const std::map& joints) { - auto& p = move_back_stage_->properties(); - p.set("joints", joints); -} - - /// ------------------------- /// setters of pick properties @@ -289,10 +237,6 @@ void Place::setMonitoredStage(Stage* monitored) { provider_plugin_stage_->setMonitoredStage(monitored); } -void Place::setPlacePose(const geometry_msgs::PoseStamped& pose) { - provider_plugin_stage_->setPose(pose); -} - } // namespace stages } // namespace task_constructor } // namespace moveit diff --git a/core/src/stages/place_provider_base.cpp b/core/src/stages/place_provider_base.cpp index 76ed37ffe..a08bbcd52 100644 --- a/core/src/stages/place_provider_base.cpp +++ b/core/src/stages/place_provider_base.cpp @@ -1,6 +1,38 @@ -// -// Created by jafar_abdi on 3/27/21. -// +/********************************************************************* + * 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 diff --git a/core/src/tasks/pick_place_task.cpp b/core/src/tasks/pick_place_task.cpp index 94059d673..5fc3e94e1 100644 --- a/core/src/tasks/pick_place_task.cpp +++ b/core/src/tasks/pick_place_task.cpp @@ -30,8 +30,8 @@ * 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 +/* Author: Henning Kayser, Simon Goldstein, Artur Karoly + Desc: MoveIt Task Constructor Task for the PickPlace Capability */ #include @@ -65,8 +65,6 @@ bool grasp_pose_is_defined(const geometry_msgs::PoseStamped& grasp_pose) bool 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 if(task_){ task_->clear(); task_->loadRobotModel(); @@ -99,7 +97,7 @@ bool PickPlaceTask::init(const Parameters& parameters) auto _current_state = std::make_unique("current state"); _current_state->setTimeout(10); - // Verify that object is not attached + // 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) { @@ -269,6 +267,11 @@ bool PickPlaceTask::plan() { return true; } +bool PickPlaceTask::preempt() { + task_->preempt(); + return true; +} + void PickPlaceTask::getSolutionMsg(moveit_task_constructor_msgs::Solution& solution) { task_->solutions().front()->fillMessage(solution); } From a25d17163973d97f5924c8e0ebd9994bc8725e79 Mon Sep 17 00:00:00 2001 From: Artur Istvan Karoly Date: Tue, 18 May 2021 20:57:56 +0200 Subject: [PATCH 23/23] Cleanup and change name --- .../stages/{pick.h => pick_place.h} | 0 .../task_constructor/tasks/pick_place_task.h | 2 +- core/src/stages/CMakeLists.txt | 4 ++-- core/src/stages/{pick.cpp => pick_place.cpp} | 2 +- core/test/pick_pr2.cpp | 2 +- core/test/pick_ur5.cpp | 2 +- msgs/action/PlanPickPlace.action | 23 +++++++++++-------- 7 files changed, 20 insertions(+), 15 deletions(-) rename core/include/moveit/task_constructor/stages/{pick.h => pick_place.h} (100%) rename core/src/stages/{pick.cpp => pick_place.cpp} (99%) diff --git a/core/include/moveit/task_constructor/stages/pick.h b/core/include/moveit/task_constructor/stages/pick_place.h similarity index 100% rename from core/include/moveit/task_constructor/stages/pick.h rename to core/include/moveit/task_constructor/stages/pick_place.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 index 4dd69f23b..7eefe7f75 100644 --- a/core/include/moveit/task_constructor/tasks/pick_place_task.h +++ b/core/include/moveit/task_constructor/tasks/pick_place_task.h @@ -53,7 +53,7 @@ #include #include #include -#include +#include #include #include diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 247797b39..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 @@ -41,7 +41,7 @@ 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}) diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick_place.cpp similarity index 99% rename from core/src/stages/pick.cpp rename to core/src/stages/pick_place.cpp index 69262f175..d73c57cf2 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick_place.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include 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/action/PlanPickPlace.action b/msgs/action/PlanPickPlace.action index 9a7be907e..8681f5aca 100644 --- a/msgs/action/PlanPickPlace.action +++ b/msgs/action/PlanPickPlace.action @@ -1,4 +1,4 @@ -# By default, the task type is +# By default, the task type is PICK_ONLY byte PICK_ONLY=0 byte PLACE_ONLY=1 byte PICK_AND_PLACE=2 @@ -9,21 +9,26 @@ string arm_group_name string hand_group_name # Robot model and links -string eef_name -string hand_frame +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 -string object_id +# 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 -string grasp_provider_plugin_name -moveit_msgs/Grasp[] grasps -geometry_msgs/Pose grasp_frame_transform +# 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 -string place_provider_plugin_name -moveit_msgs/PlaceLocation[] place_locations +# 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) ---