diff --git a/.rosinstall b/.rosinstall index 830e279d6..abfb40d37 100644 --- a/.rosinstall +++ b/.rosinstall @@ -1,5 +1,5 @@ -# This file is intended for users who want to build MoveIt! from source. -# Used with wstool, users can download source of all packages of MoveIt!. +# This file is intended for users who want to build MTC and its demos from source. +# Using wstool, users can download relevant MTC-related source packages. - git: local-name: moveit_task_constructor uri: https://github.com/ros-planning/moveit_task_constructor.git diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt new file mode 100644 index 000000000..a32d2d8af --- /dev/null +++ b/demo/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 2.8.3) +project(moveit_task_constructor_demo) + +add_compile_options(-std=c++14) + +find_package(catkin REQUIRED COMPONENTS + roscpp + moveit_core + moveit_task_constructor_core + moveit_ros_planning_interface + rosparam_shortcuts +) + +catkin_package( +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME}_lib src/pick_place_task.cpp) +set_target_properties(${PROJECT_NAME}_lib PROPERTIES OUTPUT_NAME moveit_task_constructor_demo_pick_place) +add_dependencies(${PROJECT_NAME}_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +add_executable(${PROJECT_NAME}_demo src/moveit_task_constructor_demo.cpp) +target_link_libraries(${PROJECT_NAME}_demo + ${PROJECT_NAME}_lib + ${catkin_LIBRARIES} +) +set_target_properties(${PROJECT_NAME}_demo PROPERTIES OUTPUT_NAME moveit_task_constructor_demo) +add_dependencies(${PROJECT_NAME}_demo ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +install(TARGETS ${PROJECT_NAME}_lib ${PROJECT_NAME}_demo + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) + +install(DIRECTORY launch config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/demo/README.md b/demo/README.md new file mode 100644 index 000000000..f0dd6421d --- /dev/null +++ b/demo/README.md @@ -0,0 +1,11 @@ +# moveit_task_constructor_demo + +Description: A simple pick & place demo using MoveIt Task Constructor. This uses the Panda from Franka Emika + +Developed by Henning Kayser & Simon Goldstein at [PickNik Consulting](http://picknik.ai/) + +## Run + +Run demo + + roslaunch moveit_task_constructor_demo demo.launch diff --git a/demo/config/mtc.rviz b/demo/config/mtc.rviz new file mode 100644 index 000000000..83505a9b4 --- /dev/null +++ b/demo/config/mtc.rviz @@ -0,0 +1,215 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /Motion Planning Tasks1 + - /Motion Planning Tasks1/Markers1 + Splitter Ratio: 0.5393258333206177 + Tree Height: 533 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: moveit_task_constructor/Motion Planning Tasks + Global Settings: + Task View Settings: + Task Expansion: All Expanded + Name: Motion Planning Tasks + Tasks View: + property_splitter: + - 541 + - 0 + solution_sorting: + column: 0 + order: 1 + solutions_splitter: + - 328 + - 76 + solutions_view_columns: + - 38 + - 0 + - 0 + tasks_view_columns: + - 250 + - 38 + - 38 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_task_constructor/Motion Planning Tasks + Enabled: true + Interrupt Display: false + Loop Animation: false + Markers: + All at once?: false + Value: true + approach_object: true + Name: Motion Planning Tasks + Robot: + Fixed Robot Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Use Fixed Robot Color: false + Value: "" + Robot Description: robot_description + Scene: + Attached Body Color: 150; 50; 150 + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Value: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Show Trail: false + State Display Time: 0.05 s + Task Solution Topic: /moveit_task_constructor_demo/pick_place_task/solution + Tasks: + pick_place_task: 10 + Trail Step Size: 1 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: panda_link0 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 1.9269260168075562 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.8292451500892639 + Y: -0.2452867180109024 + Z: 2.2351800055275817e-7 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.3197975158691406 + Target Frame: panda_link0 + Value: XYOrbit (rviz) + Yaw: 6.239960670471191 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 768 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + Motion Planning Tasks: + collapsed: false + Motion Planning Tasks - Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd000000020000000000000166000002a6fc020000000efb000000100044006900730070006c006100790073010000003d000002a6000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000026d000000b5000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c0069006400650072010000026f000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000294000002410000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069000000027d0000004b0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032d000001fb0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000120020002d00200053006c00690064006500720000000000ffffffff00000000000000000000000100000198000002a6fc0200000002fb0000002a004d006f00740069006f006e00200050006c0061006e006e0069006e00670020005400610073006b0073010000003d00000254000000e500fffffffb0000003c004d006f00740069006f006e00200050006c0061006e006e0069006e00670020005400610073006b00730020002d00200053006c006900640065007201000002970000004c0000004100ffffff0000028f000002a600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1433 + X: 472 + Y: 25 diff --git a/demo/config/panda_config.yaml b/demo/config/panda_config.yaml new file mode 100644 index 000000000..3546e39c8 --- /dev/null +++ b/demo/config/panda_config.yaml @@ -0,0 +1,46 @@ +# Total planning attempts +planning_attempts: 10 + +# Planning group and link names +arm_group_name: "panda_arm" +eef_name: "hand" +hand_group_name: "hand" +hand_frame: "panda_link8" + +# Poses +hand_open_pose: "open" +hand_close_pose: "close" +arm_home_pose: "ready" + +# Scene frames +world_frame: "world" +table_reference_frame: "world" +object_reference_frame: "world" +surface_link: "table" + +# Collision object for picking +# CYLINDER object specifications +object_name: "object" +object_dimensions: [0.25, 0.02] # [height, radius] +object_pose: [0.5, -0.25, 0.0, 0, 0, 0] + +# Table model +spawn_table: true +table_name: "table" +table_dimensions: [0.4, 0.5, 0.1] # [length, width, height] +table_pose: [0.5, -0.25, 0, 0, 0, 0] + +# Gripper grasp frame transform [x,y,z,r,p,y] +grasp_frame_transform: [0, 0, 0.1, 1.571, 0.785, 1.571] + +# Place pose [x,y,z,r,p,y] +place_pose: [0.6, -0.15, 0, 0, 0, 0] +place_surface_offset: 0.0001 # place offset from table + +# Valid distance range when approaching an object for picking +approach_object_min_dist: 0.1 +approach_object_max_dist: 0.15 + +# Valid height range when lifting an object after pick +lift_object_min_dist: 0.01 +lift_object_max_dist: 0.1 diff --git a/demo/include/moveit_task_constructor_demo/pick_place_task.h b/demo/include/moveit_task_constructor_demo/pick_place_task.h new file mode 100644 index 000000000..56aee8f80 --- /dev/null +++ b/demo/include/moveit_task_constructor_demo/pick_place_task.h @@ -0,0 +1,124 @@ +/********************************************************************* + * 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_task_constructor_demo { +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_; +}; +} // moveit_task_constructor_demo diff --git a/demo/launch/demo.launch b/demo/launch/demo.launch new file mode 100644 index 000000000..d6de926c0 --- /dev/null +++ b/demo/launch/demo.launch @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/demo/package.xml b/demo/package.xml new file mode 100644 index 000000000..27ef23921 --- /dev/null +++ b/demo/package.xml @@ -0,0 +1,19 @@ + + + moveit_task_constructor_demo + 0.0.1 + The moveit_task_constructor_demo package + + simon Goldstein + Henning Kayser + Henning Kayser + + BSD + + catkin + moveit_task_constructor_core + moveit_ros_planning_interface + moveit_core + rosparam_shortcuts + panda_moveit_config + diff --git a/demo/src/moveit_task_constructor_demo.cpp b/demo/src/moveit_task_constructor_demo.cpp new file mode 100644 index 000000000..89442b13d --- /dev/null +++ b/demo/src/moveit_task_constructor_demo.cpp @@ -0,0 +1,130 @@ +/********************************************************************* + * 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 + +// MTC pick/place demo implementation +#include + +#include +#include +#include +#include + +constexpr char LOGNAME[] = "moveit_task_constructor_demo"; + +void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, const moveit_msgs::CollisionObject& object) { + if (!psi.applyCollisionObject(object)) + throw std::runtime_error("Failed to spawn object: " + object.id); +} + +moveit_msgs::CollisionObject createTable() { + ros::NodeHandle pnh("~"); + std::string table_name, table_reference_frame; + std::vector table_dimensions; + geometry_msgs::Pose pose; + std::size_t errors = 0; + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_reference_frame", table_reference_frame); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose); + rosparam_shortcuts::shutdownIfError(LOGNAME, errors); + + moveit_msgs::CollisionObject object; + object.id = table_name; + object.header.frame_id = table_reference_frame; + object.primitives.resize(1); + object.primitives[0].type = shape_msgs::SolidPrimitive::BOX; + object.primitives[0].dimensions = table_dimensions; + pose.position.z -= 0.5 * table_dimensions[2]; // align surface with world + object.primitive_poses.push_back(pose); + return object; +} + +moveit_msgs::CollisionObject createObject() { + ros::NodeHandle pnh("~"); + std::string object_name, object_reference_frame; + std::vector object_dimensions; + geometry_msgs::Pose pose; + std::size_t error = 0; + error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_pose", pose); + rosparam_shortcuts::shutdownIfError(LOGNAME, error); + + moveit_msgs::CollisionObject object; + object.id = object_name; + object.header.frame_id = object_reference_frame; + object.primitives.resize(1); + object.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; + object.primitives[0].dimensions = object_dimensions; + pose.position.z += 0.5 * object_dimensions[0]; + object.primitive_poses.push_back(pose); + return object; +} + +int main(int argc, char** argv) { + ROS_INFO_NAMED(LOGNAME, "Init moveit_task_constructor_demo"); + ros::init(argc, argv, "moveit_task_constructor_demo"); + ros::NodeHandle nh; + ros::AsyncSpinner spinner(1); + spinner.start(); + + // Add table and object to planning scene + ros::Duration(1.0).sleep(); // Wait for ApplyPlanningScene service + moveit::planning_interface::PlanningSceneInterface psi; + ros::NodeHandle pnh("~"); + if (pnh.param("spawn_table", true)) + spawnObject(psi, createTable()); + spawnObject(psi, createObject()); + + // Construct and run pick/place task + moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", nh); + pick_place_task.loadParameters(); + pick_place_task.init(); + if (pick_place_task.plan()) { + ROS_INFO_NAMED(LOGNAME, "Planning succeded"); + pick_place_task.execute(); + } else { + ROS_INFO_NAMED(LOGNAME, "Planning failed"); + } + + // Keep introspection alive + ros::waitForShutdown(); + return 0; +} diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp new file mode 100644 index 000000000..6d9452447 --- /dev/null +++ b/demo/src/pick_place_task.cpp @@ -0,0 +1,450 @@ +/********************************************************************* + * 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_task_constructor_demo { +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_name_)); + Task& t = *task_; + 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" }); + 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; +} +}