├── __init__.py ├── models └── .temp ├── .gitignore ├── approach.jpg ├── few_images ├── 1.jpg ├── 2.jpg ├── 3.jpg ├── 4.jpg ├── 5.jpg ├── 6.jpg ├── 7.jpg ├── 8.jpg └── 9.jpg ├── README.md ├── grasp_learner.py ├── grasp_predictor.py ├── grasp_image.py └── graspNet.py /__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /models/.temp: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.py[cdo] 2 | /models/Grasp_model 3 | -------------------------------------------------------------------------------- /approach.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lerrel/Grasp-Detector/HEAD/approach.jpg -------------------------------------------------------------------------------- /few_images/1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lerrel/Grasp-Detector/HEAD/few_images/1.jpg -------------------------------------------------------------------------------- /few_images/2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lerrel/Grasp-Detector/HEAD/few_images/2.jpg -------------------------------------------------------------------------------- /few_images/3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lerrel/Grasp-Detector/HEAD/few_images/3.jpg -------------------------------------------------------------------------------- /few_images/4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lerrel/Grasp-Detector/HEAD/few_images/4.jpg -------------------------------------------------------------------------------- /few_images/5.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lerrel/Grasp-Detector/HEAD/few_images/5.jpg -------------------------------------------------------------------------------- /few_images/6.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lerrel/Grasp-Detector/HEAD/few_images/6.jpg -------------------------------------------------------------------------------- /few_images/7.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lerrel/Grasp-Detector/HEAD/few_images/7.jpg -------------------------------------------------------------------------------- /few_images/8.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lerrel/Grasp-Detector/HEAD/few_images/8.jpg -------------------------------------------------------------------------------- /few_images/9.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lerrel/Grasp-Detector/HEAD/few_images/9.jpg -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Grasp Detector 2 | 3 | Code to detect planar grasps using the model learnt in 4 | https://arxiv.org/pdf/1610.01685v1.pdf 5 | 6 | ![alt tag](http://www.cs.cmu.edu/~lerrelp/all_grasp_img.png) 7 | 8 | ## Getting Started 9 | 10 | This should help you run a learnt grasp detector model on a sample image. Running this code requires the following dependencies: 11 | 12 | * Python 2.7 13 | * TensorFlow (version 1.0) 14 | ``` 15 | # Instructions to install TensorFlow 1.0 16 | # Option 1 17 | Install TensorFlow with GPU support from https://www.tensorflow.org/install/install_linux 18 | 19 | # Option 2 20 | # Check available tensorflow wheel files 21 | curl -s https://storage.googleapis.com/tensorflow |xmllint --format - |grep whl | grep gpu 22 | # Install the version that works for your computer 23 | pip install https://storage.googleapis.com/tensorflow/ 24 | ``` 25 | * argparse ('pip install argparse') 26 | * cv2 ('conda install -c menpo opencv=2.4.11' or install opencv from source) 27 | * numpy ('pip install numpy' or 'conda install numpy') 28 | 29 | ## Getting Grasp Models 30 | 31 | Download the learnt grasp models from https://www.dropbox.com/s/85b483emhubr7l4/Grasp_model?dl=0 and move it to the folder models. 32 | 33 | ``` 34 | # From the repository 35 | wget https://www.dropbox.com/s/85b483emhubr7l4/Grasp_model 36 | mv Grasp_model ./models/. 37 | ``` 38 | 39 | ## Example 40 | 41 | Run grasp detector that should run the model on the image by sampling patches and displaying the best grasp on the image. Press any key to exit. 42 | 43 | ``` 44 | # For CPU 45 | python grasp_image.py --im ./approach.jpg --model ./models/Grasp_model --nbest 5 --nsamples 250 --gscale 0.234 --gpu -1 46 | 47 | # For GPU 48 | python grasp_image.py --im ./approach.jpg --model ./models/Grasp_model --nbest 5 --nsamples 1000 --gscale 0.234 --gpu 0 49 | ``` 50 | 51 | ## Contact 52 | Lerrel Pinto -- lerrelpATcsDOTcmuDOTedu. 53 | -------------------------------------------------------------------------------- /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 numpy as np 7 | import tensorflow as tf 8 | 9 | from graspNet import model as grasp_net 10 | 11 | class grasp_obj: 12 | def __init__(self, checkpoint_path='./models/shake/checkpoint.ckpt-2000', gpu_id=-1): 13 | self.checkpoint = checkpoint_path 14 | if gpu_id==-1: 15 | self.dev_name = "/cpu:0" 16 | else: 17 | self.dev_name = "/gpu:{}".format(gpu_id) 18 | 19 | self.IMAGE_SIZE = 224 20 | self.NUM_CHANNELS = 3 21 | self.GRASP_ACTION_SIZE = 18 22 | self.SEED = 66478 # Set to None for random seed. 23 | self.BATCH_SIZE = 128 24 | 25 | #CONFIG PARAMS 26 | self.INTRA_OP_THREADS = 1 27 | self.INTER_OP_THREADS = 1 28 | self.SOFT_PLACEMENT = False 29 | 30 | tf.set_random_seed(self.SEED) 31 | 32 | self.config = tf.ConfigProto(allow_soft_placement=self.SOFT_PLACEMENT, 33 | intra_op_parallelism_threads=self.INTRA_OP_THREADS, 34 | inter_op_parallelism_threads=self.INTER_OP_THREADS) 35 | 36 | def sigmoid_array(self,x): 37 | return 1 / (1 + np.exp(-x)) 38 | 39 | def test_init(self): 40 | with tf.device(self.dev_name): 41 | with tf.name_scope('Grasp_training_data'): 42 | self.Grasp_patches = tf.placeholder(tf.float32, shape=[self.BATCH_SIZE,self.IMAGE_SIZE,self.IMAGE_SIZE,self.NUM_CHANNELS]) 43 | with tf.name_scope('Grasp'): 44 | self.M = grasp_net() 45 | self.M.initial_weights(weight_file=None) 46 | self.grasp_pred = self.M.gen_model(self.Grasp_patches) 47 | with tf.device("/cpu:0"): 48 | grasp_variables = tf.get_collection(tf.GraphKeys.GLOBAL_VARIABLES, scope='Grasp') 49 | grasp_saver = tf.train.Saver(grasp_variables, max_to_keep=100) 50 | with tf.device(self.dev_name): 51 | self.sess = tf.Session(config = self.config) 52 | grasp_saver.restore(self.sess, self.checkpoint) 53 | 54 | def test_one_batch(self,Is): 55 | with tf.device(self.dev_name): 56 | grasp_feed_dict = {self.Grasp_patches : Is, self.M.dropfc6 : 1.0, self.M.dropfc7 : 1.0} 57 | g_pred = self.sess.run(self.grasp_pred, feed_dict=grasp_feed_dict) 58 | return g_pred 59 | 60 | def test_close(self): 61 | self.sess.close() 62 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /grasp_image.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Code to run grasp detection given an image using the model learnt in 3 | https://arxiv.org/pdf/1610.01685v1.pdf 4 | 5 | Dependencies: Python 2.7 6 | argparse ('pip install argparse') 7 | cv2 ('conda install -c menpo opencv=2.4.11' or install opencv from source) 8 | numpy ('pip install numpy' or 'conda install numpy') 9 | tensorflow (version 0.9) 10 | 11 | Template run: 12 | python grasp_image.py --im {'Image path'} --model {'Model path'} --nbest {1,2,...} --nsamples {1,2,...} --gscale {0, ..., 1.0} --gpu {-1,0,1,...} 13 | Example run: 14 | python grasp_image.py --im ./approach.jpg --model ./models/Grasp_model --nbest 100 --nsamples 250 --gscale 0.1 --gpu 0 15 | ''' 16 | import argparse 17 | import cv2 18 | import numpy as np 19 | from grasp_learner import grasp_obj 20 | from grasp_predictor import Predictors 21 | import time 22 | 23 | def drawRectangle(I, h, w, t, gsize=300): 24 | I_temp = I 25 | grasp_l = gsize/2.5 26 | grasp_w = gsize/5.0 27 | grasp_angle = t*(np.pi/18)-np.pi/2 28 | 29 | points = np.array([[-grasp_l, -grasp_w], 30 | [grasp_l, -grasp_w], 31 | [grasp_l, grasp_w], 32 | [-grasp_l, grasp_w]]) 33 | R = np.array([[np.cos(grasp_angle), -np.sin(grasp_angle)], 34 | [np.sin(grasp_angle), np.cos(grasp_angle)]]) 35 | rot_points = np.dot(R, points.transpose()).transpose() 36 | im_points = rot_points + np.array([w,h]) 37 | cv2.line(I_temp, tuple(im_points[0].astype(int)), tuple(im_points[1].astype(int)), color=(0,255,0), thickness=5) 38 | cv2.line(I_temp, tuple(im_points[1].astype(int)), tuple(im_points[2].astype(int)), color=(0,0,255), thickness=5) 39 | cv2.line(I_temp, tuple(im_points[2].astype(int)), tuple(im_points[3].astype(int)), color=(0,255,0), thickness=5) 40 | cv2.line(I_temp, tuple(im_points[3].astype(int)), tuple(im_points[0].astype(int)), color=(0,0,255), thickness=5) 41 | return I_temp 42 | 43 | parser = argparse.ArgumentParser() 44 | parser.add_argument('--im', type=str, default='./approach.jpg', help='The Image you want to detect grasp on') 45 | parser.add_argument('--model', type=str, default='./models/Grasp_model', help='Grasp model you want to use') 46 | parser.add_argument('--nsamples', type=int, default=128, help='Number of patch samples. More the better, but it\'ll get slower') 47 | parser.add_argument('--nbest', type=int, default=10, help='Number of grasps to display') 48 | parser.add_argument('--gscale', type=float, default=0.234375, help='Scale of grasp. Default is the one used in the paper, given a 720X1280 res image') 49 | parser.add_argument('--gpu', type=int, default=0, help='GPU device id; -1 for cpu') 50 | 51 | ## Parse arguments 52 | args = parser.parse_args() 53 | I = cv2.imread(args.im) 54 | model_path = args.model 55 | nsamples = args.nsamples 56 | nbest = args.nbest 57 | gscale = args.gscale 58 | imsize = max(I.shape[:2]) 59 | gsize = int(gscale*imsize) # Size of grasp patch 60 | max_batchsize = 128 61 | gpu_id = args.gpu 62 | 63 | ## Set up model 64 | if nsamples > max_batchsize: 65 | batchsize = max_batchsize 66 | nbatches = int(nsamples/max_batchsize) + 1 67 | else: 68 | batchsize = nsamples 69 | nbatches = 1 70 | 71 | print('Loading grasp model') 72 | st_time = time.time() 73 | G = grasp_obj(model_path, gpu_id) 74 | G.BATCH_SIZE = batchsize 75 | G.test_init() 76 | P = Predictors(I, G) 77 | print('Time taken: {}s'.format(time.time()-st_time)) 78 | 79 | fc8_predictions=[] 80 | patch_Hs = [] 81 | patch_Ws = [] 82 | 83 | print('Predicting on samples') 84 | st_time = time.time() 85 | for _ in range(nbatches): 86 | P.graspNet_grasp(patch_size=gsize, num_samples=batchsize); 87 | fc8_predictions.append(P.fc8_norm_vals) 88 | patch_Hs.append(P.patch_hs) 89 | patch_Ws.append(P.patch_ws) 90 | 91 | fc8_predictions = np.concatenate(fc8_predictions) 92 | patch_Hs = np.concatenate(patch_Hs) 93 | patch_Ws = np.concatenate(patch_Ws) 94 | 95 | r = np.sort(fc8_predictions, axis = None) 96 | r_no_keep = r[-nbest] 97 | print('Time taken: {}s'.format(time.time()-st_time)) 98 | 99 | for pindex in range(fc8_predictions.shape[0]): 100 | for tindex in range(fc8_predictions.shape[1]): 101 | if fc8_predictions[pindex, tindex] < r_no_keep: 102 | continue 103 | else: 104 | I = drawRectangle(I, patch_Hs[pindex], patch_Ws[pindex], tindex, gsize) 105 | 106 | print('displaying image') 107 | cv2.imshow('image',I) 108 | cv2.waitKey(0) 109 | 110 | cv2.destroyAllWindows() 111 | G.test_close() 112 | -------------------------------------------------------------------------------- /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 | --------------------------------------------------------------------------------