├── .clang-format ├── .clang-tidy ├── .gitignore ├── .rosinstall ├── README.md ├── deep_grasp_task ├── CMakeLists.txt ├── README.md ├── config │ ├── calib │ │ ├── camera.intr │ │ └── camera.yaml │ └── panda_object.yaml ├── include │ └── deep_grasp_task │ │ └── deep_pick_place_task.h ├── launch │ ├── gazebo_pick_place.launch │ ├── panda.launch │ ├── panda_world.launch │ └── sensor_data_gazebo.launch ├── meshes │ ├── camera │ │ ├── kinect.dae │ │ └── kinect.jpg │ └── objects │ │ ├── bar_clamp.dae │ │ └── strawberry.dae ├── package.xml ├── src │ ├── deep_grasp_demo.cpp │ └── deep_pick_place_task.cpp └── urdf │ ├── camera │ ├── camera.urdf.xacro │ └── camera_macro.urdf.xacro │ ├── objects │ ├── bar_clamp.urdf.xacro │ ├── cylinder.urdf.xacro │ └── strawberry.urdf.xacro │ └── robots │ └── panda_camera.urdf.xacro ├── dexnet_install.sh ├── dexnet_requirements.txt ├── moveit_task_constructor_dexnet ├── CMakeLists.txt ├── README.md ├── config │ └── dexnet_config.yaml ├── data │ ├── grasps │ │ └── grasp_candidates.bin │ └── images │ │ ├── depth_berry.png │ │ ├── depth_clamp.png │ │ ├── depth_cylinder.png │ │ ├── depth_cylinder_overhead.png │ │ ├── depth_object.png │ │ ├── rgb_berry.png │ │ ├── rgb_clamp.png │ │ ├── rgb_cylinder.png │ │ ├── rgb_cylinder_overhead.png │ │ └── rgb_object.png ├── include │ └── moveit_task_constructor_dexnet │ │ ├── grasp_detection.h │ │ └── image_server.h ├── launch │ └── dexnet_demo.launch ├── media │ ├── cylinder_grasp.png │ ├── gqcnn_barclamp_gazebo2.gif │ ├── gqcnn_berry.png │ ├── gqcnn_berry_gazebo.gif │ ├── gqcnn_clamp.png │ ├── gqcnn_cylinder_gazebo.gif │ └── mtc_gqcnn_panda.gif ├── package.xml ├── scripts │ └── grasp_detector ├── src │ ├── gqcnn_server │ ├── grasp_detection.cpp │ ├── grasp_image_detection.cpp │ ├── image_server.cpp │ └── process_image_server.cpp └── srv │ ├── GQCNNGrasp.srv │ └── Images.srv ├── moveit_task_constructor_gpd ├── CMakeLists.txt ├── README.md ├── config │ └── gpd_config.yaml ├── data │ └── pointclouds │ │ ├── barclamp.pcd │ │ └── cylinder.pcd ├── include │ └── moveit_task_constructor_gpd │ │ ├── cloud_server.h │ │ ├── cloud_utils.h │ │ └── grasp_detection.h ├── launch │ └── gpd_demo.launch ├── media │ ├── gpd_barclamp_gazebo.gif │ ├── gpd_barclamp_gazebo2.gif │ ├── gpd_barclamp_gazebo3.gif │ ├── gpd_berry_gazebo.gif │ ├── gpd_cylinder_gazebo.gif │ ├── gpd_cylinder_gazebo2.gif │ └── mtc_gpd_panda.gif ├── package.xml ├── src │ ├── cloud_server.cpp │ ├── grasp_cloud_detection.cpp │ ├── grasp_detection.cpp │ ├── moveit_task_constructor_gpd │ │ └── cloud_utils.cpp │ └── point_cloud_server.cpp └── srv │ └── PointCloud.srv ├── opencv_install.sh └── pcl_install.sh /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AccessModifierOffset: -2 4 | ConstructorInitializerIndentWidth: 2 5 | AlignEscapedNewlinesLeft: false 6 | AlignTrailingComments: true 7 | AllowAllParametersOfDeclarationOnNextLine: false 8 | AllowShortIfStatementsOnASingleLine: false 9 | AllowShortLoopsOnASingleLine: false 10 | AllowShortFunctionsOnASingleLine: None 11 | AllowShortLoopsOnASingleLine: false 12 | AlwaysBreakTemplateDeclarations: true 13 | AlwaysBreakBeforeMultilineStrings: false 14 | BreakBeforeBinaryOperators: false 15 | BreakBeforeTernaryOperators: false 16 | BreakConstructorInitializersBeforeComma: true 17 | BinPackParameters: true 18 | ColumnLimit: 120 19 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 20 | DerivePointerBinding: false 21 | PointerBindsToType: true 22 | ExperimentalAutoDetectBinPacking: false 23 | IndentCaseLabels: true 24 | MaxEmptyLinesToKeep: 1 25 | NamespaceIndentation: None 26 | ObjCSpaceBeforeProtocolList: true 27 | PenaltyBreakBeforeFirstCallParameter: 19 28 | PenaltyBreakComment: 60 29 | PenaltyBreakString: 100 30 | PenaltyBreakFirstLessLess: 1000 31 | PenaltyExcessCharacter: 1000 32 | PenaltyReturnTypeOnItsOwnLine: 70 33 | SpacesBeforeTrailingComments: 2 34 | Cpp11BracedListStyle: false 35 | Standard: Auto 36 | IndentWidth: 2 37 | TabWidth: 2 38 | UseTab: Never 39 | IndentFunctionDeclarationAfterType: false 40 | SpacesInParentheses: false 41 | SpacesInAngles: false 42 | SpaceInEmptyParentheses: false 43 | SpacesInCStyleCastParentheses: false 44 | SpaceAfterControlStatementKeyword: true 45 | SpaceBeforeAssignmentOperators: true 46 | ContinuationIndentWidth: 4 47 | SortIncludes: false 48 | SpaceAfterCStyleCast: false 49 | 50 | # Configure each individual brace in BraceWrapping 51 | BreakBeforeBraces: Custom 52 | 53 | # Control of individual brace wrapping cases 54 | BraceWrapping: { 55 | AfterClass: 'true' 56 | AfterControlStatement: 'true' 57 | AfterEnum : 'true' 58 | AfterFunction : 'true' 59 | AfterNamespace : 'true' 60 | AfterStruct : 'true' 61 | AfterUnion : 'true' 62 | BeforeCatch : 'true' 63 | BeforeElse : 'true' 64 | IndentBraces : 'false' 65 | } 66 | ... 67 | -------------------------------------------------------------------------------- /.clang-tidy: -------------------------------------------------------------------------------- 1 | --- 2 | Checks: '-*, 3 | performance-*, 4 | llvm-namespace-comment, 5 | modernize-redundant-void-arg, 6 | modernize-use-nullptr, 7 | modernize-use-default, 8 | modernize-use-override, 9 | modernize-loop-convert, 10 | readability-named-parameter, 11 | readability-redundant-smartptr-get, 12 | readability-redundant-string-cstr, 13 | readability-simplify-boolean-expr, 14 | readability-container-size-empty, 15 | readability-identifier-naming, 16 | ' 17 | HeaderFilterRegex: '' 18 | AnalyzeTemporaryDtors: false 19 | CheckOptions: 20 | - key: llvm-namespace-comment.ShortNamespaceLines 21 | value: '10' 22 | - key: llvm-namespace-comment.SpacesBeforeComments 23 | value: '2' 24 | - key: readability-braces-around-statements.ShortStatementLines 25 | value: '2' 26 | # type names 27 | - key: readability-identifier-naming.ClassCase 28 | value: CamelCase 29 | - key: readability-identifier-naming.EnumCase 30 | value: CamelCase 31 | - key: readability-identifier-naming.UnionCase 32 | value: CamelCase 33 | # method names 34 | - key: readability-identifier-naming.MethodCase 35 | value: camelBack 36 | # variable names 37 | - key: readability-identifier-naming.VariableCase 38 | value: lower_case 39 | - key: readability-identifier-naming.ClassMemberSuffix 40 | value: '_' 41 | # const static or global variables are UPPER_CASE 42 | - key: readability-identifier-naming.EnumConstantCase 43 | value: UPPER_CASE 44 | - key: readability-identifier-naming.StaticConstantCase 45 | value: UPPER_CASE 46 | - key: readability-identifier-naming.ClassConstantCase 47 | value: UPPER_CASE 48 | - key: readability-identifier-naming.GlobalVariableCase 49 | value: UPPER_CASE 50 | ... 51 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | cmake_install.cmake 2 | build/ 3 | bin/ 4 | lib/ 5 | msg_gen/ 6 | srv_gen/ 7 | *.o 8 | *.so 9 | *.a 10 | !*.c 11 | !Makefile 12 | -------------------------------------------------------------------------------- /.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: deep_grasp_demo 3 | uri: https://github.com/PickNikRobotics/deep_grasp_demo.git 4 | version: master 5 | - git: 6 | local-name: moveit_task_constructor 7 | uri: https://github.com/bostoncleek/moveit_task_constructor.git 8 | version: pr-deep_grasp_stage 9 | # - git: 10 | # local-name: panda_moveit_config 11 | # uri: https://github.com/tahsinkose/panda_moveit_config.git 12 | # version: melodic-devel 13 | # - git: 14 | # local-name: franka_ros 15 | # uri: https://github.com/tahsinkose/franka_ros.git 16 | # version: simulation 17 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Deep Grasp Demo 2 | 3 | 4 | 1) [Overview](#Overview)
5 | 2) [Packages](#Packages)
6 | 3) [Getting Started](#Getting-Started)
7 | 4) [Install Grasp Pose Detection](#Install-Grasp-Pose-Detection)
8 | 5) [Install Dex-Net](#Install-Dex-Net)
9 | 6) [Download ROS Packages](#Download-ROS-Packages)
10 | 7) [Launching Demos and Further Details](#Launching-Demos-and-Further-Details)
11 | 8) [Depth Sensor Data](#Depth-Sensor-Data)
12 | 9) [Camera View Point](#Camera-View-Point)
13 | 10) [Known Issues](#Known-Issues)
14 | 15 | ## Overview 16 | This repository contains several demos 17 | using deep learning methods for grasp pose generation within the MoveIt Task Constructor. 18 | 19 | The packages were developed and tested on Ubuntu 18.04 running ROS Melodic. 20 | 21 | 22 | ## Packages 23 | * [deep_grasp_task](https://github.com/PickNikRobotics/deep_grasp_demo/tree/master/deep_grasp_task): constructs a pick and place task using deep learning methods 24 | for the grasp generation stage within the MoveIt Task Constructor 25 | 26 | * [moveit_task_constructor_dexnet](https://github.com/PickNikRobotics/deep_grasp_demo/tree/master/moveit_task_constructor_dexnet): uses [Dex-Net](https://berkeleyautomation.github.io/dex-net/) to sample grasps from a depth image 27 | 28 | * [moveit_task_constructor_gpd](https://github.com/PickNikRobotics/deep_grasp_demo/tree/master/moveit_task_constructor_gpd): uses [GPD](https://github.com/atenpas/gpd) to sample grasps from 3D point clouds 29 | 30 | 31 | ## Getting Started 32 | First, Complete the [Getting Started Tutorial](https://ros-planning.github.io/moveit_tutorials/doc/getting_started/getting_started.html). 33 | 34 | Before installing the dependencies it is recommended to run: 35 | ``` 36 | sudo apt update 37 | sudo apt upgrade 38 | ``` 39 | 40 | **Important Note**: It is recommended to install dependencies that are not ROS packages outside of the 41 | catkin workspace. For GPD this includes PCL, OpenCV, and the GPD library. For Dex-Net this includes [gqcnn](https://github.com/BerkeleyAutomation/gqcnn), [autolab_core](https://github.com/BerkeleyAutomation/autolab_core), [perception](https://github.com/BerkeleyAutomation/perception), and [visualization](https://github.com/BerkeleyAutomation/visualization). The steps bellow will walk you through the installation. 42 | 43 | 44 | ## Install Grasp Pose Detection 45 | 1) Requirements 46 | * PCL >= 1.9: The `pcl_install.sh` script will install PCL 1.11 47 | ``` 48 | wget https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/master/pcl_install.sh 49 | chmod +x pcl_install.sh 50 | sudo ./pcl_install.sh 51 | ``` 52 | 53 | * OpenCV >= 3.4: The `opencv_install.sh` script will install OpenCV 3.4 54 | ``` 55 | wget https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/master/opencv_install.sh 56 | chmod +x opencv_install.sh 57 | sudo ./opencv_install.sh 58 | ``` 59 | 60 | * Eigen >= 3.0: If ROS is installed then this requirement is satisfied 61 | 62 | 2) Clone th GPD library 63 | ``` 64 | git clone https://github.com/atenpas/gpd 65 | ``` 66 | 67 | 3) Modify CMakeLists.txt 68 | 69 | First, remove the `-03` compiler optimization. This optimization can cause 70 | a segmentation fault on 18.04. 71 | 72 | ``` 73 | set(CMAKE_CXX_FLAGS "-fopenmp -fPIC -Wno-deprecated -Wenum-compare -Wno-ignored-attributes -std=c++14") 74 | ``` 75 | 76 | Next, update the `find_package()` functions for the `PCL` and `OpenCV` 77 | versions installed. If you ran the above install scripts `CMakeLists.txt` should read: 78 | 79 | ``` 80 | find_package(PCL 1.11 REQUIRED) 81 | find_package(OpenCV 3.4 REQUIRED) 82 | ``` 83 | 84 | 4) Build 85 | ``` 86 | cd gpd 87 | mkdir build && cd build 88 | cmake .. 89 | make -j 90 | sudo make install 91 | ``` 92 | 93 | 5) Configuration File Path 94 | 95 | In `moveit_task_constructor_gpd/config/gpd_congfig.yaml` navigate to line 33 and update `weights_file` to contain the absolute file path to the location of the [lenet params](https://github.com/atenpas/gpd/tree/master/models/lenet/15channels/params) directory. This directory contains the learned model weights and is located where the GPD repository was cloned. 96 | 97 | 98 | ## Install Dex-Net 99 | 1) It is recommended to upgrade pip and to create a virtual environment 100 | prior to running the install script in the next step. 101 | ``` 102 | python3 -m pip install --upgrade pip 103 | ``` 104 | 105 | 2) Run the install script to download the requirements
106 | If you have a GPU this option will install tensorflow with GPU support. This script 107 | will install packages for Python 3. 108 | ``` 109 | wget https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/master/dexnet_install.sh 110 | wget https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/master/dexnet_requirements.txt 111 | chmod +x dexnet_install.sh 112 | ./dexnet_install.sh {cpu|gpu} 113 | ``` 114 | 115 | 3) Download the pretrained models 116 | ``` 117 | ./dexnet_deps/gqcnn/scripts/downloads/models/download_models.sh 118 | ``` 119 | 120 | 4) Configuration File Paths 121 | 122 | In `moveit_task_constructor_gpd/config/dexnet_config.yaml` specify the absolute file paths to the `model_dir` and `model_params` parameters for the Dex-Net 4.0 parallel jaw configuration. The `model_name` is already set to use the Dex-Net 4.0 parallel jaw configuration. The `model_dir` parameter specifies the path to the learned model weights located in `gqcnn/cfg/examples/replication/dex-net_4.0_pj.yaml` and the `model_params` parameter specifies the model configuration located in `gqcnn/models`. If you use the `dexnet_install.sh` script the `gqcnn` directory will be located inside the `dexnet_deps` directory. 123 | 124 | 125 | ## Download ROS Packages 126 | ### Setup New Workspace 127 | For now it is recommended to create a new workspace to prevent conflicts between packages. This will be especially helpful if you want to use Gazebo with the demos. 128 | ``` 129 | mkdir -p ~/ws_grasp/src 130 | cd ~/ws_grasp/src 131 | wstool init 132 | wstool merge https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/master/.rosinstall 133 | wstool update 134 | 135 | rosdep install --from-paths . --ignore-src --rosdistro $ROS_DISTRO 136 | ``` 137 | 138 | Note: Here you will need to extend the `ws_grasp` to the `ws_moveit` that was created from the [Getting Started Tutorial](https://ros-planning.github.io/moveit_tutorials/doc/getting_started/getting_started.html). 139 | ``` 140 | cd ~/ws_grasp 141 | catkin config --extend /devel --cmake-args -DCMAKE_BUILD_TYPE=Release 142 | catkin build 143 | ``` 144 | 145 | 146 | ### Panda Gazebo Support (Optional) 147 | You will need the C++ Franka Emika library. This can be installed from [source](https://github.com/frankaemika/libfranka) or by executing: 148 | ``` 149 | sudo apt install ros-melodic-libfranka 150 | ``` 151 | 152 | You will need two additional packages. 153 | ``` 154 | git clone https://github.com/tahsinkose/panda_moveit_config.git -b melodic-devel 155 | git clone https://github.com/tahsinkose/franka_ros.git -b simulation 156 | ``` 157 | 158 | 159 | ## Launching Demos and Further Details 160 | To see how to launch the demos using GPD and Dex-Net see the [moveit_task_constructor_gpd](https://github.com/PickNikRobotics/deep_grasp_demo/tree/master/moveit_task_constructor_gpd) and [moveit_task_constructor_dexnet](https://github.com/PickNikRobotics/deep_grasp_demo/tree/master/moveit_task_constructor_dexnet) packages. 161 | 162 | 163 | ## Depth Sensor Data 164 | ### Collecting Data using Gazebo 165 | Perhaps you want to collect depth sensor data on an object and use fake controllers to execute the motion plan. The launch file `sensor_data_gazebo.launch` will launch a `process_image_server` and a `point_cloud_server` node. These will provide services to save either images or point clouds. 166 | Images will be saved to `moveit_task_constructor_dexnet/data/images` and point clouds saved to `moveit_task_constructor_gpd/data/pointclouds`. 167 | 168 | To collect either images or point clouds run: 169 | ``` 170 | roslaunch deep_grasp_task sensor_data_gazebo.launch 171 | ``` 172 | 173 | To save the depth and color images: 174 | ``` 175 | rosservice call /save_images "depth_file: 'my_depth_image.png' 176 | color_file: 'my_color_image.png'" 177 | 178 | ``` 179 | 180 | To save a point cloud: 181 | ``` 182 | rosservice call /save_point_cloud "cloud_file: 'my_cloud_file.pcd'" 183 | ``` 184 | 185 | 186 | ## Camera View Point 187 | Initially, the camera is setup to view the cylinder from the side of the robot. It is useful particularly for Dex-Net to place the camera in an overhead position above the object. To change the camera view point there are a few files to modify. You can move the camera to a preset overhead position or follow the general format to create a new position. 188 | 189 | First, modify the camera or the panda + camera urdf. 190 | 191 | If you want to move the camera position just for collecting sensor data, in `deep_grasp_task/urdf/camera/camera.urdf.xacro` change the camera xacro macro line to read: 192 | ```XML 193 | 194 | ``` 195 | 196 | If you want to move the camera position and use the robot to execute trajectories. Go to `deep_grasp_task/urdf/robots/panda_camera.urdf.xacro` and change the camera xacro macro line to read: 197 | ```XML 198 | 199 | ``` 200 | 201 | Next, specify the transformation from the robot base link to the camera link. 202 | 203 | Change `deep_grasp_task/config/calib/camera.yaml` to read: 204 | ```YAML 205 | trans_base_cam: [0.500, 0.000, 0.700, 0.707, 0.000, 0.707, 0.000] 206 | ``` 207 | 208 | Finally, this is optional depending on whether the camera is added to the planning scene. If the camera is in the planning scene you need to modify `deep_grasp_task/config/panda_object.yaml` to read: 209 | ```YAML 210 | spawn_camera: true 211 | camera_pose: [0.5, 0, 0.7, 0, 1.571, 1.571] 212 | ``` 213 | 214 | ## Known Issues 215 | 1) When running with Gazebo 216 | ``` 217 | ros.moveit_simple_controller_manager.SimpleControllerManager: Controller panda_hand_controller failed with error GOAL_TOLERANCE_VIOLATED: 218 | ros.moveit_ros_planning.trajectory_execution_manager: Controller handle panda_hand_controller reports status ABORTED 219 | ``` 220 | 221 | 2) Planning may fail 222 | 223 | If using GPD, increase the number of points sampled by setting `num_samples` in `config/gpd_config.yaml`. 224 | Another option is to run either algorithm again. Maybe low quality grasps were sampled or they were not kinematically feasible. 225 | -------------------------------------------------------------------------------- /deep_grasp_task/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(deep_grasp_task) 3 | 4 | # C++ 14 5 | add_compile_options(-std=c++14) 6 | 7 | # Warnings 8 | add_definitions(-W -Wall -Wextra 9 | -Wwrite-strings -Wunreachable-code -Wpointer-arith 10 | -Winit-self -Wredundant-decls 11 | -Wno-unused-parameter -Wno-unused-function) 12 | 13 | # Find catkin macros and libraries 14 | find_package(catkin REQUIRED COMPONENTS 15 | actionlib 16 | moveit_core 17 | moveit_ros_planning_interface 18 | moveit_task_constructor_core 19 | moveit_task_constructor_msgs 20 | roscpp 21 | rosparam_shortcuts 22 | ) 23 | 24 | ################################### 25 | ## Catkin specific configuration ## 26 | ################################### 27 | catkin_package( 28 | INCLUDE_DIRS include 29 | CATKIN_DEPENDS 30 | moveit_task_constructor_msgs 31 | roscpp 32 | ) 33 | 34 | ########### 35 | ## Build ## 36 | ########### 37 | 38 | # Specify additional locations of header files 39 | # Your package locations should be listed before other locations 40 | include_directories( 41 | include 42 | ${catkin_INCLUDE_DIRS} 43 | ) 44 | 45 | # Declare a C++ executable 46 | add_executable(deep_grasp_demo 47 | src/deep_grasp_demo.cpp 48 | src/deep_pick_place_task.cpp 49 | ) 50 | 51 | # Specify libraries to link a library or executable target against 52 | target_link_libraries(deep_grasp_demo 53 | ${catkin_LIBRARIES} 54 | ) 55 | 56 | ############# 57 | ## Install ## 58 | ############# 59 | 60 | # Mark executables and/or libraries for installation 61 | install( 62 | TARGETS 63 | deep_grasp_demo 64 | ARCHIVE DESTINATION 65 | ${CATKIN_PACKAGE_LIB_DESTINATION} 66 | LIBRARY DESTINATION 67 | ${CATKIN_PACKAGE_LIB_DESTINATION} 68 | RUNTIME DESTINATION 69 | ${CATKIN_PACKAGE_BIN_DESTINATION} 70 | ) 71 | 72 | # Mark cpp header files for installation 73 | install( 74 | DIRECTORY 75 | include/${PROJECT_NAME}/ 76 | DESTINATION 77 | ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 78 | ) 79 | 80 | # # Mark roslaunch files for installation 81 | # install( 82 | # DIRECTORY 83 | # launch 84 | # DESTINATION 85 | # ${CATKIN_PACKAGE_SHARE_DESTINATION} 86 | # ) 87 | # 88 | # # Mark config files for installation 89 | # install( 90 | # DIRECTORY 91 | # config 92 | # DESTINATION 93 | # ${CATKIN_PACKAGE_SHARE_DESTINATION} 94 | # ) 95 | -------------------------------------------------------------------------------- /deep_grasp_task/README.md: -------------------------------------------------------------------------------- 1 | # Deep Grasp Task 2 | 3 | 4 | ## Overview 5 | This package constructs a pick and place task using the MoveIt Task Constructor. It also provides tools for collecting depth sensor data that can be used with the deep grasping libraries. 6 | 7 | ## Nodes 8 | ### deep_grasp_demo 9 | This node is renamed at launch to mtc_tutorial. It constructs the pick and place task and adds objects to the planning scene, published on the `planning_scene` topic. 10 | 11 | ## Config 12 | * camera.intr: depth camera intrinsic parameters used by Dex-Net 13 | 14 | * camera.yaml: depth camera extrinsic parameters used to transform the grasp candidates from the depth camera optical link to the frame of the base link of the robot 15 | 16 | * panda_object.yaml: Panda configurations and object pick and place configurations 17 | -------------------------------------------------------------------------------- /deep_grasp_task/config/calib/camera.intr: -------------------------------------------------------------------------------- 1 | {"_cy": 240.5, "_cx": 320.5, "_fy": 554.254691191187, "_fx": 554.254691191187, "_height": 480, "_width": 640, "_skew": 0.0, "_frame": "camera_depth_optical_frame"} 2 | -------------------------------------------------------------------------------- /deep_grasp_task/config/calib/camera.yaml: -------------------------------------------------------------------------------- 1 | # camera extrinsics 2 | # transformation from camera link to optical link [x y z qw qx qy qz] 3 | trans_cam_opt: [0.0, 0.0, 0.0, 0.5, -0.5, 0.5, -0.5] 4 | 5 | # transformation from base link to camera link [x y z qw qx qy qz] 6 | # when camera is to the right of the robot (cylinder) 7 | trans_base_cam: [0.0, -0.25, 0.04, 0.0, 0.0, 0.0, 0.0] 8 | # when camera is in the overhead position 9 | # trans_base_cam: [0.500, 0.000, 0.700, 0.707, 0.000, 0.707, 0.000] 10 | -------------------------------------------------------------------------------- /deep_grasp_task/config/panda_object.yaml: -------------------------------------------------------------------------------- 1 | # Total planning attempts 2 | max_solutions: 10 3 | 4 | # Planning group and link names 5 | arm_group_name: "panda_arm" 6 | eef_name: "hand" 7 | hand_group_name: "hand" 8 | hand_frame: "panda_link8" 9 | 10 | # Poses 11 | hand_open_pose: "open" 12 | hand_close_pose: "close" 13 | arm_home_pose: "ready" 14 | 15 | # Scene frames 16 | world_frame: "world" 17 | table_reference_frame: "world" 18 | object_reference_frame: "world" 19 | camera_reference_frame: "world" 20 | surface_link: "table" 21 | 22 | # Collision object for picking 23 | # CYLINDER object specifications 24 | spawn_mesh: false 25 | object_name: "object" 26 | object_dimensions: [0.25, 0.02] # [height, radius] 27 | object_pose: [0.5, -0.25, 0.0, 0, 0, 0] 28 | 29 | # # BAR CLAMP 30 | # spawn_mesh: true 31 | # object_name: "object" 32 | # object_mesh_file: "package://deep_grasp_task/meshes/objects/bar_clamp.dae" 33 | # object_dimensions: [0.06, 0.12, 0.04] # [length, width, height] 34 | # object_pose: [0.5, 0, 0.03, 0, 0, 0] 35 | 36 | # STRAWBERRY 37 | # spawn_mesh: true 38 | # object_name: "object" 39 | # object_mesh_file: "package://deep_grasp_task/meshes/objects/strawberry.dae" 40 | # object_dimensions: [0.02] # [radius] 41 | # object_pose: [0.5, 0, 0., 0, 0, 0] 42 | 43 | # Camera collision object 44 | spawn_camera: false 45 | camera_name: "camera" 46 | camera_mesh_file: "package://deep_grasp_task/meshes/camera/kinect.dae" 47 | camera_pose: [0.5, 0, 0.7, 0, 0, 0] # side 48 | # camera_pose: [0.5, 0, 0.7, 0, 1.571, 1.571] # over head 49 | 50 | # Table model 51 | spawn_table: true 52 | table_name: "table" 53 | table_dimensions: [0.4, 0.5, 0.1] # [length, width, height] 54 | table_pose: [0.5, -0.25, 0, 0, 0, 0] 55 | 56 | # Gripper grasp frame transform [x,y,z,r,p,y] 57 | grasp_frame_transform: [0, 0, 0.1, 1.571, 0.785, 1.571] 58 | 59 | # Place pose [x,y,z,r,p,y] 60 | # place_pose: [0.5, 0.3, 0, 0, 0, 0] 61 | place_pose: [0.6, -0.15, 0, 0, 0, 0] 62 | place_surface_offset: 0.0001 # place offset from table 63 | 64 | # Valid distance range when approaching an object for picking 65 | approach_object_min_dist: 0.1 66 | approach_object_max_dist: 0.15 67 | 68 | # Valid height range when lifting an object after pick 69 | lift_object_min_dist: 0.01 70 | lift_object_max_dist: 0.1 71 | -------------------------------------------------------------------------------- /deep_grasp_task/include/deep_grasp_task/deep_pick_place_task.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2020 PickNik Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************/ 32 | 33 | /* Author: Henning Kayser, Simon Goldstein, Boston Cleek 34 | Desc: A demo to show MoveIt Task Constructor using a deep learning based 35 | grasp generator 36 | */ 37 | 38 | #pragma once 39 | 40 | // ROS 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | // MoveIt 47 | #include 48 | #include 49 | #include 50 | 51 | // MTC 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | #include 68 | 69 | namespace deep_grasp_task 70 | { 71 | using namespace moveit::task_constructor; 72 | 73 | class DeepPickPlaceTask 74 | { 75 | public: 76 | DeepPickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh); 77 | ~DeepPickPlaceTask() = default; 78 | 79 | void loadParameters(); 80 | 81 | void init(); 82 | 83 | bool plan(); 84 | 85 | bool execute(); 86 | 87 | private: 88 | ros::NodeHandle nh_; 89 | 90 | std::string task_name_; 91 | moveit::task_constructor::TaskPtr task_; 92 | 93 | // planning group properties 94 | std::string arm_group_name_; 95 | std::string eef_name_; 96 | std::string hand_group_name_; 97 | std::string hand_frame_; 98 | 99 | // object + surface 100 | std::vector support_surfaces_; 101 | std::string object_reference_frame_; 102 | std::string surface_link_; 103 | std::string object_name_; 104 | std::string world_frame_; 105 | std::vector object_dimensions_; 106 | 107 | // Predefined pose targets 108 | std::string hand_open_pose_; 109 | std::string hand_close_pose_; 110 | std::string arm_home_pose_; 111 | 112 | // Deep grasp properties 113 | std::string action_name_; 114 | 115 | // Execution 116 | actionlib::SimpleActionClient execute_; 117 | 118 | // Pick metrics 119 | Eigen::Isometry3d grasp_frame_transform_; 120 | double approach_object_min_dist_; 121 | double approach_object_max_dist_; 122 | double lift_object_min_dist_; 123 | double lift_object_max_dist_; 124 | 125 | // Place metrics 126 | geometry_msgs::Pose place_pose_; 127 | double place_surface_offset_; 128 | }; 129 | } // namespace deep_grasp_task 130 | -------------------------------------------------------------------------------- /deep_grasp_task/launch/gazebo_pick_place.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 16 | 17 | 18 | 21 | 22 | -------------------------------------------------------------------------------- /deep_grasp_task/launch/panda.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | [/move_group/fake_controller_joint_states] 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /deep_grasp_task/launch/panda_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /deep_grasp_task/launch/sensor_data_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | $(arg xyz_lower_limits) 22 | $(arg xyz_upper_limits) 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 46 | 47 | 48 | 49 | 50 | 52 | 53 | 54 | 57 | 58 | 59 | 62 | 63 | -------------------------------------------------------------------------------- /deep_grasp_task/meshes/camera/kinect.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/deep_grasp_task/meshes/camera/kinect.jpg -------------------------------------------------------------------------------- /deep_grasp_task/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | deep_grasp_task 4 | 0.0.1 5 | Constucts a pick place task using deep learning for grasp synthesis 6 | 7 | Boston Cleek 8 | Boston Cleek 9 | 10 | BSD 11 | 12 | catkin 13 | actionlib 14 | moveit_task_constructor_core 15 | moveit_task_constructor_msgs 16 | moveit_ros_planning_interface 17 | moveit_core 18 | roscpp 19 | rosparam_shortcuts 20 | 21 | -------------------------------------------------------------------------------- /deep_grasp_task/src/deep_grasp_demo.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2020 PickNik Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************/ 32 | 33 | /* Author: Henning Kayser, Simon Goldstein, Boston Cleek 34 | Desc: A demo to show MoveIt Task Constructor using a deep learning based 35 | grasp generator 36 | */ 37 | 38 | // ROS 39 | #include 40 | 41 | // MTC demo implementation 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | #include 50 | 51 | #include 52 | #include 53 | #include 54 | 55 | constexpr char LOGNAME[] = "deep_grasp_demo"; 56 | 57 | void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, const moveit_msgs::CollisionObject& object) 58 | { 59 | if (!psi.applyCollisionObject(object)) 60 | throw std::runtime_error("Failed to spawn object: " + object.id); 61 | } 62 | 63 | moveit_msgs::CollisionObject createTable() 64 | { 65 | ros::NodeHandle pnh("~"); 66 | std::string table_name, table_reference_frame; 67 | std::vector table_dimensions; 68 | geometry_msgs::Pose pose; 69 | std::size_t errors = 0; 70 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name); 71 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_reference_frame", table_reference_frame); 72 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions); 73 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose); 74 | rosparam_shortcuts::shutdownIfError(LOGNAME, errors); 75 | 76 | moveit_msgs::CollisionObject object; 77 | object.id = table_name; 78 | object.header.frame_id = table_reference_frame; 79 | object.primitives.resize(1); 80 | object.primitives[0].type = shape_msgs::SolidPrimitive::BOX; 81 | object.primitives[0].dimensions = table_dimensions; 82 | pose.position.z -= 0.5 * table_dimensions[2]; // align surface with world 83 | object.primitive_poses.push_back(pose); 84 | object.operation = moveit_msgs::CollisionObject::ADD; 85 | 86 | return object; 87 | } 88 | 89 | moveit_msgs::CollisionObject createObject() 90 | { 91 | ros::NodeHandle pnh("~"); 92 | std::string object_name, object_reference_frame; 93 | std::vector object_dimensions; 94 | geometry_msgs::Pose pose; 95 | std::size_t error = 0; 96 | error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name); 97 | error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame); 98 | error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions); 99 | error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_pose", pose); 100 | rosparam_shortcuts::shutdownIfError(LOGNAME, error); 101 | 102 | moveit_msgs::CollisionObject object; 103 | object.id = object_name; 104 | object.header.frame_id = object_reference_frame; 105 | object.primitives.resize(1); 106 | object.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; 107 | object.primitives[0].dimensions = object_dimensions; 108 | pose.position.z += 0.5 * object_dimensions[0]; 109 | object.primitive_poses.push_back(pose); 110 | object.operation = moveit_msgs::CollisionObject::ADD; 111 | 112 | return object; 113 | } 114 | 115 | moveit_msgs::CollisionObject createCamera() 116 | { 117 | ros::NodeHandle pnh("~"); 118 | std::string camera_name, camera_reference_frame, camera_mesh_file; 119 | geometry_msgs::Pose pose; 120 | std::size_t error = 0; 121 | error += !rosparam_shortcuts::get(LOGNAME, pnh, "camera_name", camera_name); 122 | error += !rosparam_shortcuts::get(LOGNAME, pnh, "camera_mesh_file", camera_mesh_file); 123 | error += !rosparam_shortcuts::get(LOGNAME, pnh, "camera_reference_frame", camera_reference_frame); 124 | error += !rosparam_shortcuts::get(LOGNAME, pnh, "camera_pose", pose); 125 | rosparam_shortcuts::shutdownIfError(LOGNAME, error); 126 | 127 | shapes::Mesh* obj_mesh = shapes::createMeshFromResource(camera_mesh_file); 128 | 129 | shapes::ShapeMsg mesh_msg; 130 | shapes::constructMsgFromShape(obj_mesh, mesh_msg); 131 | shape_msgs::Mesh mesh = boost::get(mesh_msg); 132 | 133 | moveit_msgs::CollisionObject object; 134 | object.id = camera_name; 135 | object.header.frame_id = camera_reference_frame; 136 | object.meshes.emplace_back(mesh); 137 | object.mesh_poses.emplace_back(pose); 138 | object.operation = moveit_msgs::CollisionObject::ADD; 139 | 140 | return object; 141 | } 142 | 143 | moveit_msgs::CollisionObject createObjectMesh() 144 | { 145 | ros::NodeHandle pnh("~"); 146 | std::string object_name, object_reference_frame, object_mesh_file; 147 | std::vector object_dimensions; 148 | geometry_msgs::Pose pose; 149 | std::size_t error = 0; 150 | error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name); 151 | error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_mesh_file", object_mesh_file); 152 | error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame); 153 | error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions); 154 | error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_pose", pose); 155 | rosparam_shortcuts::shutdownIfError(LOGNAME, error); 156 | 157 | shapes::Mesh* obj_mesh = shapes::createMeshFromResource(object_mesh_file); 158 | 159 | shapes::ShapeMsg mesh_msg; 160 | shapes::constructMsgFromShape(obj_mesh, mesh_msg); 161 | shape_msgs::Mesh mesh = boost::get(mesh_msg); 162 | 163 | moveit_msgs::CollisionObject object; 164 | object.id = object_name; 165 | object.header.frame_id = object_reference_frame; 166 | object.meshes.emplace_back(mesh); 167 | object.mesh_poses.emplace_back(pose); 168 | object.operation = moveit_msgs::CollisionObject::ADD; 169 | 170 | // moveit_msgs::CollisionObject object; 171 | // object.id = object_name; 172 | // object.header.frame_id = object_reference_frame; 173 | // object.primitives.resize(1); 174 | // object.primitives[0].type = shape_msgs::SolidPrimitive::BOX; 175 | // object.primitives[0].dimensions = object_dimensions; 176 | // pose.position.z += 0.5 * object_dimensions[0]; 177 | // object.primitive_poses.push_back(pose); 178 | // object.operation = moveit_msgs::CollisionObject::ADD; 179 | 180 | return object; 181 | } 182 | 183 | int main(int argc, char** argv) 184 | { 185 | ROS_INFO_NAMED(LOGNAME, "Init deep_grasp_demo"); 186 | ros::init(argc, argv, "deep_grasp_demo"); 187 | ros::NodeHandle nh; 188 | 189 | ros::AsyncSpinner spinner(1); 190 | spinner.start(); 191 | 192 | // Wait for ApplyPlanningScene service 193 | ros::Duration(1.0).sleep(); 194 | 195 | // Add table and object to planning scene 196 | moveit::planning_interface::PlanningSceneInterface psi; 197 | ros::NodeHandle pnh("~"); 198 | if (pnh.param("spawn_table", true)) 199 | { 200 | spawnObject(psi, createTable()); 201 | } 202 | 203 | // Add camera to planning scene 204 | if (pnh.param("spawn_camera", true)) 205 | { 206 | spawnObject(psi, createCamera()); 207 | } 208 | 209 | // Add object to planning scene either as mesh or geometric primitive 210 | if (pnh.param("spawn_mesh", true)) 211 | { 212 | spawnObject(psi, createObjectMesh()); 213 | } 214 | else 215 | { 216 | spawnObject(psi, createObject()); 217 | } 218 | 219 | // Construct and run task 220 | deep_grasp_task::DeepPickPlaceTask deep_pick_place_task("deep_pick_place_task", nh); 221 | deep_pick_place_task.loadParameters(); 222 | deep_pick_place_task.init(); 223 | 224 | if (deep_pick_place_task.plan()) 225 | { 226 | ROS_INFO_NAMED(LOGNAME, "Planning succeded"); 227 | if (pnh.param("execute", false)) 228 | { 229 | deep_pick_place_task.execute(); 230 | ROS_INFO_NAMED(LOGNAME, "Execution complete"); 231 | } 232 | else 233 | { 234 | ROS_INFO_NAMED(LOGNAME, "Execution disabled"); 235 | } 236 | } 237 | else 238 | { 239 | ROS_INFO_NAMED(LOGNAME, "Planning failed"); 240 | } 241 | 242 | // Keep introspection alive 243 | ros::waitForShutdown(); 244 | return 0; 245 | } 246 | -------------------------------------------------------------------------------- /deep_grasp_task/src/deep_pick_place_task.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2020 PickNik Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************/ 32 | 33 | /* Author: Henning Kayser, Simon Goldstein, Boston Cleek 34 | Desc: A demo to show MoveIt Task Constructor using a deep learning based 35 | grasp generator 36 | */ 37 | 38 | #include 39 | #include 40 | 41 | namespace deep_grasp_task 42 | { 43 | constexpr char LOGNAME[] = "pick_place_task"; 44 | DeepPickPlaceTask::DeepPickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh) 45 | : nh_(nh), task_name_(task_name), execute_("execute_task_solution", true) 46 | { 47 | } 48 | 49 | void DeepPickPlaceTask::loadParameters() 50 | { 51 | /**************************************************** 52 | * * 53 | * Load Parameters * 54 | * * 55 | ***************************************************/ 56 | ROS_INFO_NAMED(LOGNAME, "Loading task parameters"); 57 | ros::NodeHandle pnh("~"); 58 | 59 | // Planning group properties 60 | size_t errors = 0; 61 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_group_name", arm_group_name_); 62 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_group_name", hand_group_name_); 63 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "eef_name", eef_name_); 64 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_frame", hand_frame_); 65 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "world_frame", world_frame_); 66 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "grasp_frame_transform", grasp_frame_transform_); 67 | 68 | // Predefined pose targets 69 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_open_pose", hand_open_pose_); 70 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_close_pose", hand_close_pose_); 71 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_home_pose", arm_home_pose_); 72 | 73 | // Target object 74 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name_); 75 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions_); 76 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame_); 77 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "surface_link", surface_link_); 78 | support_surfaces_ = { surface_link_ }; 79 | 80 | // Deep grasp properties 81 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "action_name", action_name_); 82 | 83 | // Pick/Place metrics 84 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_min_dist", approach_object_min_dist_); 85 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_max_dist", approach_object_max_dist_); 86 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_min_dist", lift_object_min_dist_); 87 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_max_dist", lift_object_max_dist_); 88 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_surface_offset", place_surface_offset_); 89 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_pose", place_pose_); 90 | rosparam_shortcuts::shutdownIfError(LOGNAME, errors); 91 | } 92 | 93 | void DeepPickPlaceTask::init() 94 | { 95 | ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); 96 | const std::string object = object_name_; 97 | 98 | // Reset ROS introspection before constructing the new object 99 | // TODO(henningkayser): verify this is a bug, fix if possible 100 | task_.reset(); 101 | task_.reset(new moveit::task_constructor::Task()); 102 | 103 | Task& t = *task_; 104 | t.stages()->setName(task_name_); 105 | t.loadRobotModel(); 106 | 107 | // Sampling planner 108 | auto sampling_planner = std::make_shared(); 109 | sampling_planner->setProperty("goal_joint_tolerance", 1e-5); 110 | 111 | // Cartesian planner 112 | auto cartesian_planner = std::make_shared(); 113 | cartesian_planner->setMaxVelocityScaling(1.0); 114 | cartesian_planner->setMaxAccelerationScaling(1.0); 115 | cartesian_planner->setStepSize(.001); 116 | 117 | // Set task properties 118 | t.setProperty("group", arm_group_name_); 119 | t.setProperty("eef", eef_name_); 120 | t.setProperty("hand", hand_group_name_); 121 | t.setProperty("hand_grasping_frame", hand_frame_); 122 | t.setProperty("ik_frame", hand_frame_); 123 | 124 | /**************************************************** 125 | * * 126 | * Current State * 127 | * * 128 | ***************************************************/ 129 | Stage* current_state_ptr = nullptr; // Forward current_state on to grasp pose generator 130 | { 131 | auto current_state = std::make_unique("current state"); 132 | 133 | // Verify that object is not attached 134 | auto applicability_filter = 135 | std::make_unique("applicability test", std::move(current_state)); 136 | applicability_filter->setPredicate([object](const SolutionBase& s, std::string& comment) { 137 | if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) 138 | { 139 | comment = "object with id '" + object + "' is already attached and cannot be picked"; 140 | return false; 141 | } 142 | return true; 143 | }); 144 | 145 | current_state_ptr = applicability_filter.get(); 146 | t.add(std::move(applicability_filter)); 147 | } 148 | 149 | /**************************************************** 150 | * * 151 | * Open Hand * 152 | * * 153 | ***************************************************/ 154 | { // Open Hand 155 | auto stage = std::make_unique("open hand", sampling_planner); 156 | stage->setGroup(hand_group_name_); 157 | stage->setGoal(hand_open_pose_); 158 | t.add(std::move(stage)); 159 | } 160 | 161 | /**************************************************** 162 | * * 163 | * Move to Pick * 164 | * * 165 | ***************************************************/ 166 | { // Move-to pre-grasp 167 | auto stage = std::make_unique( 168 | "move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); 169 | stage->setTimeout(5.0); 170 | stage->properties().configureInitFrom(Stage::PARENT); 171 | t.add(std::move(stage)); 172 | } 173 | 174 | /**************************************************** 175 | * * 176 | * Pick Object * 177 | * * 178 | ***************************************************/ 179 | Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator 180 | { 181 | auto grasp = std::make_unique("pick object"); 182 | t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" }); 183 | grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" }); 184 | 185 | /**************************************************** 186 | ---- * Approach Object * 187 | ***************************************************/ 188 | { 189 | auto stage = std::make_unique("approach object", cartesian_planner); 190 | stage->properties().set("marker_ns", "approach_object"); 191 | stage->properties().set("link", hand_frame_); 192 | stage->properties().configureInitFrom(Stage::PARENT, { "group" }); 193 | stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_); 194 | 195 | // Set hand forward direction 196 | geometry_msgs::Vector3Stamped vec; 197 | vec.header.frame_id = hand_frame_; 198 | vec.vector.z = 1.0; 199 | stage->setDirection(vec); 200 | grasp->insert(std::move(stage)); 201 | } 202 | 203 | /**************************************************** 204 | ---- * Generate Grasp Pose * 205 | ***************************************************/ 206 | { 207 | auto stage = std::make_unique>( 208 | action_name_, "generate grasp pose"); 209 | stage->properties().configureInitFrom(Stage::PARENT); 210 | stage->properties().set("marker_ns", "grasp_pose"); 211 | stage->setPreGraspPose(hand_open_pose_); 212 | stage->setObject(object); 213 | stage->setMonitoredStage(current_state_ptr); // Hook into current state 214 | 215 | // Compute IK 216 | auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); 217 | wrapper->setMaxIKSolutions(8); 218 | wrapper->setMinSolutionDistance(1.0); 219 | wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); 220 | wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); 221 | wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); 222 | grasp->insert(std::move(wrapper)); 223 | } 224 | 225 | /**************************************************** 226 | ---- * Allow Collision (hand object) * 227 | ***************************************************/ 228 | { 229 | auto stage = std::make_unique("allow collision (hand,object)"); 230 | stage->allowCollisions( 231 | object, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), 232 | true); 233 | grasp->insert(std::move(stage)); 234 | } 235 | 236 | /**************************************************** 237 | ---- * Close Hand * 238 | ***************************************************/ 239 | { 240 | auto stage = std::make_unique("close hand", sampling_planner); 241 | stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_group_name_); 242 | stage->setGoal(hand_close_pose_); 243 | grasp->insert(std::move(stage)); 244 | } 245 | 246 | /**************************************************** 247 | .... * Attach Object * 248 | ***************************************************/ 249 | { 250 | auto stage = std::make_unique("attach object"); 251 | stage->attachObject(object, hand_frame_); 252 | attach_object_stage = stage.get(); 253 | grasp->insert(std::move(stage)); 254 | } 255 | 256 | /**************************************************** 257 | .... * Allow collision (object support) * 258 | ***************************************************/ 259 | { 260 | auto stage = std::make_unique("allow collision (object,support)"); 261 | stage->allowCollisions({ object }, support_surfaces_, true); 262 | grasp->insert(std::move(stage)); 263 | } 264 | 265 | /**************************************************** 266 | .... * Lift object * 267 | ***************************************************/ 268 | { 269 | auto stage = std::make_unique("lift object", cartesian_planner); 270 | stage->properties().configureInitFrom(Stage::PARENT, { "group" }); 271 | stage->setMinMaxDistance(lift_object_min_dist_, lift_object_max_dist_); 272 | stage->setIKFrame(hand_frame_); 273 | stage->properties().set("marker_ns", "lift_object"); 274 | 275 | // Set upward direction 276 | geometry_msgs::Vector3Stamped vec; 277 | vec.header.frame_id = world_frame_; 278 | vec.vector.z = 1.0; 279 | stage->setDirection(vec); 280 | grasp->insert(std::move(stage)); 281 | } 282 | 283 | /**************************************************** 284 | .... * Forbid collision (object support) * 285 | ***************************************************/ 286 | { 287 | auto stage = std::make_unique("forbid collision (object,surface)"); 288 | stage->allowCollisions({ object }, support_surfaces_, false); 289 | grasp->insert(std::move(stage)); 290 | } 291 | 292 | // Add grasp container to task 293 | t.add(std::move(grasp)); 294 | } 295 | 296 | /****************************************************** 297 | * * 298 | * Move to Place * 299 | * * 300 | *****************************************************/ 301 | { 302 | auto stage = std::make_unique( 303 | "move to place", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); 304 | stage->setTimeout(5.0); 305 | stage->properties().configureInitFrom(Stage::PARENT); 306 | t.add(std::move(stage)); 307 | } 308 | 309 | /****************************************************** 310 | * * 311 | * Place Object * 312 | * * 313 | *****************************************************/ 314 | { 315 | auto place = std::make_unique("place object"); 316 | t.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); 317 | place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" }); 318 | 319 | /****************************************************** 320 | ---- * Lower Object * 321 | *****************************************************/ 322 | { 323 | auto stage = std::make_unique("lower object", cartesian_planner); 324 | stage->properties().set("marker_ns", "lower_object"); 325 | stage->properties().set("link", hand_frame_); 326 | stage->properties().configureInitFrom(Stage::PARENT, { "group" }); 327 | stage->setMinMaxDistance(.03, .13); 328 | 329 | // Set downward direction 330 | geometry_msgs::Vector3Stamped vec; 331 | vec.header.frame_id = world_frame_; 332 | vec.vector.z = -1.0; 333 | stage->setDirection(vec); 334 | place->insert(std::move(stage)); 335 | } 336 | 337 | /****************************************************** 338 | ---- * Generate Place Pose * 339 | *****************************************************/ 340 | { 341 | // Generate Place Pose 342 | auto stage = std::make_unique("generate place pose"); 343 | stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" }); 344 | stage->properties().set("marker_ns", "place_pose"); 345 | stage->setObject(object); 346 | 347 | // Set target pose 348 | geometry_msgs::PoseStamped p; 349 | p.header.frame_id = object_reference_frame_; 350 | p.pose = place_pose_; 351 | p.pose.position.z += 0.5 * object_dimensions_.at(0) + place_surface_offset_; 352 | stage->setPose(p); 353 | stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage 354 | 355 | // Compute IK 356 | auto wrapper = std::make_unique("place pose IK", std::move(stage)); 357 | wrapper->setMaxIKSolutions(2); 358 | wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); 359 | wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); 360 | wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); 361 | place->insert(std::move(wrapper)); 362 | } 363 | 364 | /****************************************************** 365 | ---- * Open Hand * 366 | *****************************************************/ 367 | { 368 | auto stage = std::make_unique("open hand", sampling_planner); 369 | stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_group_name_); 370 | stage->setGoal(hand_open_pose_); 371 | place->insert(std::move(stage)); 372 | } 373 | 374 | /****************************************************** 375 | ---- * Forbid collision (hand, object) * 376 | *****************************************************/ 377 | { 378 | auto stage = std::make_unique("forbid collision (hand,object)"); 379 | stage->allowCollisions( 380 | object_name_, 381 | t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), false); 382 | place->insert(std::move(stage)); 383 | } 384 | 385 | /****************************************************** 386 | ---- * Detach Object * 387 | *****************************************************/ 388 | { 389 | auto stage = std::make_unique("detach object"); 390 | stage->detachObject(object_name_, hand_frame_); 391 | place->insert(std::move(stage)); 392 | } 393 | 394 | /****************************************************** 395 | ---- * Retreat Motion * 396 | *****************************************************/ 397 | { 398 | auto stage = std::make_unique("retreat after place", cartesian_planner); 399 | stage->properties().configureInitFrom(Stage::PARENT, { "group" }); 400 | stage->setMinMaxDistance(.12, .25); 401 | stage->setIKFrame(hand_frame_); 402 | stage->properties().set("marker_ns", "retreat"); 403 | geometry_msgs::Vector3Stamped vec; 404 | vec.header.frame_id = hand_frame_; 405 | vec.vector.z = -1.0; 406 | stage->setDirection(vec); 407 | place->insert(std::move(stage)); 408 | } 409 | 410 | // Add place container to task 411 | t.add(std::move(place)); 412 | } 413 | 414 | /****************************************************** 415 | * * 416 | * Move to Home * 417 | * * 418 | *****************************************************/ 419 | { 420 | auto stage = std::make_unique("move home", sampling_planner); 421 | stage->properties().configureInitFrom(Stage::PARENT, { "group" }); 422 | stage->setGoal(arm_home_pose_); 423 | stage->restrictDirection(stages::MoveTo::FORWARD); 424 | t.add(std::move(stage)); 425 | } 426 | } 427 | 428 | bool DeepPickPlaceTask::plan() 429 | { 430 | ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions"); 431 | ros::NodeHandle pnh("~"); 432 | int max_solutions = pnh.param("max_solutions", 10); 433 | 434 | try 435 | { 436 | task_->plan(max_solutions); 437 | } 438 | catch (InitStageException& e) 439 | { 440 | ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e); 441 | return false; 442 | } 443 | if (task_->numSolutions() == 0) 444 | { 445 | ROS_ERROR_NAMED(LOGNAME, "Planning failed"); 446 | return false; 447 | } 448 | return true; 449 | } 450 | 451 | bool DeepPickPlaceTask::execute() 452 | { 453 | ROS_INFO_NAMED(LOGNAME, "Executing solution trajectory"); 454 | moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal; 455 | task_->solutions().front()->fillMessage(execute_goal.solution); 456 | execute_.sendGoal(execute_goal); 457 | execute_.waitForResult(); 458 | moveit_msgs::MoveItErrorCodes execute_result = execute_.getResult()->error_code; 459 | 460 | if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) 461 | { 462 | ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed and returned: " << execute_.getState().toString()); 463 | return false; 464 | } 465 | 466 | return true; 467 | } 468 | } // namespace deep_grasp_task 469 | -------------------------------------------------------------------------------- /deep_grasp_task/urdf/camera/camera.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /deep_grasp_task/urdf/camera/camera_macro.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | true 46 | 20 47 | 48 | 1.047198 49 | 50 | 640 51 | 480 52 | R8G8B8 53 | 54 | 55 | 0.05 56 | 8.0 57 | 58 | 59 | 60 | camera 61 | true 62 | 10 63 | rgb/image_raw 64 | depth/image_raw 65 | depth/color/points 66 | rgb/camera_info 67 | depth/camera_info 68 | camera_optical_link 69 | 0.1 70 | 0.0 71 | 0.0 72 | 0.0 73 | 0.0 74 | 0.0 75 | 0.4 76 | 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /deep_grasp_task/urdf/objects/bar_clamp.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 0.005 22 | 1000000 23 | 1.0 24 | 1000.0 25 | 1000.0 26 | 0.001 27 | 28 | 29 | -------------------------------------------------------------------------------- /deep_grasp_task/urdf/objects/cylinder.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 0.005 32 | 1000000 33 | 1.0 34 | 100.0 35 | 100.0 36 | 0.001 37 | 38 | 39 | -------------------------------------------------------------------------------- /deep_grasp_task/urdf/objects/strawberry.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 0.005 22 | 1000000 23 | 1.0 24 | 1000.0 25 | 1000.0 26 | 0.001 27 | 28 | 29 | -------------------------------------------------------------------------------- /deep_grasp_task/urdf/robots/panda_camera.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 100.0 16 | 100.0 17 | 18 | 19 | 20 | 100.0 21 | 100.0 22 | 23 | 24 | -------------------------------------------------------------------------------- /dexnet_install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # read cmd line inputs 4 | VERSION=$1 # cpu or gpu 5 | 6 | # set cpu/gpu conditional libraries 7 | case "${VERSION}" 8 | in 9 | cpu) 10 | pip install tensorflow==1.15.0 11 | ;; 12 | gpu) 13 | pip install tensorflow-gpu==1.13.1 14 | ;; 15 | *) 16 | echo "Usage: $0 {cpu|gpu}" 17 | exit 1 18 | esac 19 | 20 | # install apt deps 21 | sudo apt install cmake libvtk6-dev python-vtk6 python-sip python-qt4 libosmesa6-dev meshlab libhdf5-dev 22 | 23 | # install pip deps 24 | python3 -m pip install -r dexnet_requirements.txt 25 | 26 | # install deps from source 27 | mkdir dexnet_deps 28 | cd dexnet_deps 29 | 30 | # install autolab modules 31 | git clone https://github.com/BerkeleyAutomation/autolab_core.git 32 | git clone https://github.com/BerkeleyAutomation/perception.git 33 | git clone https://github.com/BerkeleyAutomation/gqcnn.git 34 | git clone https://github.com/BerkeleyAutomation/visualization.git 35 | 36 | 37 | # install all Berkeley AUTOLAB modules 38 | # autolab_core 39 | cd autolab_core 40 | sudo python3 setup.py develop 41 | cd .. 42 | 43 | # perception 44 | cd perception 45 | sudo python3 setup.py develop 46 | cd .. 47 | 48 | # gqcnn 49 | cd gqcnn 50 | sudo python3 setup.py develop 51 | cd .. 52 | 53 | # visualization 54 | cd visualization 55 | sudo python3 setup.py develop 56 | cd .. 57 | -------------------------------------------------------------------------------- /dexnet_requirements.txt: -------------------------------------------------------------------------------- 1 | numpy==1.18.1 2 | scipy==1.4.1 3 | scikit-learn==0.22.2.post1 4 | scikit-image==0.17.2 5 | opencv-python==4.2.0.34 6 | Shapely==1.5.3 7 | h5py==2.10.0 8 | matplotlib==2.2.0 9 | multiprocess==0.70.10 10 | dill==0.3.2 11 | cvxopt==1.2.5 12 | ipython==5.5.0 13 | pillow==5.1.0 14 | pyglet==1.5.7 15 | setproctitle==1.1.10 16 | trimesh==3.7.14 17 | -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(moveit_task_constructor_dexnet) 3 | 4 | # C++ 14 5 | add_compile_options(-std=c++14) 6 | 7 | # Warnings 8 | add_definitions(-W -Wall -Wextra 9 | -Wwrite-strings -Wunreachable-code -Wpointer-arith 10 | -Winit-self -Wredundant-decls 11 | -Wno-unused-parameter -Wno-unused-function) 12 | 13 | # Find catkin macros and libraries 14 | find_package(catkin REQUIRED COMPONENTS 15 | actionlib 16 | cv_bridge 17 | geometry_msgs 18 | image_transport 19 | message_generation 20 | moveit_task_constructor_msgs 21 | roscpp 22 | rosparam_shortcuts 23 | rospy 24 | sensor_msgs 25 | std_msgs 26 | ) 27 | 28 | # System dependencies are found with CMake's conventions 29 | find_package(Eigen3 REQUIRED) 30 | find_package(OpenCV 3.2 REQUIRED) 31 | 32 | # Service files 33 | add_service_files( 34 | FILES 35 | GQCNNGrasp.srv 36 | Images.srv 37 | ) 38 | 39 | # Message generation dependencies 40 | generate_messages( 41 | DEPENDENCIES 42 | geometry_msgs 43 | std_msgs 44 | ) 45 | 46 | ################################### 47 | ## Catkin specific configuration ## 48 | ################################### 49 | catkin_package( 50 | INCLUDE_DIRS include 51 | CATKIN_DEPENDS 52 | geometry_msgs 53 | message_runtime 54 | moveit_task_constructor_msgs 55 | roscpp 56 | rospy 57 | sensor_msgs 58 | std_msgs 59 | DEPENDS 60 | EIGEN3 61 | OpenCV 62 | ) 63 | 64 | # Specify additional locations of header files 65 | # Your package locations should be listed before other locations 66 | include_directories( 67 | SYSTEM 68 | ${EIGEN3_INCLUDE_DIRS} 69 | ${OpenCV_INCLUDE_DIRS} 70 | ) 71 | 72 | include_directories( 73 | include 74 | ${catkin_INCLUDE_DIRS} 75 | ) 76 | 77 | # Declare a C++ executable 78 | add_executable(grasp_image_detection 79 | src/grasp_detection.cpp 80 | src/grasp_image_detection.cpp 81 | ) 82 | 83 | add_executable(process_image_server 84 | src/image_server.cpp 85 | src/process_image_server.cpp 86 | ) 87 | 88 | 89 | ## Add cmake target dependencies of the executable 90 | ## same as for the library above 91 | add_dependencies(grasp_image_detection 92 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 93 | ${catkin_EXPORTED_TARGETS} 94 | ) 95 | 96 | add_dependencies(process_image_server 97 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 98 | ${catkin_EXPORTED_TARGETS} 99 | ) 100 | 101 | # Specify libraries to link a library or executable target against 102 | target_link_libraries(grasp_image_detection 103 | ${catkin_LIBRARIES} 104 | ${Eigen3_LIBRARIES} 105 | ) 106 | 107 | target_link_libraries(process_image_server 108 | ${catkin_LIBRARIES} 109 | ${OpenCV_LIBRARIES} 110 | ) 111 | 112 | ############# 113 | ## Install ## 114 | ############# 115 | 116 | # Mark python scripts for install 117 | catkin_install_python(PROGRAMS 118 | src/gqcnn_server 119 | scripts/grasp_detector 120 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 121 | ) 122 | 123 | # Mark executables and/or libraries for installation 124 | install( 125 | TARGETS 126 | grasp_image_detection process_image_server 127 | ARCHIVE DESTINATION 128 | ${CATKIN_PACKAGE_LIB_DESTINATION} 129 | LIBRARY DESTINATION 130 | ${CATKIN_PACKAGE_LIB_DESTINATION} 131 | RUNTIME DESTINATION 132 | ${CATKIN_PACKAGE_BIN_DESTINATION} 133 | ) 134 | 135 | # Mark cpp header files for installation 136 | install( 137 | DIRECTORY 138 | include/${PROJECT_NAME}/ 139 | DESTINATION 140 | ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 141 | ) 142 | 143 | # Mark roslaunch files for installation 144 | install( 145 | DIRECTORY 146 | launch 147 | DESTINATION 148 | ${CATKIN_PACKAGE_SHARE_DESTINATION} 149 | ) 150 | 151 | # Mark config files for installation 152 | install( 153 | DIRECTORY 154 | config 155 | DESTINATION 156 | ${CATKIN_PACKAGE_SHARE_DESTINATION} 157 | ) 158 | -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/README.md: -------------------------------------------------------------------------------- 1 | # MoveIt Task Constructor Dex-Net 2 | 3 | 4 | 1) [Overview](#Overview)
5 | 2) [Running](#Running)
6 | 3) [Nodes](#Nodes)
7 | 4) [Tips](#Tips)
8 | 5) [Results](#Results)
9 | 10 | ## Overview 11 | Demo showing how to use Dex-Net for grasp pose generation within the MoveIt Task Constructor. Dex-Net uses a grasp quality CNN (GQ-CNN) to compose the probability a grasp candidate will be successful. The inputs are a color and a depth image and outputs a list of the grasp candidates and their probabilities of success. 12 | 13 | The process of sampling grasps using Dex-Net is messy because the library is written in python 2 which is not officially supported in ROS Melodic. The `grasp_image_detection` node calls the `gqcnn_grasp` service offered by the `gqcnn_server` node which returns the grasp candidates. The `gqcnn_server` node interacts with a python 3 `grasp_detector` script running the Dex-Net 4.0 policy. The images are saved by the `process_image_server` node and the files are then loaded by `grasp_detector` script. 14 | 15 | 16 | ## Running 17 | ### Using Fake Controllers 18 | You don't need Gazebo for this one. The images for the cylinder were collected ahead of time and located in `data/images/depth_cylinder.png` and `data/images/rgb_cylinder.png`. 19 | 20 | To run the pick and place demo: 21 | ``` 22 | roslaunch moveit_task_constructor_demo demo.launch 23 | roslaunch moveit_task_constructor_dexnet dexnet_demo.launch 24 | ``` 25 | 26 | ### Using Gazebo 27 | This demo allows you to execute the motion plan in Gazebo. You have the option to load the images from a file or use the simulated depth camera. The `load_images` argument in `dexnet_demo.launch` specifies whether or not to load the images from a file. Set `load_images:=false` to use the simulated depth camera. Both the `color_image_file` and `depth_image_file` arguments in `dexnet_demo.launch` specify the name of the color and depth images to load. 28 | 29 | launch the robot in Gazebo: 30 | ``` 31 | roslaunch deep_grasp_task gazebo_pick_place.launch 32 | ``` 33 | 34 | **Important Note:** It is recommended to use the provided images for the cylinder demo. The Dex-Net data set contains only images using a downward facing camera, therefore, the GQ-CNN works best using images taken from above. The provided images of the cylinder have been tested and work with this demo. If you use set `load_images:=false` the demo is less likely to work. However, if you change the camera position to overhead as described in the `Camera View Point` section and use a different object such as the bar clamp then Dex-Net will perform better when `load_images:=false`. 35 | 36 | To run the pick and place demo: 37 | ``` 38 | roslaunch moveit_task_constructor_dexnet dexnet_demo.launch 39 | ``` 40 | 41 | 42 | ## Nodes 43 | ### grasp_image_detection 44 | This node bridges the gap between Dex-Net and the MoveIt Task Constructor. Communication with the MoveIt Task Constructor is achieved using ROS action messages. The action client sends the grasp candidates along with the costs back to the `DeepGraspPose` stage. When an action goal is received the `gqcnn_grasp` service is called and the grasp candidates from Dex-Net are received. The images can either be loaded from a file or the node will call the `save_images` service to collect the data. 45 | 46 | ### gqcnn_server 47 | This node interacts with a python 3 `grasp_detector` script running the Dex-Net 4.0 policy. The `grasp_detector` script is executed as a subprocess to the node and communication with the node is achieved through a Unix Pipe. This node offers a `gqcnn_grasp` service that returns the sampled grasps and their probabilities of success. 48 | 49 | ### process_image_server 50 | This node is used primarily to save images. It subscribes to color and depth image topic names of type `sensor_msgs/Image`. It offers a `save_images` service that will save the color and depth images to the user specified file names. 51 | 52 | 53 | ## Tips 54 | Adjust some of the parameters in `gqcnn/cfg/examples/replication/dex-net_4.0_pj.yaml`. The `num_seed_samples` parameter determines how many grasps are sampled from the depth image. The `deterministic` parameter will deterministically sample the grasp candidates. Set this to 0 to get different results. 55 | 56 | 57 | ## Results 58 | The output to the cylinder demo using fake controller should look like this. 59 | 60 |

61 | 62 |

63 | 64 | Here is the highest ranked grasp candidate with a 64% probability of success. When the images are taken overhead this probability is better. 65 | 66 |

67 | 68 |

69 | 70 | Panda picking up the cylinder in Gazebo. 71 | 72 |

73 | 74 |

75 | 76 | Now lets try a few more difficult objects, first, the strawberry. 77 | 78 |

79 | 80 |

81 | 82 | The most difficult to grasp is the bar clamp. 83 | 84 |

85 | 86 |

87 | 88 | Here are the highest ranked grasp candidates for the strawberry and bar clamp viewed from above. 89 | 90 |

91 | 92 | 93 |

94 | -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/config/dexnet_config.yaml: -------------------------------------------------------------------------------- 1 | # file path to neural network weights and model config 2 | model_name: "GQCNN-4.0-PJ" 3 | model_dir: "/home/bostoncleek/dex-net/deps/gqcnn/models/GQCNN-4.0-PJ" 4 | model_params: "/home/bostoncleek/dex-net/deps/gqcnn/cfg/examples/replication/dex-net_4.0_pj.yaml" 5 | -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/data/grasps/grasp_candidates.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_dexnet/data/grasps/grasp_candidates.bin -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/data/images/depth_berry.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_dexnet/data/images/depth_berry.png -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/data/images/depth_clamp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_dexnet/data/images/depth_clamp.png -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/data/images/depth_cylinder.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_dexnet/data/images/depth_cylinder.png -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/data/images/depth_cylinder_overhead.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_dexnet/data/images/depth_cylinder_overhead.png -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/data/images/depth_object.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_dexnet/data/images/depth_object.png -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/data/images/rgb_berry.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_dexnet/data/images/rgb_berry.png -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/data/images/rgb_clamp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_dexnet/data/images/rgb_clamp.png -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/data/images/rgb_cylinder.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_dexnet/data/images/rgb_cylinder.png -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/data/images/rgb_cylinder_overhead.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_dexnet/data/images/rgb_cylinder_overhead.png -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/data/images/rgb_object.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_dexnet/data/images/rgb_object.png -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/include/moveit_task_constructor_dexnet/grasp_detection.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2020 PickNik Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************/ 32 | 33 | /* Author: Boston Cleek 34 | Desc: Samples grasp candidates using GQ-CNN and Dex-Net model weights/data sets 35 | */ 36 | 37 | // C++ 38 | #include 39 | #include 40 | 41 | // Eigen 42 | #include 43 | 44 | // Action Server 45 | #include 46 | #include 47 | 48 | namespace moveit_task_constructor_dexnet 49 | { 50 | constexpr char LOGNAME[] = "grasp_image_detection"; 51 | 52 | /** 53 | * @brief Generates grasp poses for a generator stage with MTC 54 | * @details Interfaces with the GQ-CNN and Dex-Net data sets using ROS messages 55 | * and interfaces with MTC using an Action Server. 56 | */ 57 | class GraspDetection 58 | { 59 | public: 60 | /** 61 | * @brief Constructor 62 | * @param nh - node handle 63 | * @details loads parameter and registers callbacks for the action server 64 | */ 65 | GraspDetection(const ros::NodeHandle& nh); 66 | 67 | private: 68 | /** 69 | * @brief Loads parameters for action server, image data, and relevant transformations 70 | */ 71 | void loadParameters(); 72 | 73 | /** 74 | * @brief Initialize action server callbacks and Image service client (optional) 75 | */ 76 | void init(); 77 | 78 | /** 79 | * @brief Action server goal callback 80 | * @details Accepts goal from client and samples grasp candidates 81 | */ 82 | void goalCallback(); 83 | 84 | /** 85 | * @brief Preempt callback 86 | * @details Preempts goal 87 | */ 88 | void preemptCallback(); 89 | 90 | /** 91 | * @brief Requests images by calling the save_images service 92 | * @details If Images are not loaded from the data directory they need to be 93 | * requested. This assumes a camera is publishing the images. 94 | */ 95 | void requestImages(); 96 | 97 | /** 98 | * @brief Samples grasp candidates using a GQ-CNN and Dex-Net model weights/data sets 99 | * @details Compose grasp candidates, the candidates are sent back to the client 100 | * using the feedback message. The calls the gqcnn_grasp service to 101 | * execute the learned policy. 102 | */ 103 | void sampleGrasps(); 104 | 105 | private: 106 | ros::NodeHandle nh_; // node handle 107 | ros::ServiceClient gqcnn_client_; // gqcnn service client 108 | ros::ServiceClient image_client_; // image saving service client 109 | 110 | std::unique_ptr> 111 | server_; // action server 112 | moveit_task_constructor_msgs::SampleGraspPosesFeedback feedback_; // action feedback message 113 | moveit_task_constructor_msgs::SampleGraspPosesResult result_; // action result message 114 | 115 | std::string goal_name_; // action name 116 | std::string action_name_; // action namespace 117 | std::string frame_id_; // frame of point cloud/grasps 118 | 119 | std::string image_dir_; // directory images saved 120 | std::string color_image_file_; // file path to color image 121 | std::string depth_image_file_; // file path to depth image 122 | 123 | bool load_images_; // load images from file 124 | 125 | Eigen::Isometry3d trans_base_cam_; // transformation from base link to camera link 126 | Eigen::Isometry3d transform_cam_opt_; // transformation from camera link to optical link 127 | }; 128 | } // namespace moveit_task_constructor_dexnet 129 | -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/include/moveit_task_constructor_dexnet/image_server.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2020 PickNik Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************/ 32 | 33 | /* Author: Boston Cleek 34 | Desc: Image server for saving images 35 | */ 36 | 37 | #pragma once 38 | 39 | // ROS 40 | #include 41 | 42 | // C++ 43 | #include 44 | 45 | #include 46 | 47 | namespace moveit_task_constructor_dexnet 48 | { 49 | constexpr char LOGNAME[] = "image_server"; 50 | 51 | /** 52 | * @brief Provides a service for saving RGB and depth images 53 | */ 54 | class ImageServer 55 | { 56 | public: 57 | /** 58 | * @brief Constructor 59 | * @param nh - node handle 60 | */ 61 | ImageServer(ros::NodeHandle& nh); 62 | 63 | private: 64 | /** 65 | * @brief Loads parameters 66 | */ 67 | void loadParameters(); 68 | 69 | /** 70 | * @brief Initialize ROS communication 71 | */ 72 | void init(); 73 | 74 | /** 75 | * @brief RGB image callback 76 | * @param msg - GGB image 77 | */ 78 | void colorCallback(const sensor_msgs::Image::ConstPtr& msg); 79 | 80 | /** 81 | * @brief Depth image callback 82 | * @param msg - Depth image 83 | */ 84 | void depthCallback(const sensor_msgs::Image::ConstPtr& msg); 85 | 86 | /** 87 | * @brief Service callback for saving images 88 | * @param req - Service request contains the file name 89 | * @param res [out] - Service result true if image type is saved 90 | */ 91 | bool saveCallback(moveit_task_constructor_dexnet::Images::Request& req, 92 | moveit_task_constructor_dexnet::Images::Response& res); 93 | 94 | /** 95 | * @brief Saves image based on encoding and to specified file 96 | * @param msg - Image 97 | * @param image_name - File name of image 98 | * @details Images are saved as CV_8UC3 (BGR8) by OpenCV by default 99 | */ 100 | bool saveImage(const sensor_msgs::Image::ConstPtr& msg, const std::string& image_name); 101 | 102 | private: 103 | ros::NodeHandle nh_; // node handle 104 | ros::Subscriber color_img_sub_; // color image subscriber 105 | ros::Subscriber depth_img_sub_; // depth image subscriber 106 | ros::ServiceServer saver_srv_; // image saver service 107 | 108 | sensor_msgs::Image::ConstPtr color_img_; // image to save 109 | sensor_msgs::Image::ConstPtr depth_img_; // image to save 110 | 111 | std::string color_img_topic_; // color image topic name 112 | std::string depth_img_topic_; // depth image topic name 113 | std::string image_dir_; // directory to save images 114 | }; 115 | } // namespace moveit_task_constructor_dexnet 116 | -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/launch/dexnet_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/media/cylinder_grasp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_dexnet/media/cylinder_grasp.png -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/media/gqcnn_barclamp_gazebo2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_dexnet/media/gqcnn_barclamp_gazebo2.gif -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/media/gqcnn_berry.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_dexnet/media/gqcnn_berry.png -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/media/gqcnn_berry_gazebo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_dexnet/media/gqcnn_berry_gazebo.gif -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/media/gqcnn_clamp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_dexnet/media/gqcnn_clamp.png -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/media/gqcnn_cylinder_gazebo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_dexnet/media/gqcnn_cylinder_gazebo.gif -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/media/mtc_gqcnn_panda.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_dexnet/media/mtc_gqcnn_panda.gif -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | moveit_task_constructor_dexnet 4 | 0.0.1 5 | MoveIt task constructor pick and place using a GQ-CNN to select a grasp candidate 6 | 7 | Boston Cleek 8 | Boston Cleek 9 | 10 | BSD 11 | 12 | catkin 13 | message_generation 14 | 15 | actionlib 16 | cv_bridge 17 | geometry_msgs 18 | moveit_task_constructor_msgs 19 | image_transport 20 | roscpp 21 | rospy 22 | rosparam_shortcuts 23 | sensor_msgs 24 | std_msgs 25 | message_runtime 26 | 27 | -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/scripts/grasp_detector: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """ 4 | ********************************************************************* 5 | * Software License Agreement (BSD License) 6 | * 7 | * Copyright (c) 2020, PickNik Inc 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of PickNik Inc nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | ********************************************************************* 37 | Author: Boston Cleek 38 | Desc: GQCNN- selects the grasp with the highest probability of success 39 | """ 40 | 41 | import json 42 | import os 43 | import sys 44 | import time 45 | import cv2 46 | import pickle 47 | import numpy as np 48 | from matplotlib import pyplot as plt 49 | 50 | from autolab_core import YamlConfig, Logger 51 | from perception import (BinaryImage, CameraIntrinsics, ColorImage, DepthImage, 52 | RgbdImage) 53 | from visualization import Visualizer2D as vis 54 | 55 | from gqcnn.grasping import (RobustGraspingPolicy, 56 | CrossEntropyRobustGraspingPolicy, RgbdImageState, 57 | FullyConvolutionalGraspingPolicyParallelJaw, 58 | FullyConvolutionalGraspingPolicySuction) 59 | from gqcnn.utils import GripperMode 60 | 61 | 62 | if __name__ == "__main__": 63 | 64 | input_msg = sys.stdin.read().split('\n') 65 | color_img_filename = input_msg[0] 66 | depth_img_filename = input_msg[1] 67 | sampled_grasps_filename = input_msg[2] 68 | camera_intr_filename = input_msg[3] 69 | model_name = input_msg[4] 70 | model_dir = input_msg[5] 71 | model_params = input_msg[6] 72 | 73 | # model weights 74 | model_config = json.load(open(os.path.join(model_dir, "config.json"), "r")) 75 | 76 | try: 77 | gqcnn_config = model_config["gqcnn"] 78 | gripper_mode = gqcnn_config["gripper_mode"] 79 | except KeyError: 80 | gqcnn_config = model_config["gqcnn_config"] 81 | input_data_mode = gqcnn_config["input_data_mode"] 82 | if input_data_mode == "tf_image": 83 | gripper_mode = GripperMode.LEGACY_PARALLEL_JAW 84 | elif input_data_mode == "tf_image_suction": 85 | gripper_mode = GripperMode.LEGACY_SUCTION 86 | elif input_data_mode == "suction": 87 | gripper_mode = GripperMode.SUCTION 88 | elif input_data_mode == "multi_suction": 89 | gripper_mode = GripperMode.MULTI_SUCTION 90 | elif input_data_mode == "parallel_jaw": 91 | gripper_mode = GripperMode.PARALLEL_JAW 92 | else: 93 | raise ValueError( 94 | "Input data mode {} not supported!".format(input_data_mode)) 95 | 96 | # policy params 97 | config = YamlConfig(model_params) 98 | policy_config = config["policy"] 99 | policy_config["metric"]["gqcnn_model"] = model_dir 100 | 101 | # # sensor 102 | camera_intr = CameraIntrinsics.load(camera_intr_filename) 103 | 104 | # images 105 | color_cvmat = cv2.imread(color_img_filename) 106 | depth_cvmat = cv2.imread(depth_img_filename) # load as 8UC3 107 | depth_cvmat = cv2.cvtColor(depth_cvmat, cv2.COLOR_BGR2GRAY) # 8UC1 108 | depth_cvmat = depth_cvmat.astype(np.float32) * 1.0/255.0 # 32FC1 109 | 110 | # cv2.imshow('img', color_cvmat) 111 | # cv2.waitKey(0) 112 | # cv2.destroyAllWindows() 113 | 114 | # create wrapped BerkeleyAutomation/perception RGB and depth images 115 | color_im = ColorImage(color_cvmat, frame=camera_intr.frame, encoding="bgr8") 116 | depth_im = DepthImage(depth_cvmat, frame=camera_intr.frame) 117 | 118 | # check image sizes. 119 | if (color_im.height != depth_im.height or color_im.width != depth_im.width): 120 | msg = ("Color image and depth image must be the same shape! Color" 121 | " is %d x %d but depth is %d x %d") % ( 122 | color_im.height, color_im.width, depth_im.height, 123 | depth_im.width) 124 | print(msg) 125 | 126 | # assume not mask is provided 127 | # segmask = depth_im.invalid_pixel_mask().inverse() 128 | segmask = BinaryImage(255 * 129 | np.ones(depth_im.shape).astype(np.uint8), 130 | frame=color_im.frame) 131 | 132 | # inpaint images. 133 | # color_im = color_im.inpaint( 134 | # rescale_factor=config["inpaint_rescale_factor"]) 135 | # depth_im = depth_im.inpaint( 136 | # rescale_factor=config["inpaint_rescale_factor"]) 137 | 138 | # Aggregate color and depth images into a single 139 | # BerkeleyAutomation/perception `RgbdImage`. 140 | rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im) 141 | 142 | state = RgbdImageState(rgbd_im, camera_intr, segmask=segmask) 143 | policy = CrossEntropyRobustGraspingPolicy(policy_config) 144 | 145 | # Query policy for all sampled grasps 146 | grasp_candidates, q_values = policy.action_set(state) 147 | 148 | candidate_array = [] 149 | for i in range(len(grasp_candidates)): 150 | # Position: [x y z] 151 | # Orientation: [qw qx qy qz] 152 | candidate_array.append(np.array([grasp_candidates[i].pose().to_frame, 153 | grasp_candidates[i].pose().position, 154 | grasp_candidates[i].pose().quaternion, 155 | q_values[i]])) 156 | 157 | pickle_sols = open(sampled_grasps_filename, "wb") 158 | pickle.dump(candidate_array, pickle_sols, protocol=2) 159 | pickle_sols.close() 160 | 161 | 162 | # # Query policy for best grasp 163 | # policy_start = time.time() 164 | # action = policy(state) 165 | # print("Planning took %.3f sec" % (time.time() - policy_start)) 166 | # print("Gripper pose: ", action.grasp.pose()) 167 | # 168 | # # Position: [x y z] 169 | # # Orientation: [qw qx qy qz] 170 | # grasp_candidate = np.array([action.grasp.pose().to_frame, 171 | # action.grasp.pose().position, 172 | # action.grasp.pose().quaternion, 173 | # action.q_value]) 174 | # 175 | # # serialize candidates 176 | # pickle_sols = open(sampled_grasps_filename, "wb") 177 | # pickle.dump([grasp_candidate], pickle_sols, protocol=2) 178 | # pickle_sols.close() 179 | # 180 | # # Vis final grasp. 181 | # if policy_config["vis"]["final_grasp"]: 182 | # vis.figure(size=(10, 10)) 183 | # vis.imshow(rgbd_im.depth, 184 | # vmin=policy_config["vis"]["vmin"], 185 | # vmax=policy_config["vis"]["vmax"]) 186 | # vis.grasp(action.grasp, scale=2.5, show_center=False, show_axis=True) 187 | # vis.title("Planned grasp at depth {0:.3f}m with Q={1:.3f}".format( 188 | # action.grasp.depth, action.q_value)) 189 | # vis.show() 190 | -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/src/gqcnn_server: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | ********************************************************************* 5 | * Software License Agreement (BSD License) 6 | * 7 | * Copyright (c) 2020, PickNik Inc. 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of PickNik Inc nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | ********************************************************************* 37 | Author: Boston Cleek 38 | Desc: The server calls a script running the GQCNN 39 | 40 | PARAMETERS: 41 | dexnet_policy_script - executable python3 script of the dexnet policy 42 | SERVICES: 43 | gqcnn_grasp (GQCNNGrasp) - service requests the grasp candidate from 44 | the learned policy and the probability of success 45 | """ 46 | 47 | # Python 48 | import subprocess 49 | import numpy as np 50 | 51 | # ROS 52 | import rospy 53 | import pickle 54 | from geometry_msgs.msg import PoseStamped 55 | from moveit_task_constructor_dexnet.srv import GQCNNGrasp, GQCNNGraspResponse 56 | 57 | class GraspServer(): 58 | """ Advertise a grasp candidate service """ 59 | 60 | def __init__(self): 61 | """ Constructor """ 62 | 63 | # neural network parameters 64 | self.model_name = rospy.get_param('~model_name') 65 | self.model_dir = rospy.get_param('~model_dir') 66 | self.model_params = rospy.get_param('~model_params') 67 | 68 | # camera intrinsics 69 | self.camera_calib_intr = rospy.get_param('~camera_calib_intr') 70 | 71 | # dexnet policy executable script 72 | self.policy_script = rospy.get_param('~dexnet_policy_script') 73 | 74 | # sampled grasp candidates storage 75 | self.sampled_grasps_file = rospy.get_param('~sampled_grasps') 76 | 77 | # sample grasps service 78 | self.grasp_serv = rospy.Service('gqcnn_grasp', GQCNNGrasp, self.handle_grasp) 79 | 80 | def handle_grasp(self, req): 81 | """ Callback for GQCNNGrasp sevice 82 | 83 | Creates a subprocess (python3 script running the GQCNN) to compose the 84 | grasp candidate and q_value 85 | 86 | Args: 87 | req (GQCNNGrasp): input is a string for the name of the request 88 | 89 | Return: 90 | GQCNNGraspResponse (geometry_msgs/PoseStamped and float64): the 6DOF 91 | grasp pose and the score representing the probability of success 92 | """ 93 | 94 | rospy.loginfo("GQCNN service activated") 95 | 96 | ###################################################################### 97 | # start subprocess 98 | proc = subprocess.Popen(['python3', self.policy_script], stdin=subprocess.PIPE) 99 | 100 | 101 | # sends data to stdin, waits until process reaches end of file 102 | proc_out = proc.communicate(req.color_img_file_path + '\n' + 103 | req.depth_img_file_path + '\n' + 104 | self.sampled_grasps_file + '\n' + 105 | self.camera_calib_intr + '\n' + 106 | self.model_name + '\n' + 107 | self.model_dir + '\n' + 108 | self.model_params + '\n')[0] 109 | # end subproces 110 | ###################################################################### 111 | 112 | # sampled grasps 113 | sols_file = open(self.sampled_grasps_file, 'rb') 114 | data = pickle.load(sols_file) 115 | sols_file.close() 116 | 117 | # service response 118 | grasp_response = GQCNNGraspResponse() 119 | 120 | for i in range(len(data)): 121 | grasp_pose = PoseStamped() 122 | grasp_pose.header.frame_id = data[i][0] 123 | # position [x y z] 124 | grasp_pose.pose.position.x = data[i][1][0] 125 | grasp_pose.pose.position.y = data[i][1][1] 126 | grasp_pose.pose.position.z = data[i][1][2] 127 | 128 | # orientation [qw qx qy qz] 129 | grasp_pose.pose.orientation.w = data[i][2][0] 130 | grasp_pose.pose.orientation.x = data[i][2][1] 131 | grasp_pose.pose.orientation.y = data[i][2][2] 132 | grasp_pose.pose.orientation.z = data[i][2][3] 133 | 134 | grasp_response.grasps.append(grasp_pose) 135 | grasp_response.q_values.append(data[i][3]) 136 | 137 | return grasp_response 138 | 139 | 140 | def main(): 141 | rospy.init_node('gqcnn_server') 142 | grasp_server = GraspServer() 143 | rospy.spin() 144 | 145 | 146 | if __name__ == "__main__": 147 | try: 148 | main() 149 | except rospy.ROSInterruptException: 150 | pass 151 | -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/src/grasp_detection.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2020 PickNik Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************/ 32 | 33 | /* Author: Boston Cleek 34 | Desc: Samples grasp candidates using GQ-CNN and Dex-Net model weights/data sets 35 | */ 36 | 37 | // ROS 38 | #include 39 | #include 40 | #include 41 | 42 | // C++ 43 | #include 44 | 45 | #include 46 | #include 47 | #include 48 | 49 | namespace moveit_task_constructor_dexnet 50 | { 51 | GraspDetection::GraspDetection(const ros::NodeHandle& nh) : nh_(nh) 52 | { 53 | loadParameters(); 54 | init(); 55 | } 56 | 57 | void GraspDetection::loadParameters() 58 | { 59 | ROS_INFO_NAMED(LOGNAME, "Loading grasp action server parameters"); 60 | ros::NodeHandle pnh("~"); 61 | 62 | size_t errors = 0; 63 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "load_images", load_images_); 64 | if (load_images_) 65 | { 66 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "color_image_file", color_image_file_); 67 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "depth_image_file", depth_image_file_); 68 | } 69 | 70 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "action_name", action_name_); 71 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "frame_id", frame_id_); 72 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "image_dir", image_dir_); 73 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "trans_cam_opt", transform_cam_opt_); 74 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "trans_base_cam", trans_base_cam_); 75 | rosparam_shortcuts::shutdownIfError(LOGNAME, errors); 76 | } 77 | 78 | void GraspDetection::init() 79 | { 80 | // action server 81 | server_.reset(new actionlib::SimpleActionServer( 82 | nh_, action_name_, false)); 83 | server_->registerGoalCallback(std::bind(&GraspDetection::goalCallback, this)); 84 | server_->registerPreemptCallback(std::bind(&GraspDetection::preemptCallback, this)); 85 | server_->start(); 86 | 87 | // gqcnn service 88 | gqcnn_client_ = nh_.serviceClient("gqcnn_grasp"); 89 | ros::service::waitForService("gqcnn_grasp", ros::Duration(3.0)); 90 | 91 | // not loading images from file so need to call server to save them 92 | if (!load_images_) 93 | { 94 | color_image_file_ = "rgb_object.png"; 95 | depth_image_file_ = "depth_object.png"; 96 | 97 | image_client_ = nh_.serviceClient("save_images"); 98 | ros::service::waitForService("save_images", ros::Duration(3.0)); 99 | } 100 | } 101 | 102 | void GraspDetection::goalCallback() 103 | { 104 | goal_name_ = server_->acceptNewGoal()->action_name; 105 | ROS_INFO_NAMED(LOGNAME, "New goal accepted: %s", goal_name_.c_str()); 106 | 107 | // save images 108 | if (!load_images_) 109 | { 110 | requestImages(); 111 | } 112 | 113 | sampleGrasps(); 114 | } 115 | 116 | void GraspDetection::preemptCallback() 117 | { 118 | ROS_INFO_NAMED(LOGNAME, "Preempted %s:", goal_name_.c_str()); 119 | server_->setPreempted(); 120 | } 121 | 122 | void GraspDetection::requestImages() 123 | { 124 | moveit_task_constructor_dexnet::Images image_srv; 125 | image_srv.request.color_file = color_image_file_; 126 | image_srv.request.depth_file = depth_image_file_; 127 | 128 | if (image_client_.call(image_srv)) 129 | { 130 | ROS_INFO_NAMED(LOGNAME, "Called save_images service, waiting for response..."); 131 | if (image_srv.response.color_saved && image_srv.response.color_saved) 132 | { 133 | ROS_INFO_NAMED(LOGNAME, "Images saved"); 134 | } 135 | else 136 | { 137 | ROS_ERROR_NAMED(LOGNAME, "Images not saved"); 138 | } 139 | } 140 | else 141 | { 142 | ROS_ERROR_NAMED(LOGNAME, "Failed to call save_images service"); 143 | } 144 | } 145 | 146 | void GraspDetection::sampleGrasps() 147 | { 148 | moveit_task_constructor_dexnet::GQCNNGrasp grasp_srv; 149 | grasp_srv.request.color_img_file_path = image_dir_ + color_image_file_; 150 | grasp_srv.request.depth_img_file_path = image_dir_ + depth_image_file_; 151 | 152 | if (gqcnn_client_.call(grasp_srv)) 153 | { 154 | ROS_INFO_NAMED(LOGNAME, "Called gqcnn_grasp service, waiting for response..."); 155 | const std::vector grasp_candidates = grasp_srv.response.grasps; 156 | const std::vector q_values = grasp_srv.response.q_values; 157 | ROS_INFO_NAMED(LOGNAME, "Results recieved"); 158 | 159 | if (grasp_candidates.empty()) 160 | { 161 | ROS_ERROR_NAMED(LOGNAME, "No grasp candidates found"); 162 | result_.grasp_state = "failed"; 163 | server_->setAborted(result_); 164 | return; 165 | } 166 | 167 | for (unsigned int i = 0; i < grasp_candidates.size(); i++) 168 | { 169 | // transform grasp from camera optical link into frame_id (panda_link0) 170 | // the demo is using fake_controllers there is no tf data for the camera 171 | // convert PoseStamped to transform (optical link to grasp) 172 | const Eigen::Isometry3d transform_opt_grasp = 173 | Eigen::Translation3d(grasp_candidates.at(i).pose.position.x, grasp_candidates.at(i).pose.position.y, 174 | grasp_candidates.at(i).pose.position.z) * 175 | Eigen::Quaterniond(grasp_candidates.at(i).pose.orientation.w, grasp_candidates.at(i).pose.orientation.x, 176 | grasp_candidates.at(i).pose.orientation.y, grasp_candidates.at(i).pose.orientation.z); 177 | 178 | // the 6dof grasp pose in frame_id (panda_link0) 179 | const Eigen::Isometry3d transform_base_grasp = trans_base_cam_ * transform_cam_opt_ * transform_opt_grasp; 180 | const Eigen::Vector3d trans = transform_base_grasp.translation(); 181 | const Eigen::Quaterniond rot(transform_base_grasp.rotation()); 182 | 183 | // convert back to PoseStamped 184 | geometry_msgs::PoseStamped grasp_pose; 185 | grasp_pose.header.frame_id = frame_id_; 186 | grasp_pose.pose.position.x = trans.x(); 187 | grasp_pose.pose.position.y = trans.y(); 188 | grasp_pose.pose.position.z = trans.z(); 189 | 190 | grasp_pose.pose.orientation.w = rot.w(); 191 | grasp_pose.pose.orientation.x = rot.x(); 192 | grasp_pose.pose.orientation.y = rot.y(); 193 | grasp_pose.pose.orientation.z = rot.z(); 194 | 195 | // send feedback to action client 196 | feedback_.grasp_candidates.emplace_back(grasp_pose); 197 | 198 | // Q_value (probability of success) 199 | // cost = 1.0 - Q_value, to represent cost 200 | const double cost = 1.0 - q_values.at(i); 201 | // ROS_INFO_NAMED(LOGNAME, "ID: %u Cost: %f", i, cost); 202 | feedback_.costs.emplace_back(cost); 203 | } 204 | 205 | server_->publishFeedback(feedback_); 206 | result_.grasp_state = "success"; 207 | server_->setSucceeded(result_); 208 | } 209 | 210 | else 211 | { 212 | ROS_ERROR_NAMED(LOGNAME, "Failed to call gqcnn_grasp service"); 213 | result_.grasp_state = "failed"; 214 | server_->setAborted(result_); 215 | } 216 | } 217 | } // namespace moveit_task_constructor_dexnet 218 | -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/src/grasp_image_detection.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2020 PickNik Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************/ 32 | 33 | /* Author: Boston Cleek 34 | Desc: Grasp pose generation using Dex-Net, option to load images from a directory 35 | or subscribe to call the save_images service. To sample grasps the 36 | gqcnn_grasp service is called. This service returns the grasp candidates 37 | and the associated probabilities of success. Communication with MTC 38 | is achieved through an action service. This node contains the action client 39 | that sends a list grasp candidates and associated costs to MTC as feedback. 40 | 41 | PARAMETERS: 42 | load_images - load images from data directory 43 | image_dir - image data directory 44 | color_image_file (optional) - name of RGB image used as input to GQ-CNN 45 | depth_image_file (optional) - name of depth image used as input to GQ-CNN 46 | action_name - action namespace 47 | frame_id - frame of the grasp candidates sent to MTC 48 | trans_cam_opt - transform from camera link to optical link 49 | trans_base_cam - transform from robot base link to camera link 50 | */ 51 | 52 | // ROS 53 | #include 54 | 55 | #include 56 | 57 | int main(int argc, char** argv) 58 | { 59 | ROS_INFO_STREAM_NAMED("main", "Starting grasp_image_detection"); 60 | ros::init(argc, argv, "grasp_image_detection"); 61 | ros::NodeHandle nh; 62 | 63 | ros::AsyncSpinner spinner(1); 64 | spinner.start(); 65 | 66 | moveit_task_constructor_dexnet::GraspDetection grasp_detection(nh); 67 | 68 | ros::waitForShutdown(); 69 | ROS_INFO_STREAM_NAMED("main", "Shutting down."); 70 | 71 | return 0; 72 | } 73 | -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/src/image_server.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2020 PickNik Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************/ 32 | 33 | /* Author: Boston Cleek 34 | Desc: Image server for saving images 35 | */ 36 | 37 | // ROS 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | // OpenCV 45 | #include 46 | #include 47 | #include 48 | 49 | #include 50 | 51 | namespace moveit_task_constructor_dexnet 52 | { 53 | ImageServer::ImageServer(ros::NodeHandle& nh) : nh_(nh) 54 | { 55 | loadParameters(); 56 | init(); 57 | } 58 | 59 | void ImageServer::loadParameters() 60 | { 61 | ros::NodeHandle pnh("~"); 62 | size_t errors = 0; 63 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "color_img_topic", color_img_topic_); 64 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "depth_img_topic", depth_img_topic_); 65 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "image_dir", image_dir_); 66 | rosparam_shortcuts::shutdownIfError(LOGNAME, errors); 67 | } 68 | 69 | void ImageServer::init() 70 | { 71 | color_img_sub_ = nh_.subscribe(color_img_topic_, 1, &ImageServer::colorCallback, this); 72 | depth_img_sub_ = nh_.subscribe(depth_img_topic_, 1, &ImageServer::depthCallback, this); 73 | saver_srv_ = nh_.advertiseService("save_images", &ImageServer::saveCallback, this); 74 | } 75 | 76 | void ImageServer::colorCallback(const sensor_msgs::Image::ConstPtr& msg) 77 | { 78 | color_img_ = msg; 79 | } 80 | 81 | void ImageServer::depthCallback(const sensor_msgs::Image::ConstPtr& msg) 82 | { 83 | depth_img_ = msg; 84 | } 85 | 86 | bool ImageServer::saveCallback(moveit_task_constructor_dexnet::Images::Request& req, 87 | moveit_task_constructor_dexnet::Images::Response& res) 88 | { 89 | ROS_INFO_NAMED(LOGNAME, "Saving image service active"); 90 | res.color_saved = saveImage(color_img_, req.color_file) ? true : false; 91 | res.depth_saved = saveImage(depth_img_, req.depth_file) ? true : false; 92 | return true; 93 | } 94 | 95 | bool ImageServer::saveImage(const sensor_msgs::Image::ConstPtr& msg, const std::string& image_name) 96 | { 97 | cv_bridge::CvImagePtr cv_ptr; 98 | cv_ptr = cv_bridge::toCvCopy(msg, msg->encoding); 99 | 100 | // imwrite() saves image as BGR 101 | cv::Mat img_converted; 102 | 103 | if (msg->encoding == "rgb8") 104 | { 105 | cv_ptr->image.convertTo(img_converted, CV_8UC3); // convert to BGR 106 | } 107 | else if (msg->encoding == "32FC1") 108 | { 109 | cv_ptr->image.convertTo(img_converted, CV_8UC3, 255.0); // conver to BGR and scale 110 | } 111 | else 112 | { 113 | ROS_ERROR_NAMED(LOGNAME, "Image encoding not recognized (encoding): %s", msg->encoding.c_str()); 114 | return false; 115 | } 116 | 117 | if (cv::imwrite(image_dir_ + image_name, img_converted)) 118 | { 119 | ROS_INFO_NAMED(LOGNAME, "Saving image %s (encoding): %s ", image_name.c_str(), msg->encoding.c_str()); 120 | } 121 | else 122 | { 123 | ROS_WARN_NAMED(LOGNAME, "Image %s not saved", image_name.c_str()); 124 | return false; 125 | } 126 | 127 | return true; 128 | } 129 | } // namespace moveit_task_constructor_dexnet 130 | -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/src/process_image_server.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2020 PickNik Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************/ 32 | 33 | /* Author: Boston Cleek 34 | Desc: Image server node for saving images 35 | 36 | PARAMETERS: 37 | color_img_topic - color image topic name 38 | depth_img_topic - depth image topic name 39 | image_dir - directory to save images 40 | SUBSCRIBES: 41 | color_img_topic (sensor_msgs/Image) - color image 42 | depth_img_topic (sensor_msgs/Image) - depth image 43 | SERVICES: 44 | save_images (moveit_task_constructor_dexnet/Images) - save images service 45 | */ 46 | 47 | // ROS 48 | #include 49 | 50 | #include 51 | 52 | int main(int argc, char** argv) 53 | { 54 | ROS_INFO_STREAM_NAMED("main", "Starting process_image_server"); 55 | ros::init(argc, argv, "process_image_server"); 56 | ros::NodeHandle nh; 57 | 58 | ros::AsyncSpinner spinner(1); 59 | spinner.start(); 60 | 61 | moveit_task_constructor_dexnet::ImageServer image_server(nh); 62 | 63 | ros::waitForShutdown(); 64 | ROS_INFO_STREAM_NAMED("main", "Shutting down."); 65 | 66 | return 0; 67 | } 68 | -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/srv/GQCNNGrasp.srv: -------------------------------------------------------------------------------- 1 | # Ideally this input would be the depth and color image. 2 | # Input file paths to the images 3 | # Outputs are the grasps selected from the GQCNN and the corresponding probabilities of success. 4 | string color_img_file_path 5 | string depth_img_file_path 6 | --- 7 | geometry_msgs/PoseStamped[] grasps 8 | float64[] q_values 9 | -------------------------------------------------------------------------------- /moveit_task_constructor_dexnet/srv/Images.srv: -------------------------------------------------------------------------------- 1 | # Request saving depth and rgb images 2 | # Input is file names to save them as 3 | # Output is true if image is saved 4 | string depth_file 5 | string color_file 6 | --- 7 | bool depth_saved 8 | bool color_saved 9 | -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(moveit_task_constructor_gpd) 3 | 4 | # C++ 14 5 | add_compile_options(-std=c++14) 6 | 7 | # Warnings 8 | add_definitions(-W -Wall -Wextra 9 | -Wwrite-strings -Wunreachable-code -Wpointer-arith 10 | -Winit-self -Wredundant-decls 11 | -Wno-unused-parameter -Wno-unused-function) 12 | 13 | # Find catkin macros and libraries 14 | find_package(catkin REQUIRED COMPONENTS 15 | actionlib 16 | eigen_conversions 17 | geometry_msgs 18 | message_generation 19 | moveit_task_constructor_msgs 20 | roscpp 21 | rosparam_shortcuts 22 | sensor_msgs 23 | std_msgs 24 | ) 25 | 26 | # System dependencies are found with CMake's conventions 27 | find_package(Eigen3 REQUIRED) 28 | find_package(PCL 1.8 REQUIRED) 29 | 30 | find_library(GPD_LIB NAMES gpd PATHS /usr/local/lib PATH_SUFFIXES lib NO_DEFAULT_PATH) 31 | if (GPD_LIB) 32 | message(STATUS "Library GPD found in ${GPD_LIB}") 33 | else() 34 | message(FATAL_ERROR "Library GPD not found") 35 | endif() 36 | 37 | find_path(GPD_INCLUDE_DIRS NAMES gpd PATHS /usr/local/include NO_DEFAULT_PATH) 38 | if (GPD_INCLUDE_DIRS) 39 | message(STATUS "Include directory GPD found in ${GPD_INCLUDE_DIRS}") 40 | else() 41 | message(FATAL_ERROR "Include directory GPD not found") 42 | endif() 43 | 44 | # Service files 45 | add_service_files( 46 | FILES 47 | PointCloud.srv 48 | ) 49 | 50 | # Message generation dependencies 51 | generate_messages( 52 | DEPENDENCIES 53 | std_msgs 54 | ) 55 | 56 | ################################### 57 | ## Catkin specific configuration ## 58 | ################################### 59 | catkin_package( 60 | INCLUDE_DIRS include 61 | LIBRARIES ${PROJECT_NAME} 62 | CATKIN_DEPENDS 63 | eigen_conversions 64 | geometry_msgs 65 | message_runtime 66 | moveit_task_constructor_msgs 67 | sensor_msgs 68 | std_msgs 69 | DEPENDS 70 | EIGEN3 71 | PCL 72 | ) 73 | 74 | ########### 75 | ## Build ## 76 | ########### 77 | 78 | # Specify additional locations of header files 79 | # Your package locations should be listed before other locations 80 | include_directories( 81 | SYSTEM 82 | ${EIGEN3_INCLUDE_DIRS} 83 | ${PCL_INCLUDE_DIRS} 84 | ${GPD_INCLUDE_DIRS} 85 | ) 86 | 87 | include_directories( 88 | include 89 | ${catkin_INCLUDE_DIRS} 90 | ) 91 | 92 | # Declare a C++ library with project namespace to avoid naming collision 93 | add_library(${PROJECT_NAME} 94 | src/${PROJECT_NAME}/cloud_utils.cpp 95 | ) 96 | 97 | ## Add cmake target dependencies of the library 98 | ## as an example, code may need to be generated before libraries 99 | ## either from message generation or dynamic reconfigure 100 | add_dependencies(${PROJECT_NAME} 101 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 102 | ${catkin_EXPORTED_TARGETS} 103 | ) 104 | 105 | 106 | # Specify libraries to link a library or executable target against 107 | target_link_libraries(${PROJECT_NAME} 108 | ${catkin_LIBRARIES} 109 | ${PCL_LIBRARIES} 110 | ) 111 | 112 | # Declare a C++ executable 113 | add_executable(grasp_cloud_detection 114 | src/grasp_cloud_detection.cpp 115 | src/grasp_detection.cpp 116 | ) 117 | 118 | add_executable(point_cloud_server 119 | src/cloud_server.cpp 120 | src/point_cloud_server.cpp 121 | ) 122 | 123 | 124 | ## Add cmake target dependencies of the executable 125 | ## same as for the library above 126 | add_dependencies(grasp_cloud_detection 127 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 128 | ${catkin_EXPORTED_TARGETS} 129 | ) 130 | 131 | add_dependencies(point_cloud_server 132 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 133 | ${catkin_EXPORTED_TARGETS} 134 | ) 135 | 136 | 137 | # Specify libraries to link a library or executable target against 138 | target_link_libraries(grasp_cloud_detection 139 | ${catkin_LIBRARIES} 140 | ${PROJECT_NAME} 141 | ${Eigen3_LIBRARIES} 142 | ${PCL_LIBRARIES} 143 | ${GPD_LIB} 144 | ) 145 | 146 | target_link_libraries(point_cloud_server 147 | ${catkin_LIBRARIES} 148 | ${PROJECT_NAME} 149 | ${PCL_LIBRARIES} 150 | ) 151 | 152 | ############# 153 | ## Install ## 154 | ############# 155 | 156 | # Mark executables and/or libraries for installation 157 | install( 158 | TARGETS 159 | grasp_cloud_detection ${PROJECT_NAME} point_cloud_server 160 | ARCHIVE DESTINATION 161 | ${CATKIN_PACKAGE_LIB_DESTINATION} 162 | LIBRARY DESTINATION 163 | ${CATKIN_PACKAGE_LIB_DESTINATION} 164 | RUNTIME DESTINATION 165 | ${CATKIN_PACKAGE_BIN_DESTINATION} 166 | ) 167 | 168 | # Mark cpp header files for installation 169 | install( 170 | DIRECTORY 171 | include/${PROJECT_NAME}/ 172 | DESTINATION 173 | ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 174 | ) 175 | 176 | # Mark roslaunch files for installation 177 | install( 178 | DIRECTORY 179 | launch 180 | DESTINATION 181 | ${CATKIN_PACKAGE_SHARE_DESTINATION} 182 | ) 183 | 184 | # Mark config files for installation 185 | install( 186 | DIRECTORY 187 | config 188 | DESTINATION 189 | ${CATKIN_PACKAGE_SHARE_DESTINATION} 190 | ) 191 | -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/README.md: -------------------------------------------------------------------------------- 1 | # MoveIt Task Constructor Grasp Pose Detection 2 | 3 | 4 | 1) [Overview](#Overview)
5 | 2) [Running](#Running)
6 | 3) [Nodes](#Nodes)
7 | 4) [Tips](#Tips)
8 | 5) [Results](#Results)
9 | 10 | ## Overview 11 | Demo shows how to use the Grasp Pose Detection (GPD) library within the MoveIt Task Constructor. GPD samples grasp candidates within a point cloud and returns a list of the candidates and their costs. 12 | 13 | ## Running 14 | ### Using Fake Controllers 15 | You don't need Gazebo for this one. The point cloud data for the cylinder was collected ahead of time and located in `data/pointclouds/cylinder.pcd` 16 | 17 | To run the pick and place demo: 18 | ``` 19 | roslaunch moveit_task_constructor_demo demo.launch 20 | roslaunch moveit_task_constructor_gpd gpd_demo.launch 21 | ``` 22 | 23 | ### Using Gazebo 24 | This demo allows you to execute the motion plan in Gazebo. You have the option to load the point cloud data from a file or use the simulated depth camera. The `path_to_pcd_file` argument in `gpd_demo.launch` specifies the point cloud that will be used. 25 | The `load_cloud` argument in `gpd_demo.launch` specifies whether or not to load the point cloud from a file. Set `load_cloud:=false` to use the simulated depth camera. 26 | 27 | Launch the robot in Gazebo: 28 | ``` 29 | roslaunch deep_grasp_task gazebo_pick_place.launch 30 | ``` 31 | 32 | To run the pick and place demo: 33 | ``` 34 | roslaunch moveit_task_constructor_gpd gpd_demo.launch 35 | ``` 36 | 37 | ## Nodes 38 | ### grasp_cloud_detection 39 | This node uses GPD to sample grasp candidates from a point cloud. Communication with the MoveIt Task Constructor is achieved using ROS action messages. The action client sends the grasp candidates along with the costs back to the `DeepGraspPose` stage. The point cloud can either be loaded from a file or the node can subscribe to the point cloud topic name of type `(sensor_msgs/PointCloud2)` specified as an input parameter. If subscribing to a point cloud topic, the table plane is removed and the resulting point cloud is published on `segmented_cloud`. 40 | 41 | ### point_cloud_server 42 | This node is primarily used for collecting point cloud data. The `save_point_cloud` service is offered to save point clouds to the user specified file. The node subscribes to the point cloud topic name of type `(sensor_msgs/PointCloud2)` specified as an input parameter. This node also offers the functionality to keep points within specified cartesian lower and upper bounds. Additionally, the table plane can be removed from the point cloud specified by the `remove_table` parameter. The resulting point cloud is published on `filtered_cloud`. 43 | 44 | ## Tips 45 | If you are processing the point clouds on your own, it is recommended to remove the table plane. GPD will find grasp candidates that try to pick up the object by the table plane. Sometimes segmentation is not enough and GPD tries to grasp other pieces of a point cloud that are not the object. Other portions of the point cloud can be removed using PCL's `filter()` function. This allows you to specify cartesian limits on which points to keep. See the `removeTable()` and `passThroughFilter()` functions in `cloud_utils.cpp.` 46 | 47 | ## Results 48 | The output to the cylinder demo using fake controller should look like this. 49 | 50 |

51 | 52 |

53 | 54 | You may notice that picking up objects on Gazebo is more difficult. I was not able to successfully pick and place any of the tested objects in Gazebo. 55 | In the example bellow 30 points from the point cloud were sampled by GPD. 56 | 57 |

58 | 59 | 60 |

61 | 62 | 63 | Other objects such as the strawberry and the bar clamp where tested and GPD was unsuccessful. With the strawberry, 30 points were sampled here as well. I noticed the grasp candidates were usually unable to create a force closure. 64 | 65 |

66 | 67 |

68 | 69 | 70 | In the example below 30 points from the point cloud were sampled by GPD. 71 | 72 |

73 | 74 | 75 |

76 | 77 | Finally, 500 points were sampled from the bar clamp's point cloud. This grasp candidate appears close to the one Dex-Net selects. 78 | 79 |

80 | 81 |

82 | -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/config/gpd_config.yaml: -------------------------------------------------------------------------------- 1 | # Path to config file for robot hand geometry 2 | hand_geometry_filename = 0 3 | 4 | # Path to config file for volume and image geometry 5 | image_geometry_filename = 0 6 | 7 | # ==== Robot Hand Geometry ==== 8 | # finger_width: the width of the finger 9 | # outer_diameter: the diameter of the robot hand (= maximum aperture + 2 * finger width) 10 | # hand_depth: the finger length (measured from hand base to finger tip) 11 | # hand_height: the height of the hand 12 | # init_bite: the minimum amount of the object to be covered by the hand 13 | # Panda (Visual approximation) 14 | finger_width = 0.01 15 | hand_outer_diameter = 0.12 16 | hand_depth = 0.06 17 | hand_height = 0.02 18 | init_bite = 0.01 19 | 20 | # ==== Grasp Descriptor ==== (cm) 21 | # volume_width: the width of the cube inside the robot hand 22 | # volume_depth: the depth of the cube inside the robot hand 23 | # volume_height: the height of the cube inside the robot hand 24 | # image_size: the size of the image (width and height; image is square) 25 | # image_num_channels: the number of image channels 26 | volume_width = 0.10 27 | volume_depth = 0.06 28 | volume_height = 0.02 29 | image_size = 60 30 | image_num_channels = 15 31 | 32 | # Path to directory that contains neural network parameters 33 | weights_file = /home/bostoncleek/GPD/gpd/models/lenet/15channels/params/ 34 | 35 | # Preprocessing of point cloud 36 | # voxelize: if the cloud gets voxelized/downsampled 37 | # remove_outliers: if statistical outliers are removed from the cloud (used to remove noise) 38 | # workspace: the workspace of the robot (dimensions of a cube centered at origin of point cloud) 39 | # camera_position: the position of the camera from which the cloud was taken 40 | # sample_above_plane: only draws samples which do not belong to the table plane 41 | voxelize = 1 42 | voxel_size = 0.003 43 | remove_outliers = 0 44 | workspace = -1.0 1.0 -1.0 1.0 -1.0 1.0 45 | # workspace = -2.0 2.0 -2.0 2.0 -2.0 2.0 46 | camera_position = 0 0 0 47 | # camera_position = 0.15 0 0.04 48 | sample_above_plane = 0 49 | 50 | # Grasp candidate generation 51 | # num_samples: the number of samples to be drawn from the point cloud 52 | # num_threads: the number of CPU threads to be used 53 | # nn_radius: the radius for the neighborhood search 54 | # num_orientations: the number of robot hand orientations to evaluate 55 | # rotation_axes: the axes about which the point neighborhood gets rotated 56 | num_samples = 50 57 | num_threads = 4 58 | nn_radius = 0.01 59 | num_orientations = 8 60 | num_finger_placements = 10 61 | hand_axes = 2 62 | deepen_hand = 1 63 | 64 | # Filtering of candidates (mm) 65 | # min_aperture: the minimum gripper width 66 | # max_aperture: the maximum gripper width 67 | # workspace_grasps: dimensions of a cube centered at origin of point cloud; should be smaller than 68 | min_aperture = 0.0 69 | max_aperture = 0.85 70 | workspace_grasps = -1 1 -1 1 -1 1 71 | # workspace_grasps = -2 2 -2 2 -2 2 72 | 73 | 74 | 75 | # Filtering of candidates based on their approach direction 76 | # filter_approach_direction: turn filtering on/off 77 | # direction: the direction to compare against 78 | # angle_thresh: angle in radians above which grasps are filtered 79 | filter_approach_direction = 1 80 | direction = 0 0 1 81 | thresh_rad = 2.0 82 | 83 | # Clustering of grasps 84 | # min_inliers: minimum number of inliers per cluster; set to 0 to turn off clustering 85 | min_inliers = 1 86 | 87 | # Grasp selection 88 | # num_selected: number of selected grasps (sorted by score) 89 | num_selected = 100 90 | 91 | # Visualization 92 | # plot_normals: plot the surface normals 93 | # plot_samples: plot the samples 94 | # plot_candidates: plot the grasp candidates 95 | # plot_filtered_candidates: plot the grasp candidates which remain after filtering 96 | # plot_valid_grasps: plot the candidates that are identified as valid grasps 97 | # plot_clustered_grasps: plot the grasps that after clustering 98 | # plot_selected_grasps: plot the selected grasps (final output) 99 | plot_normals = 0 100 | plot_samples = 0 101 | plot_candidates = 0 102 | plot_filtered_candidates = 0 103 | plot_valid_grasps = 0 104 | plot_clustered_grasps = 0 105 | plot_selected_grasps = 0 106 | -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/include/moveit_task_constructor_gpd/cloud_server.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2020 PickNik Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************/ 32 | 33 | /* Author: Boston Cleek 34 | Desc: Point cloud server for saving point cloud data 35 | */ 36 | 37 | #pragma once 38 | 39 | // ROS 40 | #include 41 | 42 | // C++ 43 | #include 44 | #include 45 | 46 | #include 47 | 48 | namespace moveit_task_constructor_gpd 49 | { 50 | constexpr char LOGNAME[] = "cloud_server"; 51 | 52 | /** 53 | * @brief Provides a service for saving and process a point cloud 54 | * @details When the service is called the point cloud received through 55 | * the point cloud topic is saved. Optionally, either the ground plane 56 | * is removed and/or points outside the specified cartesian limits 57 | * are removed. The resulting cloud is published. 58 | */ 59 | class CloudServer 60 | { 61 | public: 62 | /** 63 | * @brief Constructor 64 | * @param nh - node handle 65 | */ 66 | CloudServer(ros::NodeHandle& nh); 67 | 68 | private: 69 | /** 70 | * @brief Loads parameters 71 | */ 72 | void loadParameters(); 73 | 74 | /** 75 | * @brief Initialize ROS communication 76 | */ 77 | void init(); 78 | 79 | /** 80 | * @brief Point cloud call back 81 | * @param msg - point cloud message 82 | * @details Optionally, either the ground plane 83 | * is removed and/or points outside the specified cartesian limits 84 | * are removed. The resulting cloud is published. 85 | */ 86 | void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg); 87 | 88 | /** 89 | * @brief Service callback for saving a point cloud 90 | * @param req - Service request contains the file name 91 | * @return true when called 92 | * @details The response is empty 93 | */ 94 | bool saveCallback(moveit_task_constructor_gpd::PointCloud::Request& req, 95 | moveit_task_constructor_gpd::PointCloud::Response&); 96 | 97 | private: 98 | ros::NodeHandle nh_; // node handle 99 | ros::Subscriber cloud_sub_; // point cloud subscriber 100 | ros::Publisher cloud_pub_; // publishes the point cloud saved 101 | ros::ServiceServer saver_srv_; // service for saving point clouds 102 | 103 | std::string point_cloud_topic_; // point cloud topic 104 | std::string cloud_dir_; // directory to save 105 | std::string file_name_; // file name to save as 106 | 107 | std::vector xyz_lower_limits_; // lower limits on point cloud 108 | std::vector xyz_upper_limits_; // upper limits on point cloud 109 | 110 | bool save_; // save service activated 111 | bool remove_table_; // specify if to remove table points 112 | bool cartesian_limits_; // specify if to remove points outside limits 113 | }; 114 | } // namespace moveit_task_constructor_gpd 115 | -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/include/moveit_task_constructor_gpd/cloud_utils.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2020 PickNik Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************/ 32 | 33 | /* Author: Boston Cleek 34 | Desc: Utility functions for processing point clouds 35 | */ 36 | 37 | #pragma once 38 | 39 | // C++ 40 | #include 41 | 42 | // PCL 43 | #include 44 | #include 45 | 46 | namespace moveit_task_constructor_gpd 47 | { 48 | typedef pcl::PointCloud PointCloudRGBA; 49 | typedef pcl::PointCloud PointCloudRGB; 50 | 51 | /** 52 | * @brief Segments objects from table plane 53 | * @param [out] cloud - Segemented point cloud XYZRGB 54 | */ 55 | void removeTable(PointCloudRGB::Ptr cloud); 56 | 57 | /** 58 | * @brief Removes points outside the specified cartesian limits 59 | * @param xyz_lower - 3dim vector x,y,z lower limits on cloud 60 | * @param xyz_upper - 3dim vector x,y,z upper limits on cloud 61 | * @param [out] cloud - cloud XYZRGB 62 | */ 63 | void passThroughFilter(const std::vector& xyz_lower, const std::vector& xyz_upper, 64 | PointCloudRGB::Ptr cloud); 65 | } // namespace moveit_task_constructor_gpd 66 | -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/include/moveit_task_constructor_gpd/grasp_detection.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2020 PickNik Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************/ 32 | 33 | /* Author: Boston Cleek 34 | Desc: Grasp pose detection (GPD) using point clouds 35 | */ 36 | 37 | // ROS 38 | #include 39 | 40 | // C++ 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | // Eigen 47 | #include 48 | 49 | // GPD 50 | #include 51 | #include 52 | 53 | // Action Server 54 | #include 55 | #include 56 | 57 | namespace moveit_task_constructor_gpd 58 | { 59 | constexpr char LOGNAME[] = "grasp_pose_detection"; 60 | 61 | /** 62 | * @brief Generates grasp poses for a generator stage with MTC 63 | * @details Interfaces with the GPD lib using ROS messages and interfaces 64 | * with MTC using an Action Server 65 | */ 66 | class GraspDetection 67 | { 68 | public: 69 | /** 70 | * @brief Constructor 71 | * @param nh - node handle 72 | * @details loads parameters, registers callbacks for the action server, 73 | and initializes GPD 74 | */ 75 | GraspDetection(const ros::NodeHandle& nh); 76 | 77 | private: 78 | /** 79 | * @brief Loads parameters for action server, GPD, and relevant transformations 80 | */ 81 | void loadParameters(); 82 | 83 | /** 84 | * @brief Initialize action server callbacks and GPD 85 | * @details The point cloud (frame: panda_link0) is loaded from a file and 86 | * the camera's origin relative to the point cloud is assumed to be at (0,0,0). 87 | */ 88 | void init(); 89 | 90 | /** 91 | * @brief Action server goal callback 92 | * @details Accepts goal from client and samples grasp candidates 93 | */ 94 | void goalCallback(); 95 | 96 | /** 97 | * @brief Preempt callback 98 | * @details Preempts goal 99 | */ 100 | void preemptCallback(); 101 | 102 | /** 103 | * @brief Samples grasp candidates using GPD 104 | * @details Compose grasp candidates, the candidates are sent back to the client 105 | * using the feedback message. Only candidates with a positive grasp 106 | * score are used. If there is at least one candidate with a positive 107 | * score the result is set to success else it is a failure. 108 | */ 109 | void sampleGrasps(); 110 | 111 | /** 112 | * @brief Point cloud call back 113 | * @param msg - point cloud message 114 | * @details Segments objects from table plane 115 | */ 116 | void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg); 117 | 118 | private: 119 | ros::NodeHandle nh_; // node handle 120 | ros::Subscriber cloud_sub_; // subscribes to point cloud 121 | ros::Publisher cloud_pub_; // publishes segmented cloud 122 | 123 | std::unique_ptr> 124 | server_; // action server 125 | moveit_task_constructor_msgs::SampleGraspPosesFeedback feedback_; // action feedback message 126 | moveit_task_constructor_msgs::SampleGraspPosesResult result_; // action result message 127 | 128 | std::string path_to_pcd_file_; // path to cylinder pcd file 129 | std::string path_to_gpd_config_; // path to GPD config file 130 | std::string point_cloud_topic_; // point cloud topic name 131 | std::string goal_name_; // action name 132 | std::string action_name_; // action namespace 133 | std::string frame_id_; // frame of point cloud/grasps 134 | 135 | bool goal_active_; // action goal status 136 | bool load_cloud_; // load cloud from file 137 | 138 | std::vector view_point_; // origin of the camera 139 | std::unique_ptr grasp_detector_; // used to run the GPD algorithm 140 | std::unique_ptr cloud_camera_; // stores point cloud with (optional) camera information 141 | 142 | Eigen::Isometry3d trans_base_cam_; // transformation from base link to camera link 143 | Eigen::Isometry3d transform_cam_opt_; // transformation from camera link to optical link 144 | }; 145 | } // namespace moveit_task_constructor_gpd 146 | -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/launch/gpd_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | [0.0, 0.0, 0.0] 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/media/gpd_barclamp_gazebo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_gpd/media/gpd_barclamp_gazebo.gif -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/media/gpd_barclamp_gazebo2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_gpd/media/gpd_barclamp_gazebo2.gif -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/media/gpd_barclamp_gazebo3.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_gpd/media/gpd_barclamp_gazebo3.gif -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/media/gpd_berry_gazebo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_gpd/media/gpd_berry_gazebo.gif -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/media/gpd_cylinder_gazebo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_gpd/media/gpd_cylinder_gazebo.gif -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/media/gpd_cylinder_gazebo2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_gpd/media/gpd_cylinder_gazebo2.gif -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/media/mtc_gpd_panda.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/deep_grasp_demo/f41031c7502284f1d66a747f8e0dd06b8e70ec35/moveit_task_constructor_gpd/media/mtc_gpd_panda.gif -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | moveit_task_constructor_gpd 4 | 0.0.1 5 | MoveIt task constructor pick and place using grasp pose detection 6 | 7 | Boston Cleek 8 | Boston Cleek 9 | 10 | BSD 11 | 12 | catkin 13 | roscpp 14 | message_generation 15 | 16 | actionlib 17 | eigen_conversions 18 | geometry_msgs 19 | moveit_task_constructor_msgs 20 | rosparam_shortcuts 21 | sensor_msgs 22 | std_msgs 23 | message_runtime 24 | 25 | 26 | -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/src/cloud_server.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2020 PickNik Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************/ 32 | 33 | /* Author: Boston Cleek 34 | Desc: Point cloud server for saving point cloud data 35 | */ 36 | 37 | // ROS 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | namespace moveit_task_constructor_gpd 46 | { 47 | CloudServer::CloudServer(ros::NodeHandle& nh) : nh_(nh), save_(false) 48 | { 49 | loadParameters(); 50 | init(); 51 | } 52 | 53 | void CloudServer::loadParameters() 54 | { 55 | ros::NodeHandle pnh("~"); 56 | size_t errors = 0; 57 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "point_cloud_topic", point_cloud_topic_); 58 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "cloud_dir", cloud_dir_); 59 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "remove_table", remove_table_); 60 | 61 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "cartesian_limits", cartesian_limits_); 62 | if (cartesian_limits_) 63 | { 64 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "xyz_lower_limits", xyz_lower_limits_); 65 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "xyz_upper_limits", xyz_upper_limits_); 66 | } 67 | 68 | rosparam_shortcuts::shutdownIfError(LOGNAME, errors); 69 | } 70 | 71 | void CloudServer::init() 72 | { 73 | cloud_sub_ = nh_.subscribe(point_cloud_topic_, 1, &CloudServer::cloudCallback, this); 74 | cloud_pub_ = nh_.advertise("filtered_cloud", 1, true); 75 | saver_srv_ = nh_.advertiseService("save_point_cloud", &CloudServer::saveCallback, this); 76 | } 77 | 78 | void CloudServer::cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) 79 | { 80 | if (save_) 81 | { 82 | // convert from ROS msg to a point cloud 83 | PointCloudRGB::Ptr cloud(new PointCloudRGB); 84 | pcl::fromROSMsg(*msg.get(), *cloud.get()); 85 | 86 | // segment out table 87 | if (remove_table_) 88 | { 89 | removeTable(cloud); 90 | } 91 | 92 | // remove points out of limits 93 | if (cartesian_limits_) 94 | { 95 | passThroughFilter(xyz_lower_limits_, xyz_upper_limits_, cloud); 96 | } 97 | 98 | // publish the cloud for visualization and debugging purposes 99 | sensor_msgs::PointCloud2 cloud_msg; 100 | pcl::toROSMsg(*cloud.get(), cloud_msg); 101 | cloud_pub_.publish(cloud_msg); 102 | 103 | if (!cloud->points.empty()) 104 | { 105 | ROS_INFO_NAMED(LOGNAME, "Saving point cloud to file..."); 106 | 107 | if (!pcl::io::savePCDFile(cloud_dir_ + file_name_, *cloud.get())) 108 | { 109 | ROS_INFO_NAMED(LOGNAME, "Point cloud saved"); 110 | } 111 | else 112 | { 113 | ROS_ERROR_NAMED(LOGNAME, "Failed to save cloud"); 114 | } 115 | } 116 | else 117 | { 118 | ROS_ERROR_NAMED(LOGNAME, "Point cloud is empty"); 119 | } 120 | 121 | save_ = false; 122 | } 123 | } 124 | 125 | bool CloudServer::saveCallback(moveit_task_constructor_gpd::PointCloud::Request& req, 126 | moveit_task_constructor_gpd::PointCloud::Response&) 127 | { 128 | ROS_INFO_NAMED(LOGNAME, "Saving point cloud service active"); 129 | save_ = true; 130 | file_name_ = req.cloud_file; 131 | return true; 132 | } 133 | } // namespace moveit_task_constructor_gpd 134 | -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/src/grasp_cloud_detection.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2020 PickNik Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************/ 32 | 33 | /* Author: Boston Cleek 34 | Desc: Grasp pose detection (GPD) node, option to load a point cloud from a file 35 | or subscribe to a point cloud topic. If loading from file the the point cloud 36 | subscriber and publisher will not be available. Communication with MTC 37 | is achieved through an action service. This node contains the action client 38 | that sends a list grasp candidates and associated costs to MTC as feedback. 39 | 40 | PARAMETERS: 41 | load_cloud - load point cloud from file 42 | path_to_pcd_file (optional) - path to point cloud file 43 | point_cloud_topic (optional) - point cloud topic 44 | path_to_gpd_config - path to GPD config file 45 | action_name - action namespace 46 | frame_id - frame of the grasp candidates sent to MTC 47 | trans_cam_opt - transform from camera link to optical link 48 | trans_base_cam - transform from robot base link to camera link 49 | view_point - (x,y,z) view point of camera usually (0,0,0) 50 | PUBLISHES: 51 | segmented_cloud (optional) (sensor_msgs/PointCloud2) - Point cloud after table is removed 52 | SUBSCRIBES: 53 | point_cloud_topic (optinal) (sensor_msgs/PointCloud2) - Point cloud from depth camera 54 | */ 55 | 56 | // ROS 57 | #include 58 | 59 | #include 60 | 61 | int main(int argc, char** argv) 62 | { 63 | ROS_INFO_STREAM_NAMED("main", "Starting grasp_cloud_detection"); 64 | ros::init(argc, argv, "grasp_cloud_detection"); 65 | ros::NodeHandle nh; 66 | 67 | ros::AsyncSpinner spinner(1); 68 | spinner.start(); 69 | 70 | moveit_task_constructor_gpd::GraspDetection grasp_detection(nh); 71 | ros::waitForShutdown(); 72 | 73 | ROS_INFO_STREAM_NAMED("main", "Shutting down."); 74 | return 0; 75 | } 76 | -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/src/grasp_detection.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2020 PickNik Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************/ 32 | 33 | /* Author: Boston Cleek 34 | Desc: Grasp pose detection (GPD) using point clouds 35 | */ 36 | 37 | // ROS 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | // Eigen 45 | #include 46 | 47 | #include 48 | #include 49 | 50 | namespace moveit_task_constructor_gpd 51 | { 52 | GraspDetection::GraspDetection(const ros::NodeHandle& nh) : nh_(nh), goal_active_(false) 53 | { 54 | loadParameters(); 55 | init(); 56 | } 57 | 58 | void GraspDetection::loadParameters() 59 | { 60 | ROS_INFO_NAMED(LOGNAME, "Loading grasp action server parameters"); 61 | ros::NodeHandle pnh("~"); 62 | 63 | size_t errors = 0; 64 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "load_cloud", load_cloud_); 65 | if (load_cloud_) 66 | { 67 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "path_to_pcd_file", path_to_pcd_file_); 68 | } 69 | else 70 | { 71 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "point_cloud_topic", point_cloud_topic_); 72 | } 73 | 74 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "path_to_gpd_config", path_to_gpd_config_); 75 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "trans_cam_opt", transform_cam_opt_); 76 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "trans_base_cam", trans_base_cam_); 77 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "action_name", action_name_); 78 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "view_point", view_point_); 79 | errors += !rosparam_shortcuts::get(LOGNAME, pnh, "frame_id", frame_id_); 80 | rosparam_shortcuts::shutdownIfError(LOGNAME, errors); 81 | } 82 | 83 | void GraspDetection::init() 84 | { 85 | // action server 86 | server_.reset(new actionlib::SimpleActionServer( 87 | nh_, action_name_, false)); 88 | server_->registerGoalCallback(std::bind(&GraspDetection::goalCallback, this)); 89 | server_->registerPreemptCallback(std::bind(&GraspDetection::preemptCallback, this)); 90 | server_->start(); 91 | 92 | // GPD point cloud camera, load cylinder from file 93 | // set camera view origin 94 | // assume cloud was taken using one camera 95 | if (load_cloud_) 96 | { 97 | Eigen::Matrix3Xd camera_view_point(3, 1); 98 | camera_view_point << view_point_.at(0), view_point_.at(1), view_point_.at(2); 99 | cloud_camera_.reset(new gpd::util::Cloud(path_to_pcd_file_, camera_view_point)); 100 | } 101 | else 102 | { 103 | cloud_sub_ = nh_.subscribe(point_cloud_topic_, 1, &GraspDetection::cloudCallback, this); 104 | cloud_pub_ = nh_.advertise("segmented_cloud", 1, true); 105 | } 106 | 107 | // Grasp detector 108 | grasp_detector_.reset(new gpd::GraspDetector(path_to_gpd_config_)); 109 | } 110 | 111 | void GraspDetection::goalCallback() 112 | { 113 | goal_name_ = server_->acceptNewGoal()->action_name; 114 | ROS_INFO_NAMED(LOGNAME, "New goal accepted: %s", goal_name_.c_str()); 115 | goal_active_ = true; 116 | 117 | // sample grasps now else need to wait to callback 118 | // use GPD to find the grasp candidates 119 | if (load_cloud_) 120 | { 121 | sampleGrasps(); 122 | } 123 | } 124 | 125 | void GraspDetection::preemptCallback() 126 | { 127 | ROS_INFO_NAMED(LOGNAME, "Preempted %s:", goal_name_.c_str()); 128 | server_->setPreempted(); 129 | } 130 | 131 | void GraspDetection::sampleGrasps() 132 | { 133 | std::vector> grasps; // detect grasp poses 134 | grasp_detector_->preprocessPointCloud(*cloud_camera_); // preprocess the point cloud 135 | grasps = grasp_detector_->detectGrasps(*cloud_camera_); // detect grasps in the point cloud 136 | 137 | // Use grasps with score > 0 138 | std::vector grasp_id; 139 | for (unsigned int i = 0; i < grasps.size(); i++) 140 | { 141 | if (grasps.at(i)->getScore() > 0.0) 142 | { 143 | grasp_id.push_back(i); 144 | } 145 | } 146 | 147 | if (grasp_id.empty()) 148 | { 149 | ROS_ERROR_NAMED(LOGNAME, "No grasp candidates found with a positive cost"); 150 | result_.grasp_state = "failed"; 151 | server_->setAborted(result_); 152 | return; 153 | } 154 | 155 | for (auto id : grasp_id) 156 | { 157 | // transform grasp from camera optical link into frame_id (panda_link0) 158 | const Eigen::Isometry3d transform_opt_grasp = 159 | Eigen::Translation3d(grasps.at(id)->getPosition()) * Eigen::Quaterniond(grasps.at(id)->getOrientation()); 160 | 161 | const Eigen::Isometry3d transform_base_grasp = trans_base_cam_ * transform_cam_opt_ * transform_opt_grasp; 162 | const Eigen::Vector3d trans = transform_base_grasp.translation(); 163 | const Eigen::Quaterniond rot(transform_base_grasp.rotation()); 164 | 165 | // convert back to PoseStamped 166 | geometry_msgs::PoseStamped grasp_pose; 167 | grasp_pose.header.frame_id = frame_id_; 168 | grasp_pose.pose.position.x = trans.x(); 169 | grasp_pose.pose.position.y = trans.y(); 170 | grasp_pose.pose.position.z = trans.z(); 171 | 172 | grasp_pose.pose.orientation.w = rot.w(); 173 | grasp_pose.pose.orientation.x = rot.x(); 174 | grasp_pose.pose.orientation.y = rot.y(); 175 | grasp_pose.pose.orientation.z = rot.z(); 176 | 177 | feedback_.grasp_candidates.emplace_back(grasp_pose); 178 | 179 | // Grasp is selected based on cost not score 180 | // Invert score to represent grasp with lowest cost 181 | feedback_.costs.emplace_back(static_cast(1.0 / grasps.at(id)->getScore())); 182 | } 183 | 184 | server_->publishFeedback(feedback_); 185 | result_.grasp_state = "success"; 186 | server_->setSucceeded(result_); 187 | } 188 | 189 | void GraspDetection::cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) 190 | { 191 | if (goal_active_) 192 | { 193 | PointCloudRGB::Ptr cloud(new PointCloudRGB); 194 | pcl::fromROSMsg(*msg.get(), *cloud.get()); 195 | 196 | // Segementation works best with XYXRGB 197 | removeTable(cloud); 198 | 199 | // publish the cloud for visualization and debugging purposes 200 | sensor_msgs::PointCloud2 cloud_msg; 201 | pcl::toROSMsg(*cloud.get(), cloud_msg); 202 | cloud_pub_.publish(cloud_msg); 203 | 204 | // TODO: set alpha channel to 1 205 | // GPD required XYZRGBA 206 | PointCloudRGBA::Ptr grasp_cloud(new PointCloudRGBA); 207 | pcl::copyPointCloud(*cloud.get(), *grasp_cloud.get()); 208 | 209 | // Construct the cloud camera 210 | Eigen::Matrix3Xd camera_view_point(3, 1); 211 | camera_view_point << view_point_.at(0), view_point_.at(1), view_point_.at(2); 212 | cloud_camera_.reset(new gpd::util::Cloud(grasp_cloud, 0, camera_view_point)); 213 | 214 | // use GPD to find the grasp candidates 215 | sampleGrasps(); 216 | } 217 | 218 | goal_active_ = false; 219 | } 220 | } // namespace moveit_task_constructor_gpd 221 | -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/src/moveit_task_constructor_gpd/cloud_utils.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2020 PickNik Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************/ 32 | 33 | /* Author: Boston Cleek 34 | Desc: Utility functions for processing point clouds 35 | */ 36 | 37 | // ROS 38 | #include 39 | 40 | // PCL 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | 49 | namespace moveit_task_constructor_gpd 50 | { 51 | void removeTable(PointCloudRGB::Ptr cloud) 52 | { 53 | // SAC segmentor without normals 54 | pcl::SACSegmentation segmentor; 55 | segmentor.setOptimizeCoefficients(true); 56 | segmentor.setModelType(pcl::SACMODEL_PLANE); 57 | segmentor.setMethodType(pcl::SAC_RANSAC); 58 | 59 | // Max iterations and model tolerance 60 | segmentor.setMaxIterations(1000); 61 | segmentor.setDistanceThreshold(0.01); 62 | 63 | // Input cloud 64 | segmentor.setInputCloud(cloud); 65 | 66 | // Inliers representing points in the plane 67 | pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices); 68 | 69 | // Use a plane as the model for the segmentor 70 | pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients); 71 | segmentor.segment(*inliers_plane.get(), *coefficients_plane.get()); 72 | 73 | if (inliers_plane->indices.size() == 0) 74 | { 75 | ROS_ERROR("Could not estimate a planar model for the given dataset"); 76 | } 77 | 78 | // Extract the inliers from the cloud 79 | pcl::ExtractIndices extract_indices; 80 | extract_indices.setInputCloud(cloud); 81 | extract_indices.setIndices(inliers_plane); 82 | 83 | // Remove plane inliers and extract the rest 84 | extract_indices.setNegative(true); 85 | extract_indices.filter(*cloud.get()); 86 | 87 | if (cloud->points.empty()) 88 | { 89 | ROS_ERROR("Point cloud is empty, segementation failed"); 90 | } 91 | } 92 | 93 | void passThroughFilter(const std::vector& xyz_lower, const std::vector& xyz_upper, 94 | PointCloudRGB::Ptr cloud) 95 | { 96 | pcl::PassThrough pass; 97 | pass.setInputCloud(cloud); 98 | 99 | pass.setFilterFieldName("x"); 100 | pass.setFilterLimits(xyz_lower.at(0), xyz_upper.at(0)); 101 | pass.filter(*cloud.get()); 102 | 103 | pass.setFilterFieldName("y"); 104 | pass.setFilterLimits(xyz_lower.at(1), xyz_upper.at(1)); 105 | pass.filter(*cloud.get()); 106 | 107 | pass.setFilterFieldName("z"); 108 | pass.setFilterLimits(xyz_lower.at(2), xyz_upper.at(2)); 109 | pass.filter(*cloud.get()); 110 | } 111 | } // namespace moveit_task_constructor_gpd 112 | -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/src/point_cloud_server.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2020 PickNik Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * * Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *********************************************************************/ 32 | 33 | /* Author: Boston Cleek 34 | Desc: Point cloud server node for saving point cloud data. Optionally, either 35 | the ground plane is removed and/or points outside the specified cartesian 36 | limits are removed. The resulting cloud is published. 37 | 38 | PARAMETERS: 39 | point_cloud_topic - point cloud topic to subscribe to 40 | cloud_dir - directory to save point clouds to 41 | remove_table - whether or not to segement table points 42 | cartesian_limits - whether or not to remove points outside limits 43 | xyz_lower_limits - lower (x,y,z) limits on points 44 | xyz_upper_limits - upper (x,y,z) limits on points 45 | PUBLISHES: 46 | filtered_cloud (sensor_msgs/PointCloud2) - Point cloud after table is removed 47 | SUBSCRIBES: 48 | point_cloud_topic (sensor_msgs/PointCloud2) - Point cloud from depth camera 49 | SERVICES: 50 | save_point_cloud (moveit_task_constructor_gpd/PointCloud) - specify file name to save point cloud 51 | */ 52 | 53 | // ROS 54 | #include 55 | 56 | #include 57 | 58 | int main(int argc, char** argv) 59 | { 60 | ROS_INFO_STREAM_NAMED("main", "Starting point_cloud_server"); 61 | ros::init(argc, argv, "point_cloud_server"); 62 | ros::NodeHandle nh; 63 | 64 | ros::AsyncSpinner spinner(1); 65 | spinner.start(); 66 | 67 | moveit_task_constructor_gpd::CloudServer cloud_server(nh); 68 | ros::waitForShutdown(); 69 | 70 | ROS_INFO_STREAM_NAMED("main", "Shutting down."); 71 | return 0; 72 | } 73 | -------------------------------------------------------------------------------- /moveit_task_constructor_gpd/srv/PointCloud.srv: -------------------------------------------------------------------------------- 1 | # Request saving depth and rgb images 2 | # Input is file names to save them as 3 | string cloud_file 4 | --- 5 | -------------------------------------------------------------------------------- /opencv_install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # install OpenCV 3.4 3 | 4 | # dependencies 5 | sudo apt install build-essential 6 | sudo apt install cmake git libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev 7 | 8 | # image processing 9 | sudo apt install python-dev python-numpy libtbb2 libtbb-dev libjpeg-dev libpng-dev libtiff-dev libjasper-dev libdc1394-22-dev 10 | 11 | # video processing 12 | sudo apt install libavcodec-dev libavformat-dev libswscale-dev libv4l-dev 13 | sudo apt install libxvidcore-dev libx264-dev 14 | 15 | # GUI support 16 | sudo apt install libgtk-3-dev 17 | 18 | # Optimization 19 | sudo apt install libatlas-base-dev gfortran pylint 20 | 21 | # Download and build OpenCV 22 | wget https://github.com/opencv/opencv/archive/3.4.0.zip -O opencv-3.4.0.zip 23 | wget https://github.com/opencv/opencv_contrib/archive/3.4.0.zip -O opencv_contrib-3.4.0.zip 24 | 25 | sudo apt install unzip 26 | 27 | unzip opencv-3.4.0.zip 28 | unzip opencv_contrib-3.4.0.zip 29 | 30 | cd opencv-3.4.0 31 | mkdir build 32 | cd build 33 | 34 | cmake -DCMAKE_BUILD_TYPE=Release -D WITH_CUDA=OFF -DCMAKE_INSTALL_PREFIX=/usr/local -DOPENCV_EXTRA_MODULES_PATH=../../opencv_contrib-3.4.0/modules -DOPENCV_ENABLE_NONFREE=True .. 35 | make -j4 36 | sudo make install 37 | 38 | sudo ldconfig 39 | -------------------------------------------------------------------------------- /pcl_install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # install PCL 1.11.0 3 | wget https://github.com/PointCloudLibrary/pcl/archive/pcl-1.11.0.zip -O pcl-pcl-1.11.0.zip 4 | sudo apt install unzip 5 | unzip pcl-pcl-1.11.0.zip 6 | cd pcl-pcl-1.11.0 7 | mkdir build && cd build 8 | cmake -DCMAKE_BUILD_TYPE=Release .. 9 | make -j4 10 | sudo make install 11 | --------------------------------------------------------------------------------