├── CMakeLists.txt ├── LICENSE ├── README.md ├── img └── demo_image.png ├── include └── grasp_detection │ └── GraspPoseConverter.h ├── launch └── grasp_detection_conversion_server.launch ├── nodes └── patch_based_grasp_detection_node.py ├── package.xml ├── setup.py ├── src ├── GraspPoseConverter.cpp ├── grasp_conversion_server_node.cpp └── grasp_detection │ ├── __init__.py │ └── patch_based_grasp_detection │ ├── __init__.py │ ├── graspNet.py │ ├── grasp_learner.py │ └── grasp_predictor.py └── srv ├── GraspConversion.srv └── GraspDetection.srv /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(grasp_detection) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | sensor_msgs 14 | std_msgs 15 | geometry_msgs 16 | moveit_msgs 17 | message_generation 18 | cv_bridge 19 | image_geometry 20 | tf 21 | ) 22 | 23 | catkin_python_setup() 24 | 25 | ## System dependencies are found with CMake's conventions 26 | # find_package(Boost REQUIRED COMPONENTS system) 27 | 28 | ## Uncomment this if the package has a setup.py. This macro ensures 29 | ## modules and global scripts declared therein get installed 30 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 31 | # catkin_python_setup() 32 | 33 | ################################################ 34 | ## Declare ROS messages, services and actions ## 35 | ################################################ 36 | 37 | ## To declare and build messages, services or actions from within this 38 | ## package, follow these steps: 39 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 40 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 41 | ## * In the file package.xml: 42 | ## * add a build_depend tag for "message_generation" 43 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 44 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 45 | ## but can be declared for certainty nonetheless: 46 | ## * add a run_depend tag for "message_runtime" 47 | ## * In this file (CMakeLists.txt): 48 | ## * add "message_generation" and every package in MSG_DEP_SET to 49 | ## find_package(catkin REQUIRED COMPONENTS ...) 50 | ## * add "message_runtime" and every package in MSG_DEP_SET to 51 | ## catkin_package(CATKIN_DEPENDS ...) 52 | ## * uncomment the add_*_files sections below as needed 53 | ## and list every .msg/.srv/.action file to be processed 54 | ## * uncomment the generate_messages entry below 55 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 56 | 57 | ## Generate messages in the 'msg' folder 58 | # add_message_files( 59 | # FILES 60 | # Message1.msg 61 | # Message2.msg 62 | # ) 63 | 64 | ## Generate services in the 'srv' folder 65 | add_service_files( 66 | DIRECTORY srv 67 | FILES 68 | GraspDetection.srv 69 | GraspConversion.srv 70 | ) 71 | 72 | ## Generate actions in the 'action' folder 73 | # add_action_files( 74 | # FILES 75 | # Action1.action 76 | # Action2.action 77 | # ) 78 | 79 | ## Generate added messages and services with any dependencies listed here 80 | generate_messages( 81 | DEPENDENCIES 82 | sensor_msgs 83 | std_msgs 84 | moveit_msgs 85 | ) 86 | 87 | ################################################ 88 | ## Declare ROS dynamic reconfigure parameters ## 89 | ################################################ 90 | 91 | ## To declare and build dynamic reconfigure parameters within this 92 | ## package, follow these steps: 93 | ## * In the file package.xml: 94 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 95 | ## * In this file (CMakeLists.txt): 96 | ## * add "dynamic_reconfigure" to 97 | ## find_package(catkin REQUIRED COMPONENTS ...) 98 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 99 | ## and list every .cfg file to be processed 100 | 101 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 102 | # generate_dynamic_reconfigure_options( 103 | # cfg/DynReconf1.cfg 104 | # cfg/DynReconf2.cfg 105 | # ) 106 | 107 | ################################### 108 | ## catkin specific configuration ## 109 | ################################### 110 | ## The catkin_package macro generates cmake config files for your package 111 | ## Declare things to be passed to dependent projects 112 | ## INCLUDE_DIRS: uncomment this if your package contains header files 113 | ## LIBRARIES: libraries you create in this project that dependent projects also need 114 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 115 | ## DEPENDS: system dependencies of this project that dependent projects also need 116 | catkin_package( 117 | # INCLUDE_DIRS include 118 | # LIBRARIES grasp_detection 119 | CATKIN_DEPENDS roscpp rospy sensor_msgs std_msgs moveit_msgs image_geometry tf 120 | # DEPENDS system_lib 121 | ) 122 | 123 | ########### 124 | ## Build ## 125 | ########### 126 | 127 | ## Specify additional locations of header files 128 | ## Your package locations should be listed before other locations 129 | include_directories( 130 | include 131 | ${catkin_INCLUDE_DIRS} 132 | ${Eigen_INCLUDE_DIRS} 133 | ) 134 | 135 | ## Declare a C++ library 136 | # add_library(${PROJECT_NAME} 137 | # src/${PROJECT_NAME}/grasp_detection.cpp 138 | # ) 139 | 140 | ## Add cmake target dependencies of the library 141 | ## as an example, code may need to be generated before libraries 142 | ## either from message generation or dynamic reconfigure 143 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 144 | 145 | ## Declare a C++ executable 146 | ## With catkin_make all packages are built within a single CMake context 147 | ## The recommended prefix ensures that target names across packages don't collide 148 | # add_executable(${PROJECT_NAME}_node src/grasp_detection_node.cpp) 149 | 150 | ## Rename C++ executable without prefix 151 | ## The above recommended prefix causes long target names, the following renames the 152 | ## target back to the shorter version for ease of user use 153 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 154 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 155 | 156 | ## Add cmake target dependencies of the executable 157 | ## same as for the library above 158 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 159 | 160 | ## Specify libraries to link a library or executable target against 161 | # target_link_libraries(${PROJECT_NAME}_node 162 | # ${catkin_LIBRARIES} 163 | # ) 164 | 165 | ############# 166 | ## Install ## 167 | ############# 168 | 169 | # all install targets should use catkin DESTINATION variables 170 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 171 | 172 | ## Mark executable scripts (Python etc.) for installation 173 | ## in contrast to setup.py, you can choose the destination 174 | # install(PROGRAMS 175 | # scripts/my_python_script 176 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 177 | # ) 178 | 179 | ## Mark executables and/or libraries for installation 180 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 181 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 182 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 183 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 184 | # ) 185 | 186 | ## Mark cpp header files for installation 187 | # install(DIRECTORY include/${PROJECT_NAME}/ 188 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 189 | # FILES_MATCHING PATTERN "*.h" 190 | # PATTERN ".svn" EXCLUDE 191 | # ) 192 | 193 | ## Mark other files for installation (e.g. launch and bag files, etc.) 194 | # install(FILES 195 | # # myfile1 196 | # # myfile2 197 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 198 | # ) 199 | 200 | ############# 201 | ## Testing ## 202 | ############# 203 | 204 | ## Add gtest based cpp test target and link libraries 205 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_grasp_detection.cpp) 206 | # if(TARGET ${PROJECT_NAME}-test) 207 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 208 | # endif() 209 | 210 | ## Add folders to be run by python nosetests 211 | # catkin_add_nosetests(test) 212 | set(OpenCV_DIR "/usr/local/include/opencv2") 213 | find_package( OpenCV REQUIRED ) 214 | find_package(PCL 1.2 REQUIRED) 215 | include_directories(${PCL_INCLUDE_DIRS}) 216 | link_directories(${PCL_LIBRARY_DIRS}) 217 | add_definitions(${PCL_DEFINITIONS}) 218 | 219 | 220 | add_executable(grasp_conversion_server_node 221 | include/grasp_detection/GraspPoseConverter.h 222 | src/GraspPoseConverter.cpp 223 | src/grasp_conversion_server_node.cpp) 224 | target_link_libraries(grasp_conversion_server_node ${catkin_LIBRARIES} ${OpenCV_LIBS} ${PCL_LIBRARIES}) 225 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Chuanhao Li 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # planar_grasp_detection 2 | A ROS package that detects grasp rectangles from rgbd image and converts that rectangle to gripper pose using depth image. 3 | 4 | The project is still under progress. Currently I adopted tons of code from many other sources. Three main components are written as ROS services and called from another python script to make it easy to tinker with. Not sure if this wastes too much time on sending and receiving messages. 5 | 6 | The three ROS services are: 7 | 1. GraspSampling.srv 8 | 2. GraspDetection.srv 9 | 3. GraspConversion.srv 10 | 11 | GraspSampling code is adopted from [gqcnn](https://github.com/BerkeleyAutomation/gqcnn) to generate candidate antipodal grasps from a depth image. Then I segment a square image patch from each antipodal grasp (size of the image patch is 1.2 times of the gripper width), and feed this to the grasp detection server where a convolutional neural network is used to make predictions based on this patch. Therefore, this step gives us image patches (potentially of different size since the gripper would look smaller in the image if it is further) at the location of antipodal grasps. 12 | 13 | For the grasp detection server, I adapted the code from paper [Supervision via Competition: Robot Adversaries for Learning Tasks](https://arxiv.org/pdf/1610.01685v1.pdf), which takes rgb image patch as input. Note that all patches are resized into shape 224\*224\*3, and then fed into convolutional neural network. The neural net is trained to produce probability of a successful grasp for each rotation angle (0, 10, ..., 170, that is 18 angles in total) on the given patch. 14 | 15 | Then I applied a weight matrix on the probability to combine the predictions on angles and the computed antipodal angle based on the angle difference. 16 | 17 | The third service takes results from grasp detection and depth image as inputs, and converts to gripper pose using the procedure described in the paper Deep Learning for Detecting Robotic Grasps. Note that I am using a robotiq 85 gripper, so people using a different gripper should change the FINGER_LENGTH accordingly. 18 | 19 | Below is screen shot of how it looks in my application. The third window on the right shows the detected grasp rectangles, and the arrows in the main window are the converted grasp poses. 20 | 21 | ![demo_img](https://github.com/cyrilli/planar_grasp_detection/blob/master/img/demo_image.png?raw=true) 22 | 23 | The following is a video demo using this ROS packge to grasp objects from a table in Gazebo simulation. 24 | 25 | [![IMAGE ALT TEXT](http://img.youtube.com/vi/qlhEtHCVZsw/0.jpg)](http://www.youtube.com/watch?v=qlhEtHCVZsw "Robot Grasp Pose Detection Demo") 26 | -------------------------------------------------------------------------------- /img/demo_image.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyrilli/planar_grasp_detection/6509505b9b85e55c70f54a41411ddc010a005a38/img/demo_image.png -------------------------------------------------------------------------------- /include/grasp_detection/GraspPoseConverter.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2017 Harbin Institute of Technology 3 | * 4 | * This file is part of the Pick and Place Project for my Master's thesis 5 | * 6 | * All Rights Reserved. 7 | ******************************************************************************/ 8 | 9 | // Author: Chuanhao Li 10 | 11 | #ifndef GRASPPOSECONVERTER_H 12 | #define GRASPPOSECONVERTER_H 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | using std::cout; 23 | 24 | class GraspPoseConverter { 25 | public: 26 | GraspPoseConverter(const cv::Mat &input_img, double c_factor, double cx, double cy, double fx, double fy); 27 | ~GraspPoseConverter(); 28 | 29 | cv::Point3f graspPoint3D; 30 | cv::Vec3f rotVec; 31 | float rotAng; 32 | 33 | void convertTo3DGrasp(const int &h, const int &w, const float &angle); 34 | 35 | private: 36 | void convertTo3DGrasp(); 37 | void robotNomrAroundClosestPt(); 38 | void ave3DNormAroundP(); 39 | void smoothDepthForNorm(); // 40 | void depthPatchAroundP(); 41 | void thresholdFromCenterValue(); // first convert to 3D space then compute surface normals 42 | void getSurfNorm3D(); 43 | void convertImagePointTo3DGraspPoint(); 44 | 45 | cv::Point2i graspPoint2D; //store the 2D grasp point 46 | cv::Point2i minDepthPoint2D; //the point with minimum depth in the region around grasp point 47 | double minD; //mimimun depth in the region around grasp point 48 | int img_height; 49 | int img_width; 50 | cv::Mat depth_img; // Depth Image 51 | cv::Mat depth_img_smooth; 52 | cv::Mat inRectMask; // mask of the ROI region centered at grasp point 53 | cv::Mat nonZeroMask; 54 | // compare the depth value of each point in depth_img_smooth, and get the mask of those who 55 | // has depth value that is smaller than centVal + thresh 56 | cv::Mat centDepthValThresholdMask; 57 | cv::Mat minDepthPointRegion; // the ROI region centered at minDepthPoint2D 58 | 59 | double camera_factor; 60 | double camera_cx; 61 | double camera_cy; 62 | double camera_fx; 63 | double camera_fy; 64 | }; 65 | 66 | #endif // GRASPPOSECONVERTER_H -------------------------------------------------------------------------------- /launch/grasp_detection_conversion_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /nodes/patch_based_grasp_detection_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | Copyright (C) 2017 Harbin Institute of Technology 4 | 5 | This file is part of the Pick and Place Project for my Master's thesis 6 | 7 | All Rights Reserved. 8 | 9 | Author: Chuanhao Li 10 | 11 | This node is the server for the service GraspDetection, which takes a RGB image as input and detects grasp pose as 12 | bounding boxes. 13 | """ 14 | 15 | 16 | import rospy 17 | import roslib 18 | import rospkg 19 | roslib.load_manifest('grasp_detection') 20 | 21 | import cv2 22 | import numpy as np 23 | import time 24 | from cv_bridge import CvBridge 25 | from std_msgs.msg import Int32MultiArray, Float32MultiArray 26 | from grasp_detection.patch_based_grasp_detection.grasp_learner import grasp_obj 27 | from grasp_detection.patch_based_grasp_detection.grasp_predictor import Predictors 28 | from grasp_detection.srv import GraspDetection, GraspDetectionResponse 29 | from sensor_msgs.msg import Image 30 | 31 | class GraspDetector(): 32 | def __init__(self): 33 | self._cv_bridge = CvBridge() 34 | rospack = rospkg.RosPack() 35 | model_path = rospack.get_path('grasp_detection')+'/models/patch_based_grasp_detection/Grasp_model' 36 | nsamples = 500 37 | self.nbest = 10 38 | max_batchsize = 128 39 | gpu_id = 0 # set to -1 when using cpu 40 | 41 | ## Set up model 42 | if nsamples > max_batchsize: 43 | self.batchsize = max_batchsize 44 | self.nbatches = int(nsamples / max_batchsize) + 1 45 | else: 46 | self.batchsize = nsamples 47 | self.nbatches = 1 48 | 49 | print('Loading grasp model') 50 | self.G = grasp_obj(model_path, gpu_id) 51 | self.G.BATCH_SIZE = self.batchsize 52 | self.G.test_init() 53 | 54 | self._visualization = rospy.get_param('~visualization', True) 55 | self.vis_pub = rospy.Publisher('/grasp_detection/visualization', Image, queue_size=1) 56 | self.s = rospy.Service('grasp_detection', GraspDetection, self.handle_grasp_detection) 57 | #self.G.test_close() 58 | rospy.loginfo("Waiting for request!") 59 | 60 | def handle_grasp_detection(self, req): 61 | """ 62 | After receiving request from client for GraspDetection service, call GraspDetector to 63 | find best grasps in the RGB image. 64 | :param req: GraspDetection service request, including color_image and gscale 65 | :return: GraspDetection service response, including h, w, and angle of best grasps 66 | """ 67 | rospy.loginfo("Request received!") 68 | I = np.array(self._cv_bridge.imgmsg_to_cv2(req.color_image, "bgr8")) 69 | imsize = max(I.shape[:2]) 70 | gsize = int(req.gscale.data * imsize) # Size of grasp patch 71 | P = Predictors(I, self.G) 72 | 73 | fc8_predictions = [] 74 | patch_Hs = [] 75 | patch_Ws = [] 76 | 77 | st_time = time.time() 78 | for _ in range(self.nbatches): 79 | P.graspNet_grasp(patch_size=gsize, num_samples=self.batchsize); 80 | fc8_predictions.append(P.fc8_norm_vals) 81 | patch_Hs.append(P.patch_hs) 82 | patch_Ws.append(P.patch_ws) 83 | 84 | fc8_predictions = np.concatenate(fc8_predictions) 85 | patch_Hs = np.concatenate(patch_Hs) 86 | patch_Ws = np.concatenate(patch_Ws) 87 | 88 | largest_idxs = self.k_largest_index_argsort(fc8_predictions, self.nbest) 89 | best_patch_Hs = [] 90 | best_patch_Ws = [] 91 | best_patch_Angles = [] 92 | for idx in largest_idxs: 93 | if self._visualization: 94 | I = self.drawRectangle(I, patch_Hs[idx[0]], patch_Ws[idx[0]], idx[1], gsize) 95 | best_patch_Hs.append(patch_Hs[idx[0]]) 96 | best_patch_Ws.append(patch_Ws[idx[0]]) 97 | grasp_angle = (idx[1] * (np.pi / 18) - np.pi / 2) * 180 / np.pi # convert to degree 98 | best_patch_Angles.append(grasp_angle) 99 | 100 | rospy.loginfo('Time taken: {}s'.format(time.time() - st_time)) 101 | resp = GraspDetectionResponse() 102 | resp.best_grasps_h = Int32MultiArray(data = best_patch_Hs) 103 | resp.best_grasps_w = Int32MultiArray(data = best_patch_Ws) 104 | resp.best_grasps_angle = Float32MultiArray(data = best_patch_Angles) 105 | if self._visualization: 106 | result_msg = self._cv_bridge.cv2_to_imgmsg(I, "bgr8") 107 | self.vis_pub.publish(result_msg) 108 | #cv2.imwrite("/home/lichuanhao/bbox.png", I) 109 | return resp 110 | 111 | def k_largest_index_argpartition(self, a, k): 112 | """ 113 | Find the index of the largest value in array. 114 | The returned index is not in order, but this is computationally cheaper. 115 | :param a: a 2D array 116 | :param k: k biggest values 117 | :return: index of k biggest value in array a 118 | """ 119 | idx = np.argpartition(a.ravel(), a.size-k)[-k:] 120 | return np.column_stack(np.unravel_index(idx, a.shape)) 121 | 122 | def k_largest_index_argsort(self, a, k): 123 | """ 124 | Find the index of the largest value in array. 125 | The returned index is in order. 126 | :param a: a 2D array 127 | :param k: k biggest values 128 | :return: index of k biggest value in array a 129 | """ 130 | idx = np.argsort(a.ravel())[:-k-1:-1] 131 | return np.column_stack(np.unravel_index(idx, a.shape)) 132 | 133 | def drawRectangle(self, I, h, w, t, gsize=300): 134 | I_temp = I 135 | grasp_l = gsize / 2.5 136 | grasp_w = gsize / 5.0 137 | grasp_angle = t * (np.pi / 18) - np.pi / 2 138 | points = np.array([[-grasp_l, -grasp_w], 139 | [grasp_l, -grasp_w], 140 | [grasp_l, grasp_w], 141 | [-grasp_l, grasp_w]]) 142 | R = np.array([[np.cos(grasp_angle), -np.sin(grasp_angle)], 143 | [np.sin(grasp_angle), np.cos(grasp_angle)]]) 144 | rot_points = np.dot(R, points.transpose()).transpose() 145 | im_points = rot_points + np.array([w, h]) 146 | cv2.line(I_temp, tuple(im_points[0].astype(int)), tuple(im_points[1].astype(int)), color=(0, 255, 0), 147 | thickness=5) 148 | cv2.line(I_temp, tuple(im_points[1].astype(int)), tuple(im_points[2].astype(int)), color=(0, 0, 255), 149 | thickness=5) 150 | cv2.line(I_temp, tuple(im_points[2].astype(int)), tuple(im_points[3].astype(int)), color=(0, 255, 0), 151 | thickness=5) 152 | cv2.line(I_temp, tuple(im_points[3].astype(int)), tuple(im_points[0].astype(int)), color=(0, 0, 255), 153 | thickness=5) 154 | return I_temp 155 | 156 | def main(self): 157 | rospy.spin() 158 | 159 | if __name__ == "__main__": 160 | rospy.init_node('patch_based_grasp_detection_node', anonymous = True) 161 | gd = GraspDetector() 162 | gd.main() 163 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | grasp_detection 4 | 0.0.0 5 | The grasp_detection package 6 | 7 | 8 | 9 | 10 | lichuanhao 11 | 12 | 13 | 14 | 15 | 16 | MIT 17 | 18 | 19 | 20 | 21 | 22 | https://github.com/cyrilli/planar_grasp_detection/ 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 | catkin 52 | roscpp 53 | rospy 54 | sensor_msgs 55 | std_msgs 56 | roscpp 57 | rospy 58 | sensor_msgs 59 | std_msgs 60 | message_generation 61 | cv_bridge 62 | geometry_msgs 63 | moveit_msgs 64 | image_geometry 65 | tf 66 | tf 67 | image_geometry 68 | moveit_msgs 69 | geometry_msgs 70 | cv_bridge 71 | message_runtime 72 | roscpp 73 | rospy 74 | sensor_msgs 75 | std_msgs 76 | python-tensorflow-pip 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['grasp_detection'], 9 | package_dir={'': 'src'}, 10 | requires=['std_msgs', 'rospy', 'sensor_msgs'] 11 | ) 12 | 13 | setup(**setup_args) 14 | -------------------------------------------------------------------------------- /src/GraspPoseConverter.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2017 Harbin Institute of Technology 3 | * 4 | * This file is part of the Pick and Place Project for my Master's thesis 5 | * 6 | * All Rights Reserved. 7 | ******************************************************************************/ 8 | 9 | // Author: Chuanhao Li 10 | 11 | #include "grasp_detection/GraspPoseConverter.h" 12 | 13 | GraspPoseConverter::GraspPoseConverter(const cv::Mat &input_img, double c_factor = 1000.0, double cx = 320.5, double cy = 240.5, 14 | double fx = 577.2959393832757, double fy = 577.2959393832757):depth_img(input_img){ 15 | if(depth_img.empty()){ 16 | printf("Converter instantiation failed! Depth image is empty!\n"); 17 | } 18 | else{ 19 | std::cout<<"======Successfully instantiate converter!======\n"<(std::floor(minDepthPointRegion.rows/2),std::floor(minDepthPointRegion.cols/2))); 90 | cv::threshold(minDepthPointRegion, centDepthValThresholdMask, centVal+DIST_THRESH, 255, cv::THRESH_BINARY_INV); 91 | } 92 | 93 | void GraspPoseConverter::getSurfNorm3D(){ 94 | /* 95 | First convert all points in the image to 3D coordinates, 96 | and then compute their surface normals. 97 | */ 98 | pcl::PointCloud::Ptr cloud (new pcl::PointCloud); 99 | // loop over the whole depth map 100 | for (int m = 0; m < minDepthPointRegion.rows; m++){ 101 | for (int n = 0; n < minDepthPointRegion.cols; n++){ 102 | // get the value at (m,n) in the depth image 103 | //double d = minDepthPointRegion.ptr(m)[n]; // the output values are sooo big!! 104 | double d = (double) minDepthPointRegion.at(m, n); 105 | // if d has no value, skip 106 | if (d == 0){ 107 | continue; 108 | } 109 | if (d-minD > 0.05){ 110 | continue; 111 | } 112 | // if d has value, add a point 113 | pcl::PointXYZ p; 114 | 115 | // compute the 3D coordinate of this point 116 | p.z = d / camera_factor; 117 | p.x = (n - camera_cx) * p.z / camera_fx; 118 | p.y = (m - camera_cy) * p.z / camera_fy; 119 | // add p into cloud 120 | cloud->points.push_back(p); 121 | } 122 | } 123 | // set some params 124 | cloud->height = 1; 125 | cloud->width = cloud->points.size(); 126 | cloud->is_dense = false; 127 | //std::cout<<"cloud contains num of points: "<points.size()< ne; 132 | ne.setInputCloud (cloud); 133 | 134 | // Create an empty kdtree representation, and pass it to the normal estimation object. 135 | // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given). 136 | pcl::search::KdTree::Ptr tree (new pcl::search::KdTree ()); 137 | ne.setSearchMethod (tree); 138 | 139 | // Output datasets 140 | pcl::PointCloud::Ptr cloud_normals (new pcl::PointCloud); 141 | 142 | // Use all neighbors in a sphere of radius 3cm 143 | ne.setRadiusSearch (0.03); 144 | 145 | // Compute the features 146 | ne.compute (*cloud_normals); 147 | 148 | float sum_normal_x, sum_normal_y, sum_normal_z; 149 | // iterate over each points in cloud_normals 150 | for(pcl::PointCloud::iterator it = cloud_normals->begin(); it!=cloud_normals->end();it++){ 151 | sum_normal_x += it->normal_x; 152 | sum_normal_y += it->normal_y; 153 | sum_normal_z += it->normal_z; 154 | } 155 | float point_num = (float) cloud_normals->points.size(); 156 | rotVec[0] = sum_normal_x / point_num; 157 | rotVec[1] = sum_normal_y / point_num; 158 | rotVec[2] = sum_normal_z / point_num; 159 | rotVec = cv::normalize(rotVec); 160 | } 161 | 162 | void GraspPoseConverter::convertImagePointTo3DGraspPoint(){ 163 | // compute the 3D coordinate of this point 164 | graspPoint3D.z = minD / camera_factor; 165 | graspPoint3D.x = (minDepthPoint2D.x - camera_cx) * graspPoint3D.z / camera_fx; 166 | graspPoint3D.y = (minDepthPoint2D.y - camera_cy) * graspPoint3D.z / camera_fy; 167 | } -------------------------------------------------------------------------------- /src/grasp_conversion_server_node.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright (C) 2017 Harbin Institute of Technology 3 | * 4 | * This file is part of the Pick and Place Project for my Master's thesis 5 | * 6 | * All Rights Reserved. 7 | ******************************************************************************/ 8 | 9 | // Author: Chuanhao Li 10 | 11 | #include "ros/ros.h" 12 | #include "std_msgs/String.h" 13 | #include "sensor_msgs/Image.h" 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include "grasp_detection/GraspPoseConverter.h" 22 | #include "grasp_detection/GraspConversion.h" //the service 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #define PI 3.14159265 29 | #define FINGER_LENGTH 0.1 30 | 31 | bool convert_grasp(grasp_detection::GraspConversion::Request &req, 32 | grasp_detection::GraspConversion::Response &res) 33 | { 34 | /* 35 | # request 36 | sensor_msgs/Image depth_image 37 | std_msgs/Int32MultiArray best_grasps_h 38 | std_msgs/Int32MultiArray best_grasps_w 39 | std_msgs/Float32MultiArray best_grasps_angle 40 | sensor_msgs/CameraInfo info 41 | */ 42 | tf::TransformListener listener; 43 | image_geometry::PinholeCameraModel cam_model; 44 | cam_model.fromCameraInfo(req.info); // Build the camera model from CameraInfo, then use it to get cx, cy, fx, fy 45 | cv::Mat depth_img = cv_bridge::toCvCopy(req.depth_image, sensor_msgs::image_encodings::TYPE_32FC1)->image; 46 | GraspPoseConverter grasp_converter(depth_img, 1.0, cam_model.cx(), cam_model.cy(), cam_model.fx(), cam_model.fy()); 47 | int i = 0; 48 | for (std::vector::const_iterator it = req.best_grasps_h.data.begin();it != req.best_grasps_h.data.end(); ++it){ 49 | grasp_converter.convertTo3DGrasp(req.best_grasps_h.data[i], req.best_grasps_w.data[i], req.best_grasps_angle.data[i]); 50 | 51 | //After get the grasp point and rotation vector, and rotation angle we need to convert it to quaternion 52 | float qw = cos((grasp_converter.rotAng)*PI/(2*180)); 53 | float qx = grasp_converter.rotVec[0]*sin((grasp_converter.rotAng)*PI/(2*180)); 54 | float qy = grasp_converter.rotVec[1]*sin((grasp_converter.rotAng)*PI/(2*180)); 55 | float qz = grasp_converter.rotVec[2]*sin((grasp_converter.rotAng)*PI/(2*180)); 56 | //Note these are in the camera frame, so we still need to convert it to the world frame 57 | geometry_msgs::PoseStamped pose_camera; 58 | geometry_msgs::PoseStamped pose_world; 59 | 60 | pose_camera.pose.position.x = grasp_converter.graspPoint3D.x + FINGER_LENGTH * grasp_converter.rotVec[0]; 61 | pose_camera.pose.position.y = grasp_converter.graspPoint3D.y + FINGER_LENGTH * grasp_converter.rotVec[1]; 62 | pose_camera.pose.position.z = grasp_converter.graspPoint3D.z + FINGER_LENGTH * grasp_converter.rotVec[2]; // some distance for the finger tip 63 | pose_camera.pose.orientation.x = qx; 64 | pose_camera.pose.orientation.y = qy; 65 | pose_camera.pose.orientation.z = qz; 66 | pose_camera.pose.orientation.w = qw; 67 | pose_camera.header.stamp = ros::Time(0); 68 | pose_camera.header.frame_id = req.info.header.frame_id; //'/rgbd_camera_optical_frame' 69 | 70 | try{ 71 | listener.waitForTransform("/world", req.info.header.frame_id, ros::Time(0), ros::Duration(1.0)); 72 | listener.transformPose("/world", pose_camera, pose_world); 73 | } 74 | catch (tf::TransformException ex){ 75 | ROS_ERROR("%s",ex.what()); 76 | ros::Duration(1.0).sleep(); 77 | } 78 | res.grasp_poses.push_back(pose_world); 79 | i++; 80 | } 81 | return true; 82 | } 83 | 84 | int main(int argc, char **argv) 85 | { 86 | ros::init(argc, argv, "grasp_conversion_server_node"); 87 | ros::NodeHandle n; 88 | ros::ServiceServer service = n.advertiseService("grasp_conversion", convert_grasp); 89 | ROS_INFO("Ready to convert grasp."); 90 | ros::spin(); 91 | return 0; 92 | } -------------------------------------------------------------------------------- /src/grasp_detection/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyrilli/planar_grasp_detection/6509505b9b85e55c70f54a41411ddc010a005a38/src/grasp_detection/__init__.py -------------------------------------------------------------------------------- /src/grasp_detection/patch_based_grasp_detection/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyrilli/planar_grasp_detection/6509505b9b85e55c70f54a41411ddc010a005a38/src/grasp_detection/patch_based_grasp_detection/__init__.py -------------------------------------------------------------------------------- /src/grasp_detection/patch_based_grasp_detection/graspNet.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | import os 5 | import time 6 | 7 | import tensorflow as tf 8 | 9 | class model: 10 | def __init__(self): 11 | self.BATCH_SIZE = 128 12 | self.THETA_SIZE = 18 13 | def initial_weights(self, weight_file='./models/weights/bvlc_alexnet.npy'): 14 | if weight_file: 15 | # Load what you want the initialisation to be 16 | print('Loading weights from {0}'.format(weight_file)) 17 | net_data = np.load(weight_file).item() 18 | conv1W_init = net_data["conv1"][0] 19 | conv1b_init = net_data["conv1"][1] 20 | conv2W_init = net_data["conv2"][0] 21 | conv2b_init = net_data["conv2"][1] 22 | conv3W_init = net_data["conv3"][0] 23 | conv3b_init = net_data["conv3"][1] 24 | conv4W_init = net_data["conv4"][0] 25 | conv4b_init = net_data["conv4"][1] 26 | conv5W_init = net_data["conv5"][0] 27 | conv5b_init = net_data["conv5"][1] 28 | fc6W_init = tf.truncated_normal([9216, 4096], stddev= 0.01) 29 | fc6b_init = tf.constant(0.1, shape=[4096]) 30 | fc7W_init = tf.truncated_normal([4096, 1024], stddev= 0.01) 31 | fc7b_init = tf.constant(0.1, shape=[1024]) 32 | fc8W_init = tf.truncated_normal([1024,18], stddev= 0.01) 33 | fc8b_init = tf.constant(0.1, shape=[18]) 34 | else: 35 | conv1W_init = tf.truncated_normal([11, 11, 3, 96], stddev= 0.1) 36 | conv1b_init = tf.constant(0.1, shape=[96]) 37 | conv2W_init = tf.truncated_normal([5, 5, 48, 256], stddev= 0.1) 38 | conv2b_init = tf.constant(0.1, shape=[256]) 39 | conv3W_init = tf.truncated_normal([3, 3, 256, 384], stddev= 0.1) 40 | conv3b_init = tf.constant(0.1, shape=[384]) 41 | conv4W_init = tf.truncated_normal([3, 3, 192, 384], stddev= 0.1) 42 | conv4b_init = tf.constant(0.1, shape=[384]) 43 | conv5W_init = tf.truncated_normal([3, 3, 192, 256], stddev= 0.1) 44 | conv5b_init = tf.constant(0.1, shape=[256]) 45 | fc6W_init = tf.truncated_normal([9216, 4096], stddev= 0.1) 46 | fc6b_init = tf.constant(0.1, shape=[4096]) 47 | fc7W_init = tf.truncated_normal([4096, 1024], stddev= 0.1) 48 | fc7b_init = tf.constant(0.1, shape=[1024]) 49 | fc8W_init = tf.truncated_normal([1024,self.THETA_SIZE], stddev= 0.1) 50 | fc8b_init = tf.constant(0.1, shape=[self.THETA_SIZE]) 51 | 52 | self.conv1W = tf.Variable(conv1W_init) 53 | self.conv1b = tf.Variable(conv1b_init) 54 | self.conv2W = tf.Variable(conv2W_init) 55 | self.conv2b = tf.Variable(conv2b_init) 56 | self.conv3W = tf.Variable(conv3W_init) 57 | self.conv3b = tf.Variable(conv3b_init) 58 | self.conv4W = tf.Variable(conv4W_init) 59 | self.conv4b = tf.Variable(conv4b_init) 60 | self.conv5W = tf.Variable(conv5W_init) 61 | self.conv5b = tf.Variable(conv5b_init) 62 | self.fc6W = tf.Variable(fc6W_init) 63 | self.fc6b = tf.Variable(fc6b_init) 64 | self.fc7W = tf.Variable(fc7W_init) 65 | self.fc7b = tf.Variable(fc7b_init) 66 | self.fc8b = tf.Variable(fc8b_init) 67 | self.fc8W = tf.Variable(fc8W_init) 68 | self.dropfc6 = tf.placeholder(tf.float32, name="dropoutfc6_keep_prob") 69 | self.dropfc7 = tf.placeholder(tf.float32, name="dropoutfc7_keep_prob") 70 | 71 | def gen_model(self, image_batch): 72 | #conv1 73 | #conv(11, 11, 96, 4, 4, padding='VALID', name='conv1') 74 | k_h = 11; k_w = 11; c_o = 96; s_h = 4; s_w = 4 75 | conv1_in = conv(image_batch, self.conv1W, self.conv1b, k_h, k_w, c_o, s_h, s_w, padding="SAME", group=1) 76 | self.conv1_in = conv1_in 77 | conv1 = tf.nn.relu(conv1_in) 78 | #lrn1 79 | #lrn(2, 2e-05, 0.75, name='norm1') 80 | radius = 2; alpha = 2e-05; beta = 0.75; bias = 1.0 81 | lrn1 = tf.nn.local_response_normalization(conv1, depth_radius=radius, alpha=alpha, beta=beta, bias=bias) 82 | #maxpool1 83 | #max_pool(3, 3, 2, 2, padding='VALID', name='pool1') 84 | k_h = 3; k_w = 3; s_h = 2; s_w = 2; padding = 'VALID' 85 | maxpool1 = tf.nn.max_pool(lrn1, ksize=[1, k_h, k_w, 1], strides=[1, s_h, s_w, 1], padding=padding) 86 | #conv2 87 | #conv(5, 5, 256, 1, 1, group=2, name='conv2') 88 | k_h = 5; k_w = 5; c_o = 256; s_h = 1; s_w = 1; group = 2 89 | conv2_in = conv(maxpool1, self.conv2W, self.conv2b, k_h, k_w, c_o, s_h, s_w, padding="SAME", group=group) 90 | conv2 = tf.nn.relu(conv2_in) 91 | #lrn2 92 | #lrn(2, 2e-05, 0.75, name='norm2') 93 | radius = 2; alpha = 2e-05; beta = 0.75; bias = 1.0 94 | lrn2 = tf.nn.local_response_normalization(conv2, depth_radius=radius, alpha=alpha, beta=beta, bias=bias) 95 | #maxpool2 96 | #max_pool(3, 3, 2, 2, padding='VALID', name='pool2') 97 | k_h = 3; k_w = 3; s_h = 2; s_w = 2; padding = 'VALID' 98 | maxpool2 = tf.nn.max_pool(lrn2, ksize=[1, k_h, k_w, 1], strides=[1, s_h, s_w, 1], padding=padding) 99 | #conv3 100 | #conv(3, 3, 384, 1, 1, name='conv3') 101 | k_h = 3; k_w = 3; c_o = 384; s_h = 1; s_w = 1; group = 1 102 | conv3_in = conv(maxpool2, self.conv3W, self.conv3b, k_h, k_w, c_o, s_h, s_w, padding="SAME", group=group) 103 | conv3 = tf.nn.relu(conv3_in) 104 | #conv4 105 | #conv(3, 3, 384, 1, 1, group=2, name='conv4') 106 | k_h = 3; k_w = 3; c_o = 384; s_h = 1; s_w = 1; group = 2 107 | conv4_in = conv(conv3, self.conv4W, self.conv4b, k_h, k_w, c_o, s_h, s_w, padding="SAME", group=group) 108 | conv4 = tf.nn.relu(conv4_in) 109 | #conv5 110 | #conv(3, 3, 256, 1, 1, group=2, name='conv5') 111 | k_h = 3; k_w = 3; c_o = 256; s_h = 1; s_w = 1; group = 2 112 | conv5_in = conv(conv4, self.conv5W, self.conv5b, k_h, k_w, c_o, s_h, s_w, padding="SAME", group=group) 113 | conv5 = tf.nn.relu(conv5_in) 114 | #maxpool5 115 | #max_pool(3, 3, 2, 2, padding='VALID', name='pool5') 116 | k_h = 3; k_w = 3; s_h = 2; s_w = 2; padding = 'VALID' 117 | maxpool5 = tf.nn.max_pool(conv5, ksize=[1, k_h, k_w, 1], strides=[1, s_h, s_w, 1], padding=padding) 118 | #fc6 119 | #fc(4096, name='fc6') 120 | fc6 = tf.nn.relu_layer(tf.reshape(maxpool5, [-1, int(np.prod(maxpool5.get_shape()[1:]))]), 121 | self.fc6W, self.fc6b) 122 | #dropout 123 | drop6 = tf.nn.dropout(fc6,self.dropfc6) 124 | #fc7 125 | #fc(4096, name='fc7') 126 | fc7 = tf.nn.relu_layer(drop6, self.fc7W, self.fc7b) 127 | #dropout 128 | drop7 = tf.nn.dropout(fc7,self.dropfc7) 129 | #fc8 130 | #fc(1000, relu=False, name='fc8') 131 | fc8 = tf.nn.xw_plus_b(drop7, self.fc8W, self.fc8b) 132 | #Debug stuff 133 | self.fc7 = fc7 134 | self.fc6 = fc6 135 | self.conv5 = conv5 136 | #End Debug 137 | return fc8 138 | 139 | def gen_loss(self, fc8, theta_label_batch, grasp_label_batch): 140 | fc8_shape = tf.shape(fc8) 141 | input_batch_size = tf.gather(fc8_shape, 0) # computiong batch size as an op from the output 142 | theta_one_hot = tf.one_hot(theta_label_batch, depth=18, on_value=1.0, off_value=0.0) 143 | theta_acted = tf.reduce_sum(fc8 * theta_one_hot, reduction_indices=1, name='theta_acted') 144 | sig_op = tf.clip_by_value(tf.nn.sigmoid( theta_acted), 0.001, 0.999, name='clipped_sigmoid') 145 | sig_loss = tf.to_float(grasp_label_batch) * -tf.log(sig_op) + (1 - 146 | tf.to_float(grasp_label_batch)) * -tf.log(1 - sig_op) 147 | self.sig_op = sig_op 148 | self.sig_loss = sig_loss 149 | loss = tf.reduce_mean(sig_loss) 150 | #loss = tf.clip_by_value(unclipped_loss, -2, 2, name='clipped_loss') 151 | #sig_loss = tf.reduce_mean(tf.square(sig_op), name='sig_loss') 152 | conf = tf.equal(tf.to_int32(tf.greater_equal(sig_op,0.5)),tf.to_int32(tf.greater_equal(grasp_label_batch,0.1)))# the plus 0.5 is there because of the alpha added on because of shake 153 | self.conf = conf 154 | accuracy = tf.reduce_mean(tf.to_float(conf)) 155 | return loss, accuracy 156 | 157 | def conv(input, kernel, biases, k_h, k_w, c_o, s_h, s_w, padding="VALID", group=1): 158 | c_i = input.get_shape()[-1] 159 | assert c_i%group==0 160 | assert c_o%group==0 161 | convolve = lambda i, k: tf.nn.conv2d(i, k, [1, s_h, s_w, 1], padding=padding) 162 | if group==1: 163 | conv = convolve(input, kernel) 164 | else: 165 | #input_groups = tf.split(3, group, input) 166 | input_groups = tf.split(input, group, 3) 167 | #kernel_groups = tf.split(3, group, kernel) 168 | kernel_groups = tf.split(kernel, group, 3) 169 | output_groups = [convolve(i, k) for i,k in zip(input_groups, kernel_groups)] 170 | conv = tf.concat(output_groups, 3) 171 | return tf.reshape(tf.nn.bias_add(conv, biases), [-1]+conv.get_shape().as_list()[1:]) 172 | -------------------------------------------------------------------------------- /src/grasp_detection/patch_based_grasp_detection/grasp_learner.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from __future__ import absolute_import 3 | from __future__ import division 4 | from __future__ import print_function 5 | 6 | import roslib 7 | roslib.load_manifest('grasp_detection') 8 | import numpy as np 9 | import tensorflow as tf 10 | from grasp_detection.patch_based_grasp_detection.graspNet import model as grasp_net 11 | 12 | class grasp_obj: 13 | def __init__(self, checkpoint_path='./models/shake/checkpoint.ckpt-2000', gpu_id=-1): 14 | self.checkpoint = checkpoint_path 15 | if gpu_id==-1: 16 | self.dev_name = "/cpu:0" 17 | else: 18 | self.dev_name = "/gpu:{}".format(gpu_id) 19 | 20 | self.IMAGE_SIZE = 224 21 | self.NUM_CHANNELS = 3 22 | self.GRASP_ACTION_SIZE = 18 23 | self.SEED = 66478 # Set to None for random seed. 24 | self.BATCH_SIZE = 128 25 | 26 | #CONFIG PARAMS 27 | self.INTRA_OP_THREADS = 1 28 | self.INTER_OP_THREADS = 1 29 | self.SOFT_PLACEMENT = False 30 | 31 | tf.set_random_seed(self.SEED) 32 | 33 | self.config = tf.ConfigProto(allow_soft_placement=self.SOFT_PLACEMENT, 34 | intra_op_parallelism_threads=self.INTRA_OP_THREADS, 35 | inter_op_parallelism_threads=self.INTER_OP_THREADS) 36 | self.config.gpu_options.allow_growth = True 37 | 38 | def sigmoid_array(self,x): 39 | return 1 / (1 + np.exp(-x)) 40 | 41 | def test_init(self): 42 | with tf.device(self.dev_name): 43 | with tf.name_scope('Grasp_training_data'): 44 | self.Grasp_patches = tf.placeholder(tf.float32, shape=[self.BATCH_SIZE,self.IMAGE_SIZE,self.IMAGE_SIZE,self.NUM_CHANNELS]) 45 | with tf.name_scope('Grasp'): 46 | self.M = grasp_net() 47 | self.M.initial_weights(weight_file=None) 48 | self.grasp_pred = self.M.gen_model(self.Grasp_patches) 49 | with tf.device("/cpu:0"): 50 | grasp_variables = tf.get_collection(tf.GraphKeys.GLOBAL_VARIABLES, scope='Grasp') 51 | grasp_saver = tf.train.Saver(grasp_variables, max_to_keep=100) 52 | with tf.device(self.dev_name): 53 | self.sess = tf.Session(config = self.config) 54 | grasp_saver.restore(self.sess, self.checkpoint) 55 | 56 | def test_one_batch(self,Is): 57 | with tf.device(self.dev_name): 58 | grasp_feed_dict = {self.Grasp_patches : Is, self.M.dropfc6 : 1.0, self.M.dropfc7 : 1.0} 59 | g_pred = self.sess.run(self.grasp_pred, feed_dict=grasp_feed_dict) 60 | return g_pred 61 | 62 | def test_close(self): 63 | self.sess.close() 64 | -------------------------------------------------------------------------------- /src/grasp_detection/patch_based_grasp_detection/grasp_predictor.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | import cv2 5 | import copy 6 | from sys import stdout 7 | # Given image, returns image point and theta to grasp 8 | class Predictors: 9 | def __init__(self, I, learner=None): 10 | self.I = I 11 | self.I_h, self.I_w, self.I_c = self.I.shape 12 | self.learner = learner 13 | def sigmoid_array(self, x): 14 | return 1 / (1 + np.exp(-x)) 15 | def to_prob_map(self, fc8_vals): 16 | sig_scale = 1 17 | no_keep = 20 18 | fc8_sig = self.sigmoid_array(sig_scale*fc8_vals) 19 | r = np.sort(fc8_sig, axis = None) 20 | r_no_keep = r[-no_keep] 21 | fc8_sig[fc8_sig 15: 60 | isWhiteFlag = 0 61 | else: 62 | isWhiteFlag = 1 63 | patch_Is_resized[looper] = cv2.resize(patch_Is[looper],(self.learner.IMAGE_SIZE,self.learner.IMAGE_SIZE), interpolation=cv2.INTER_CUBIC) 64 | #subtract mean 65 | patch_Is_resized = patch_Is_resized - 111 66 | self.fc8_vals = self.learner.test_one_batch(patch_Is_resized) 67 | # Normalizing angle uncertainity 68 | wf = [0.25, 0.5, 0.25] 69 | self.fc8_norm_vals = copy.deepcopy(self.fc8_vals) 70 | for looper in xrange(num_samples): 71 | for norm_looper in xrange(num_angle): 72 | self.fc8_norm_vals[looper,norm_looper] = (wf[1]*self.fc8_norm_vals[looper,norm_looper] + 73 | wf[0]*self.fc8_norm_vals[looper,(norm_looper-1)%num_angle] + 74 | wf[2]*self.fc8_norm_vals[looper,(norm_looper+1)%num_angle]) 75 | # Normalize to probability distribution 76 | self.fc8_prob_vals = self.to_prob_map(self.fc8_norm_vals) 77 | # Sample from probability distribution 78 | self.patch_id, self.theta_id = self.sample_from_map(self.fc8_prob_vals) 79 | #self.patch_id, self.theta_id = np.unravel_index(self.fc8_prob_vals.argmax(), self.fc8_prob_vals.shape) 80 | self.patch_hs = patch_hs 81 | self.patch_ws = patch_ws 82 | self.best_patch = patch_Is[self.patch_id] 83 | self.best_patch_h = patch_hs[self.patch_id] 84 | self.best_patch_w = patch_ws[self.patch_id] 85 | self.best_patch_resized = patch_Is_resized[self.patch_id] 86 | self.patch_Is_resized = patch_Is_resized 87 | self.patch_Is = patch_Is 88 | return self.best_patch_h, self.best_patch_w, self.theta_id 89 | -------------------------------------------------------------------------------- /srv/GraspConversion.srv: -------------------------------------------------------------------------------- 1 | # request 2 | sensor_msgs/Image depth_image 3 | std_msgs/Int32MultiArray best_grasps_h 4 | std_msgs/Int32MultiArray best_grasps_w 5 | std_msgs/Float32MultiArray best_grasps_angle 6 | sensor_msgs/CameraInfo info 7 | --- 8 | # response 9 | # the list of planned grasps 10 | geometry_msgs/PoseStamped[] grasp_poses -------------------------------------------------------------------------------- /srv/GraspDetection.srv: -------------------------------------------------------------------------------- 1 | # request 2 | sensor_msgs/Image color_image 3 | std_msgs/Float32 gscale # ratio of the patch size with respect to the image size 4 | --- 5 | # response 6 | std_msgs/Int32MultiArray best_grasps_h 7 | std_msgs/Int32MultiArray best_grasps_w 8 | std_msgs/Float32MultiArray best_grasps_angle 9 | #sensor_msgs/Image bbox_image --------------------------------------------------------------------------------