def create_data(): grasps = grasp_proxy.detect_grasps_whole_cloud(cloud, grasp_offsets) grasps = filter( lambda g: (grasp_space[0][0] < g.bottom[0] < grasp_space[0][ 1] and grasp_space[1][0] < g.bottom[1] < grasp_space[1][1] and grasp_space[2][0] < g.bottom[2] < grasp_space[2][1]), grasps) print 'grasps after filter: ' + str(len(grasps)) rviz_node.cloud_pub.publish(cloud_proxy.convert_to_point_cloud2(cloud)) rviz_node.all_grasps_pub.publish( plot.createGraspsMarkerArray(grasps, rgba=[1, 0, 0, 0.5])) rviz_node.grasp_space_pub.publish(plot.createGraspsPosCube(grasp_space)) image_3d = [] image_2d = [] for grasp in grasps: hand_des_3d = HandDescriptor( HandDescriptor.T_from_approach_axis_center( grasp.approach, grasp.axis, (grasp.bottom + grasp.top) / 2), 32) hand_des_3d.generate_3d_binary_image(cloud) image_3d.append(hand_des_3d.image) hand_des_2d = HandDescriptor( HandDescriptor.T_from_approach_axis_center( grasp.approach, grasp.axis, (grasp.bottom + grasp.top) / 2)) hand_des_2d.generate_depth_image(cloud) image_2d.append(hand_des_2d.image) image_3d = np.stack(image_3d, 0) image_2d = np.stack(image_2d, 0) f_3d = os.path.dirname(__file__) + '/../data/normal_shelf1_3d.npy' f_2d = os.path.dirname(__file__) + '/../data/normal_shelf1_2d.npy' try: old_3d = np.load(f_3d) if old_3d.shape[0] != 0: image_3d = np.vstack([image_3d, old_3d]) except: pass try: old_2d = np.load(f_2d) if old_2d.shape[0] != 0: image_2d = np.vstack([image_2d, old_2d]) except: pass finally: raw_input('write?') np.save(f_3d, image_3d) print 'current 3d data: ' + str(image_3d.shape[0]) np.save(f_2d, image_2d) print 'current 2d data: ' + str(image_2d.shape[0])
def SenseAndActMultiple(self, hand, prevDescs, prevProbs, cloud, tableHeight, t): '''TODO''' prevDesc = self.initDesc # generate input image targImage = prevDesc.GenerateHeightmap(cloud, tableHeight) handImage = zeros((self.imP, self.imP, 0)) if hand is None else hand.image timeImage = zeros((self.imP, self.imP, 0)) if not self.includeTime else \ float(self.tMax - t) / self.tMax * ones((self.imP, self.imP, 1)) o = concatenate((targImage, handImage, timeImage), axis = 2) # evaluate action-values and sample n of them values = self.EvaluateActions(o).flatten() flatActionIdxs = argsort(-values)[:len(prevDescs)] values = values[flatActionIdxs] # convert values into probabilities (of separate Bernoulli random variables) values[values > self.qMax] = self.qMax values[values < self.qMin] = self.qMin probabilities = (values - self.qMin) / (self.qMax - self.qMin) # create a descriptor for each sampled action descriptors = [] for flatIdx in flatActionIdxs: idx = unravel_index(flatIdx, self.outputShape) T = copy(prevDesc.T) T[0:3, 3] = self.actionsInHandFrame[idx] + prevDesc.center descriptors.append(HandDescriptor(T, self.imP, self.imDNext, self.imWNext)) return descriptors, probabilities
def SenseAndAct(self, hand, prevDesc, t, rlEnv, unbias): '''TODO''' handImage = zeros((self.imP, self.imP, 0), dtype='float32') if hand is None else hand.image # generate candidate descriptors descs = [] for i in xrange(self.actionSpaceSize[0]): for j in xrange(self.actionSpaceSize[1]): for k in xrange(self.actionSpaceSize[2]): T = copy(prevDesc.T) T[0:3, 3] = self.actionsInHandFrame[(i, j, k)] + prevDesc.center descs.append( HandDescriptor(T, self.imP, self.imD, self.imW)) # decide which location in the image to zoom into bestIdx, bestValue, epsilon = self.SelectIndexEpsilonGreedy( descs, handImage, unbias, rlEnv) # compose result desc = descs[bestIdx] o = concatenate((desc.image, handImage), axis=2) return o, desc, bestValue, epsilon
def SetInitialDescriptor(self, offset): '''Sets the center of the initial descriptor to the provided center.''' T = eye(4) T[0:3, 3] = array([offset[0], offset[1], offset[2] + self.imD / 2.0]) self.initDesc = HandDescriptor(T, self.imP, self.imD, self.imW) self.initDesc.imW = self.imW self.initDesc.imD = self.imD
def SetInitialDescriptor(self, center): '''Sets the center of the initial descriptor to the provided center.''' approach = array([0,0,-1]); axis = array([1,0,0]) bTh = hand_descriptor.PoseFromApproachAxisCenter(approach, axis, center) self.initDesc = HandDescriptor(bTh) self.initDesc.imD = 4*self.initDesc.imD self.initDesc.imH = 4*self.initDesc.imH self.initDesc.imW = 4*self.initDesc.imW
def ComposeAction(self, prevDesc, baseCoord, imageCoord): '''Creates a new action and hand descriptor objects.''' action = [prevDesc.image, copy(imageCoord)] T = copy(prevDesc.T) T[0:3, 3] = baseCoord desc = HandDescriptor(T) return action, desc
def UnpackGrasps(self, mGrasps): '''Extracts the list of grasps in Matlab format and returns a list in Python format.''' grasps = [] for mGrasp in mGrasps: top = array(mGrasp["top"]).flatten() bottom = array(mGrasp["bottom"]).flatten() axis = array(mGrasp["axis"]).flatten() approach = array(mGrasp["approach"]).flatten() score = mGrasp["score"] # create grasp object T = hand_descriptor.PoseFromApproachAxisCenter( approach, axis, 0.5 * bottom + 0.5 * top) grasp = HandDescriptor(T) grasp.score = score grasps.append(grasp) return grasps
def ComposeAction(self, prevDesc, theta): '''Creates a new action and hand descriptor objects.''' action = [prevDesc.image, copy(theta)] R = openravepy.matrixFromAxisAngle(prevDesc.approach, theta[0])[0:3,0:3] approach = prevDesc.approach axis = dot(R, prevDesc.axis) center = prevDesc.center T = hand_descriptor.PoseFromApproachAxisCenter(approach, axis, center) desc = HandDescriptor(T) return action, desc
def SenseAndAct(self, hand, prevDesc, t, rlEnv, unbias): '''TODO''' # generate input image targImage = prevDesc.GenerateHeightmap(rlEnv) handImage = zeros((self.imP, self.imP, 0), dtype='float32') if hand is None else hand.image o = concatenate((targImage, handImage), axis=2) # decide which location in the image to zoom into bestIdx, bestValue, epsilon = self.SelectIndexEpsilonGreedy(o, unbias) # compose result T = copy(prevDesc.T) T[0:3, 3] = self.actionsInHandFrame[bestIdx] + prevDesc.center desc = HandDescriptor(T, self.imP, self.imDNext, self.imWNext) a = bestIdx return o, a, desc, bestValue, epsilon
def InitializeHandRegions(self): '''Determines hand geometry, in the descriptor reference frame, for collision checking. Should be called once at initialization. ''' # find default descriptor geometry desc = HandDescriptor(eye(4), self.params["imP"], self.params["imD"], self.params["imW"]) # important reference points topUp = desc.top + (desc.height / 2) * desc.axis topDn = desc.top - (desc.height / 2) * desc.axis BtmUp = desc.top + (desc.height / 2) * desc.axis BtmDn = desc.top - (desc.height / 2) * desc.axis # cuboids representing hand regions, in workspace format self.handClosingRegion = [(-desc.height / 2, desc.height / 2), (-desc.width / 2, desc.width / 2), (-desc.depth / 2, desc.depth / 2)] self.handFingerRegionL = [(-desc.height / 2, desc.height / 2), (-desc.width / 2 - 0.01, -desc.width / 2), (-desc.depth / 2, desc.depth / 2)] self.handFingerRegionR = [(-desc.height / 2, desc.height / 2), (desc.width / 2, desc.width / 2 + 0.01), (-desc.depth / 2, desc.depth / 2)] self.handTopRegion = [(-desc.height / 2, desc.height / 2), (-desc.width / 2 - 0.01, desc.width / 2 + 0.01), (desc.depth / 2, desc.depth / 2 + 0.01)] # find corners of hand collision geometry self.externalHandPoints = array([ \ topUp + ((desc.width / 2) + 0.01) * desc.binormal, topUp - ((desc.width / 2) + 0.01) * desc.binormal, topDn + ((desc.width / 2) + 0.01) * desc.binormal, topDn - ((desc.width / 2) + 0.01) * desc.binormal, BtmUp + ((desc.width / 2) + 0.01) * desc.binormal, BtmUp - ((desc.width / 2) + 0.01) * desc.binormal, BtmDn + ((desc.width / 2) + 0.01) * desc.binormal, BtmDn - ((desc.width / 2) + 0.01) * desc.binormal, ])
def SenseAndAct(self, hand, prevDesc, cloud, tableHeight, t, unbias): '''TODO''' # generate input image targImage = prevDesc.GenerateHeightmap(cloud, tableHeight) handImage = zeros((self.imP, self.imP, 0), dtype="float32") if hand is None else hand.image timeImage = zeros((self.imP, self.imP, 0), dtype="float32") if not self.includeTime else \ float(self.tMax - t) / self.tMax * ones((self.imP, self.imP, 1), dtype="float32") o = concatenate((targImage, handImage, timeImage), axis = 2) # decide which location in the image to zoom into bestIdx, bestValue, epsilon = self.SelectIndexEpsilonGreedy(o, unbias) # compose result T = copy(prevDesc.T) T[0:3, 3] = self.actionsInHandFrame[bestIdx] + prevDesc.center desc = HandDescriptor(T, self.imP, self.imDNext, self.imWNext) a = bestIdx return o, a, desc, bestValue, epsilon
def SenseAndActMultiple(self, hand, prevDescs, prevProbs, cloud, tableHeight, t): '''TODO''' # generate input image observations = [] for prevDesc in prevDescs: targImage = prevDesc.GenerateHeightmap(cloud, tableHeight) handImage = zeros( (self.imP, self.imP, 0)) if hand is None else hand.image timeImage = zeros((self.imP, self.imP, 0)) if not self.includeTime else \ float(self.tMax - t) / self.tMax * ones((self.imP, self.imP, 1)) o = concatenate((targImage, handImage, timeImage), axis=2) observations.append(o) # evaluate action-values and sample n of them valuesBatch = self.EvaluateActionsMultiple(observations).flatten() flatActionIdxs = argsort(-valuesBatch)[:len(prevDescs)] values = valuesBatch[flatActionIdxs] # convert values into probabilities (of separate Bernoulli random variables) values[values > self.qMax] = self.qMax values[values < self.qMin] = self.qMin probabilities = (values - self.qMin) / (self.qMax - self.qMin) # create a descriptor for each sampled action descriptors = [] for i, flatIdx in enumerate(flatActionIdxs): idx = unravel_index(flatIdx, (len(prevDescs), ) + self.outputShape) T = copy(prevDescs[idx[0]].T) T[0:3, 3] += self.actionsInHandFrame[idx[1:]] descriptors.append( HandDescriptor(T, self.imP, self.imDNext, self.imWNext)) probabilities[i] *= prevProbs[idx[0]] if self.plotImages and i == 0: print("Probability: {}.".format(probabilities[i])) self.PlotImages(observations[idx[0]], idx[1:], descriptors[-1]) return descriptors, probabilities
def SenseAndAct(self, hand, prevDesc, cloudTree, epsilon): '''Senses the current state, s, and determines the next action, a.''' # generate image for base frame descriptor newDesc = HandDescriptor(copy(prevDesc.T)) newDesc.imH = 0.105 newDesc.imW = 0.105 newDesc.imD = 0.105 newDesc.GenerateDepthImage(cloudTree.data, cloudTree) s = self.emptyState if hand is None else [ hand.image, self.emptyStateVector ] # decide which location in the image to zoom into bX, iX = self.SampleActions(newDesc, cloudTree) bestIdx, bestValue = self.SelectIndexEpsilonGreedy( s, newDesc.image, iX, epsilon) # compose action a, desc = self.ComposeAction(newDesc, bX[bestIdx], iX[bestIdx]) return s, a, desc, bestValue
def SetInitialDescriptor(self): '''Sets the center of the initial descriptor to the provided center.''' T = eye(4) T[0:3, 3] = array([0, 0, self.imD / 2.0]) self.initDesc = HandDescriptor(T, self.imP, self.imD, self.imW)
cloud_proxy = CloudProxy() grasp_proxy = GraspProxy() rviz_node = RvizNode() plot_rviz = PlotRviz() plot_rviz.initRos() cloud = point_cloud.LoadPcd( os.path.dirname(__file__) + '/../data/pcd/' + cloud_name) cloud = point_cloud_util.filter_workspace(workspace, cloud) grasps = grasp_proxy.detect_grasps_whole_cloud(cloud, grasp_offsets) image_3d = [] for grasp in grasps: hand_des_3d = HandDescriptor( HandDescriptor.T_from_approach_axis_center( grasp.approach, grasp.axis, (grasp.bottom + grasp.top) / 2), 32) hand_des_3d.generate_3d_binary_image(cloud) image_3d.append(hand_des_3d.image) cnn.eval() image_3d = np.stack(image_3d, 0) inputs = torch.FloatTensor(image_3d).unsqueeze(1).cuda() inputs = Variable(inputs, volatile=True) outputs = cnn(inputs) prediction = outputs.data.max(1, keepdim=True)[1] prediction = prediction.squeeze().tolist() edge_grasp = [] non_edge_grasp = [] for i in range(len(grasps)): if prediction[i] == 1:
cloud_proxy = CloudProxy() grasp_proxy = GraspProxy() rviz_node = RvizNode() plot_rviz = PlotRviz() plot_rviz.initRos() cloud = point_cloud.LoadPcd( os.path.dirname(__file__) + '/../data/pcd/' + cloud_name) cloud = point_cloud_util.filter_workspace(workspace, cloud) grasps = grasp_proxy.detect_grasps_whole_cloud(cloud, grasp_offsets) image_2d = [] for grasp in grasps: hand_des_2d = HandDescriptor( HandDescriptor.T_from_approach_axis_center( grasp.approach, grasp.axis, (grasp.bottom + grasp.top) / 2)) hand_des_2d.generate_depth_image(cloud) image_2d.append(hand_des_2d.image) cnn.eval() image_2d = np.stack(image_2d, 0) inputs = torch.FloatTensor(image_2d).cuda() inputs = Variable(inputs, volatile=True) outputs = cnn(inputs) prediction = outputs.data.max(1, keepdim=True)[1] prediction = prediction.squeeze().tolist() edge_grasp = [] non_edge_grasp = [] for i in range(len(grasps)): if prediction[i] == 1: