# 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
