# Planning: ====================================================================================== didMove = False for planAttemptIdx in xrange(maxPlanningAttempts): # sample viewpoints sample, configs = planner.sampleFirstRandomViewpoint(\ laserPoint, sensorKeepout, nSphereSamples, minViewDownAngle, obstacleCloudTree, arm) if sample is None: print("No IK solutions after testing {} samples.".format( nSphereSamples)) continue # move to viewpoint targConfig = arm.findClosestIK(configs) didMove = planner.hierarchicalPlanAndMove(targConfig, arm) if not didMove: print("No motion plan found to move to sample.") continue print("Getting structure cloud") # get data and detect grasps cloud = structure.getAndProcessCloud(node, nGraspClouds, graspCloudWorkspace) grasp = node.detectGrasp(cloud, obstacleCloudTree, sample, targConfig, laserPoint, \ graspSearchRadius, graspOffsets, graspsTopK, structure, arm) #Force failure for testing #grasp = None