def GetHandPoints(self, cloud, normals=None, cloudTree=None): '''Determines which points are in the hand descriptor.''' # Step 1: Find points within a ball containing the hand. if cloudTree is None: X = cloud N = normals else: searchRadius = norm(array([self.imD, self.imW, self.imH]) / 2.0) indices = cloudTree.query_ball_point(self.center, searchRadius) X = cloud[indices, :] if normals is not None: N = normals[indices, :] # Step 2: Transform points into grasp frame. # Step 3: Filter out points that are not in the hand. hTb = inv(self.T) workspace = [(-self.imH / 2, self.imH / 2), (-self.imW / 2, self.imW / 2), (-self.imD / 2, self.imD / 2)] if normals is None: X = point_cloud.Transform(hTb, X) X = point_cloud.FilterWorkspace(workspace, X) else: X, N = point_cloud.Transform(hTb, X, N) X, N = point_cloud.FilterWorkspace(hTb, X, N) if normals is None: self.handPoints = X return X self.handNormals = N return X, N
def IsRobotInCollision(self, descriptor): '''Checks collision between the robot and the world. - Input descriptor: HandDescriptor object for the current hand pose. - Returns: True if in collision and False otherwise. - Returns objCloudsInHandFrame: List of point clouds, one for each object, in the descriptor reference frame. Or, None if a collision is detected. (This is to avoid performing transforms of all object clouds twice.) ''' # ODE misses several box-cylinder collisions. So we have to implement this ourselves. # check collision with table bX = point_cloud.Transform(descriptor.T, self.externalHandPoints) if (bX[:, 2] < self.GetTableHeight()).any(): return True, None # some preparation hTb = inv(descriptor.T) self.robot.SetTransform(descriptor.T) # for visualization only objects = self.objects + self.supportObjects objCloudsInHandFrame = [] # check if any object points intersect hand collision geometry for i, obj in enumerate(objects): bTo = obj.GetTransform() hX = point_cloud.Transform(dot(hTb, bTo), obj.cloud) X = point_cloud.FilterWorkspace(self.handFingerRegionL, hX) if X.size > 0: return True, None X = point_cloud.FilterWorkspace(self.handFingerRegionR, hX) if X.size > 0: return True, None X = point_cloud.FilterWorkspace(self.handTopRegion, hX) if X.size > 0: return True, None if i < len(self.objects): objCloudsInHandFrame.append(hX) return False, objCloudsInHandFrame
def IsAntipodalGrasp(self, descriptor, targetObject, maxAngleToFinger): '''Returns True if a grasp is near antipodal, based on the parameters. - Input descriptor: HandDescriptor object with pose of grasp. - Input targetObject: OpenRAVE object handle with cloud and normals entries. - Input maxAngleToFinger: Maximum angle between surfance normal and finger in degrees. Used 10 degrees for blocks, 15 degrees for mugs, and 15 degrees for bottles. - Returns: True if antipodal grasp, False otherwise. ''' # parameters contactWidth = 0.01 maxAntipodalDist = 0.01 maxAngleToFinger = cos(maxAngleToFinger * (pi / 180)) # put cloud into hand reference frame bTo = targetObject.GetTransform() bTh = descriptor.T hTo = dot(inv(bTh), bTo) X, N = point_cloud.Transform(hTo, targetObject.cloud, targetObject.normals) X, N = point_cloud.FilterWorkspace( [(-descriptor.height / 2, descriptor.height / 2), (-descriptor.width / 2, descriptor.width / 2), (-descriptor.depth / 2, descriptor.depth / 2)], X, N) if X.shape[0] == 0: #print("No points in hand.") return False # find contact points leftPoint = min(X[:, 1]) rightPoint = max(X[:, 1]) lX, lN = point_cloud.FilterWorkspace( [(-1, 1), (leftPoint, leftPoint + contactWidth), (-1, 1)], X, N) rX, rN = point_cloud.FilterWorkspace( [(-1, 1), (rightPoint - contactWidth, rightPoint), (-1, 1)], X, N) # find contact points normal to finger lX = lX[-lN[:, 1] >= maxAngleToFinger, :] rX = rX[rN[:, 1] >= maxAngleToFinger, :] if lX.shape[0] == 0 or rX.shape[0] == 0: #print("No contact points normal to finger.") return False # are the closest two contact points nearly antipodal? leftTree = cKDTree(lX[:, (0, 2)]) d, idxs = leftTree.query(rX[:, (0, 2)]) #if min(d) >= maxAntipodalDist: # print("Contacts not antipodal.") return min(d) < maxAntipodalDist
def IsGrasp(self, descriptor): '''Checks if, when the hand is placed at the descriptor's pose and closed, a grasp takes place. - Input descriptor: HandDescriptor object of the target hand pose. - Returns graspedObject: The handle of the grasped object if a cylinder can be grasped from the target hand pose; otherwise None. ''' # check collision if self.IsRobotInCollision(descriptor): return None # check intersection of exactly 1 object graspedObject = None for i, obj in enumerate(self.objects): bTo = obj.GetTransform() hTo = dot(inv(descriptor.T), bTo) X = point_cloud.Transform(hTo, obj.cloud) X = point_cloud.FilterWorkspace(self.handClosingRegion, X) intersect = X.size > 0 if intersect: if graspedObject is None: graspedObject = obj else: # intersection of multiple objects return None if graspedObject is None: # intersection of no objects return None # check antipodal condition if self.IsAntipodalGrasp(descriptor, graspedObject, self.graspFrictionCone): return graspedObject return None
def GetHandPoints(self, cloud): '''Determines which points are in the descriptor volume. - Input cloud: Point cloud in the base/world frame. - Returns X: Point cloud in the hand frame, including only points in the descriptor volume. ''' workspace = [(-self.imW / 2, self.imW / 2), (-self.imW / 2, self.imW / 2), (-self.imD / 2, self.imD / 2)] X = point_cloud.Transform(inv(self.T), cloud) return point_cloud.FilterWorkspace(workspace, X)
def GetCloud(self, workspace=None): '''Agent gets point cloud from its sensor from the current position.''' self.StartSensor() self.env.StepSimulation(0.001) data = self.sensor.GetSensorData(openravepy.Sensor.Type.Laser) cloud = data.ranges + data.positions[0] self.StopSensor() if workspace is not None: cloud = point_cloud.FilterWorkspace(cloud, workspace) return cloud
def main(): '''Entrypoint to the program.''' # PARAMETERS ===================================================================================== # system gpuId = 0 nAgents = 6 # objects nObjects = 10 objectFolder = os.getcwd() + "/../../Data/RaveObjects/RectangularBlocks" graspColor = [0, 0, 1] placeColor = [1, 0, 0] # reaching nActionSamples = [500, 1000, 361, 181, 361, 303] stateImageWHD = [0.09, 0.09, 0.22] # view viewCenter = array([0, 0, 0]) viewKeepout = 0.70 viewWorkspace = [(-1.0, 1.0), (-1.0, 1.0), (-1.0, 1.0)] viewWorkspaceNoTable = [(-1.0, 1.0), (-1.0, 1.0), (0.002, 1.0)] # learning nTrainingRounds = 200 nScenes = 100 nEpisodes = 20 minEpsilon = 0.05 unbiasOnRound = nTrainingRounds - 5 maxExperiences = 50000 trainingBatchSize = maxExperiences minTrainBatchSize = 1024 # visualization/saving saveFileName = "results.mat" recordLoss = True loadNetwork = False loadDatabase = False showViewer = False showSteps = False plotImages = False # visualize policy visualizePolicy = True if visualizePolicy: nEpisodes = 1 unbiasOnRound = 0 loadNetwork = True loadDatabase = False showViewer = True showSteps = True plotImages = True # INITIALIZATION ================================================================================= rlEnv = RlEnvironmentPlacing(showViewer) experiences = [] rlAgentsGrasping = [] for i in xrange(nAgents): rlAgent = eval("RlAgentLevel" + str(i) + \ "(rlEnv, gpuId, False, nActionSamples[i], \"weights-grasp-blocks/\")") rlAgentsGrasping.append(rlAgent) experiences.append([]) rlAgentsGrasping[0].SetSamplingWorkspace(-viewWorkspaceNoTable[2][1], -viewWorkspaceNoTable[2][0]) rlAgentsPlacing = [] for i in xrange(nAgents): rlAgent = eval( "RlAgentLevel" + str(i) + "(rlEnv, gpuId, True, nActionSamples[i], \"weights-place-blocks/\")" ) rlAgentsPlacing.append(rlAgent) experiences.append([]) rlAgentsPlacing[0].SetSamplingWorkspace(-viewWorkspaceNoTable[2][1], -viewWorkspaceNoTable[2][0]) rlAgents = rlAgentsGrasping + rlAgentsPlacing if loadNetwork: for rlAgent in rlAgents: rlAgent.LoadNetworkWeights() rlAgent.caffeFirstTrain = False if loadDatabase: for i, rlAgent in enumerate(rlAgents): experiences[i] = rlAgent.LoadExperienceDatabase() # RUN TEST ======================================================================================= avgReturn = [] avgPlaceReward = [] avgGraspReward = [] epsilonGraspRound = [] epsilonPlaceRound = [] graspDatabaseSize = [] placeDatabaseSize = [] roundTime = [] trainLoss = [] testLoss = [] for i in xrange(len(rlAgents)): trainLoss.append([]) testLoss.append([]) for trainingRound in xrange(nTrainingRounds): # initialization iterationStartTime = time.time() Return = [] placeReward = [] graspReward = [] # cool exploration and check if it's time to unbias data if trainingRound >= unbiasOnRound: epsilonGrasp = 0.0 epsilonPlace = 0.0 else: epsilonGrasp = max( minEpsilon, 1.0 - float(len(experiences[0])) / float(maxExperiences)) epsilonPlace = max( minEpsilon, 1.0 - float(len(experiences[-1]) / float(maxExperiences))) epsilonGraspRound.append(epsilonGrasp) epsilonPlaceRound.append(epsilonPlace) for scene in xrange(nScenes): # place random object in random orientation on table rlAgents[0].MoveHandToHoldingPose() objHandles = rlEnv.PlaceObjects(randint(2, nObjects + 1), objectFolder) if showSteps: raw_input("Placed objects.") # get a point cloud cloudScene, cloudTreeScene = rlAgentsPlacing[0].GetDualCloud( viewCenter, viewKeepout, viewWorkspace) rlAgentsPlacing[0].PlotCloud(cloudScene) if showSteps: raw_input("Acquired point cloud.") cloudSceneNoTable = point_cloud.FilterWorkspace( viewWorkspaceNoTable, cloudScene) cloudTreeSceneNoTable = cKDTree( cloudSceneNoTable) if cloudSceneNoTable.shape[0] > 0 else None for episode in xrange(nEpisodes): states = [] actions = [] graspDesc = None placeDesc = None # grasp an object for rlAgent in rlAgentsGrasping: s, a, graspDesc, v = rlAgent.SenseAndAct( None, graspDesc, cloudTreeScene, epsilonGrasp) rlAgentsGrasping[0].PlotDescriptors([graspDesc], graspColor) if plotImages: rlAgent.PlotImages(s, a) states.append(s) actions.append(a) # evaluate grasp rGrasp = rlEnv.TransitionTopGraspHalfConditions( graspDesc, rlAgentsGrasping[-1], objHandles) if showSteps: raw_input("Grasp received reward {}.".format(rGrasp)) graspReward.append(rGrasp) # if grasp was a success, try a place if rGrasp > 0.00: rlEnv.SetObjectPoses(objHandles) rlEnv.GraspObjectAndCenter(rlAgentsPlacing[0], graspDesc, objHandles) if showSteps: raw_input("Removed object.") # update hand contents graspDesc.imW = stateImageWHD[0] graspDesc.imH = stateImageWHD[1] graspDesc.imD = stateImageWHD[2] graspDesc.GenerateDepthImage(cloudSceneNoTable, cloudTreeSceneNoTable) # get a point cloud cloud, cloudTree = rlAgentsPlacing[0].GetDualCloud( viewCenter, viewKeepout, viewWorkspace) rlAgentsPlacing[0].PlotCloud(cloud) if showSteps: raw_input("Acquired point cloud.") # act at each level in the hierarchy for rlAgent in rlAgentsPlacing: s, a, placeDesc, v = rlAgent.SenseAndAct( graspDesc, placeDesc, cloudTree, epsilonPlace) rlAgentsPlacing[0].PlotDescriptors([placeDesc], placeColor) if plotImages: rlAgent.PlotImages(s, a) states.append(s) actions.append(a) if showSteps: raw_input("Decided place pose.") # check place and finish rPlace = rlEnv.TransitionPlaceOnObject( placeDesc, rlAgentsPlacing[-1], objHandles, showSteps) if showSteps: raw_input("Place received reward {}.".format(rPlace)) placeReward.append(rPlace) rlEnv.ResetObjectPoses(objHandles) else: # grasp was a failure rPlace = 0.0 # save experiences for i in xrange(nAgents): experiences[i].append( (states[i], actions[i], rGrasp + rPlace)) if len(states) > nAgents: for i in xrange(nAgents, 2 * nAgents): experiences[i].append((states[i], actions[i], rPlace)) # cleanup this episode r = rGrasp + rPlace print("Episode {}.{}.{} had return {}".format( trainingRound, scene, episode, r)) Return.append(r) # cleanup this scene rlEnv.RemoveObjectSet(objHandles) # Train each agent. for i, rlAgent in enumerate(rlAgents): if len(experiences[i]) < minTrainBatchSize: continue experiences[i] = rlAgent.PruneDatabase(experiences[i], maxExperiences) Dl = rlAgent.DownsampleData(experiences[i], trainingBatchSize) loss = rlAgent.Train(Dl, recordLoss=recordLoss) trainLoss[i].append(loss[0]) testLoss[i].append(loss[1]) # Save results avgReturn.append(mean(Return)) if len(placeReward) == 0: placeReward.append(0) avgPlaceReward.append(mean(placeReward)) avgGraspReward.append(mean(graspReward)) graspDatabaseSize.append(len(experiences[0])) placeDatabaseSize.append(len(experiences[-1])) roundTime.append(time.time() - iterationStartTime) saveData = { "gpuId": gpuId, "nAgents": nAgents, "nObjects": nObjects, "objectFolder": objectFolder, "viewCenter": viewCenter, "viewKeepout": viewKeepout, "viewWorkspace": viewWorkspace, "viewWorkspaceNoTable": viewWorkspaceNoTable, "nActionSamples": nActionSamples, "stateImageWHD": stateImageWHD, "nTrainingRounds": nTrainingRounds, "nScenes": nScenes, "nEpisodes": nEpisodes, "epsilonGraspRound": epsilonGraspRound, "epsilonPlaceRound": epsilonPlaceRound, "minEpsilon": minEpsilon, "unbiasOnRound": unbiasOnRound, "maxExperiences": maxExperiences, "trainingBatchSize": trainingBatchSize, "minTrainBatchSize": minTrainBatchSize, "avgReturn": avgReturn, "avgPlaceReward": avgPlaceReward, "avgGraspReward": avgGraspReward, "graspDatabaseSize": graspDatabaseSize, "placeDatabaseSize": placeDatabaseSize, "roundTime": roundTime, "trainLoss0": trainLoss[0], "testLoss0": testLoss[0], "trainLoss1": trainLoss[1], "testLoss1": testLoss[1], "trainLoss2": trainLoss[2], "testLoss2": testLoss[2], "trainLoss3": trainLoss[3], "testLoss3": testLoss[3], "trainLoss4": trainLoss[4], "testLoss4": testLoss[4], "trainLoss5": trainLoss[5], "testLoss5": testLoss[5], "trainLoss6": trainLoss[6], "testLoss6": testLoss[6], "trainLoss7": trainLoss[7], "testLoss7": testLoss[7], "trainLoss8": trainLoss[8], "testLoss8": testLoss[8], "trainLoss9": trainLoss[9], "testLoss9": testLoss[9], "trainLoss10": trainLoss[10], "testLoss10": testLoss[10], "trainLoss11": trainLoss[11], "testLoss11": testLoss[11] } savemat(saveFileName, saveData) # Backup experience database if (trainingRound == nTrainingRounds - 1) or (trainingRound % 10 == 9): for i, rlAgent in enumerate(rlAgents): rlAgent.SaveExperienceDatabase(experiences[i])
def IsPegGrasp(self, descriptor): '''Checks if, when the hand is placed at the descriptor's pose and closed, a grasp takes place. A grasp must be (1) collision-free (2) contain exactly 1 peg's geometry, (3) contain the cylinder's axis, and (4) not contact the side and cap of the cylinder. - Input descriptor: HandDescriptor object of the target hand pose. - Returns graspedObject: The handle of the grasped object if a cylinder can be grasped from the target hand pose; otherwise None. - Returns isCapGrasp: True if this is a good grasp and each finger contacts the bottom/top of the peg. ''' # check collision collision, objCloudsInHandFrame = self.IsRobotInCollision(descriptor) if collision: return None, False # check intersection of exactly 1 object graspedObject = None pointsInHand = None for i, obj in enumerate(self.objects): X = point_cloud.FilterWorkspace(self.handClosingRegion, objCloudsInHandFrame[i]) intersect = X.size > 0 if intersect: if graspedObject is None: graspedObject = obj pointsInHand = X else: # intersection of multiple objects return None, False if graspedObject is None: # intersection of no objects return None, False # A cylinder can only be upright or on the side. We handle these two cases separately. bTo = graspedObject.GetTransform() if self.IsPegUpright(graspedObject): # Top-center of cylinder in the hand is necessary and sufficient. bp = copy(bTo[0:3, 3]) bp[2] += graspedObject.height / 2.0 hp = point_cloud.Transform(inv(descriptor.T), array([bp])) hP = point_cloud.FilterWorkspace(self.handClosingRegion, hp) if hP.size == 0: return None, False return graspedObject, False # Cylinder is on its side. # check if finger tips are below cylinder axis cylinderZ = bTo[2, 3] fingerZ = descriptor.center[2] - descriptor.depth / 2.0 if fingerZ > cylinderZ: return None, False # make sure cylinder caps are not in hand contactIdxs = array( [argmax(pointsInHand[:, 1]), argmin(pointsInHand[:, 1])]) contacts = pointsInHand[contactIdxs, :] oX = point_cloud.Transform(dot(inv(bTo), descriptor.T), pointsInHand) capIdxs = sum( power(oX[:, 0:2], 2), 1) < (graspedObject.radius - self.pointToRealRadiusError)**2 capIdxs = capIdxs.flatten() nContactsOnCap = sum(capIdxs[contactIdxs]) if nContactsOnCap == 1 or sum(power(contacts[0, 0:2] - contacts[1, 0:2], 2)) < \ (min(2 * graspedObject.radius, graspedObject.height) - 2 * self.pointToRealRadiusError)**2: # 1 finger contacts cap, other finger contacts side return None, False # side grasp is good return graspedObject, nContactsOnCap == 2