# load poly data dataDir = app.getTestingDataDirectory() polyData = ioUtils.readPolyData(os.path.join(dataDir, 'amazon-pod/01-small-changes.vtp')) vis.showPolyData(polyData, 'pointcloud snapshot', visible=False) # remove ground and clip to just the pod: groundPoints, polyData = segmentation.removeGround(polyData) vis.showPolyData(polyData, 'scene', visible=False) polyData = segmentation.addCoordArraysToPolyData(polyData) polyData = segmentation.thresholdPoints(polyData, 'y', [1, 1.6]) polyData = segmentation.thresholdPoints(polyData, 'x', [-1.2, 0.5]) vis.showPolyData(polyData, 'clipped', visible=False) # remove outliers polyData = segmentation.labelOutliers(polyData, searchRadius=0.03, neighborsInSearchRadius=40) polyData = segmentation.thresholdPoints(polyData, 'is_outlier', [0, 0]) vis.showPolyData(polyData, 'inliers', visible=False) # remove walls, and points behind temp: polyData = removePlaneAndBeyond(polyData, expectedNormal=[0,1,0], filterRange=[-np.inf, -0.03], whichAxis=1, whichAxisLetter='y', percentile = 95) polyData = removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[-np.inf, -0.03], whichAxis=0, whichAxisLetter='x', percentile = 95) polyData = removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[0.03, np.inf], whichAxis=0, whichAxisLetter='x', percentile = 5) vis.updatePolyData(polyData, 'only shelves', parent='segmentation', visible=False) polyData = fitObjectsOnShelf(polyData) polyData = fitObjectsOnShelf(polyData) polyData = fitObjectsOnShelf(polyData) polyData = fitObjectsOnShelf(polyData, maxHeight=0.2)
dataDir = app.getTestingDataDirectory()
polyData = ioUtils.readPolyData(
os.path.join(dataDir, 'amazon-pod/01-small-changes.vtp'))
vis.showPolyData(polyData, 'pointcloud snapshot', visible=False)
# remove ground and clip to just the pod:
groundPoints, polyData = segmentation.removeGround(polyData)
vis.showPolyData(polyData, 'scene', visible=False)
polyData = segmentation.addCoordArraysToPolyData(polyData)
polyData = segmentation.thresholdPoints(polyData, 'y', [1, 1.6])
polyData = segmentation.thresholdPoints(polyData, 'x', [-1.2, 0.5])
vis.showPolyData(polyData, 'clipped', visible=False)
# remove outliers
polyData = segmentation.labelOutliers(polyData,
searchRadius=0.03,
neighborsInSearchRadius=40)
polyData = segmentation.thresholdPoints(polyData, 'is_outlier', [0, 0])
vis.showPolyData(polyData, 'inliers', visible=False)
# remove walls, and points behind temp:
polyData = removePlaneAndBeyond(polyData,
expectedNormal=[0, 1, 0],
filterRange=[-np.inf, -0.03],
whichAxis=1,
whichAxisLetter='y',
percentile=95)
polyData = removePlaneAndBeyond(polyData,
expectedNormal=[1, 0, 0],
filterRange=[-np.inf, -0.03],
whichAxis=0,
def replanFootsteps(self, polyData, standingFootName, removeFirstLeftStep=True, doStereoFiltering=True, nextDoubleSupportPose=None):
obj = om.getOrCreateContainer('continuous')
om.getOrCreateContainer('cont debug', obj)
vis.updatePolyData( polyData, 'walking snapshot', parent='cont debug', visible=False)
standingFootFrame = self.robotStateModel.getLinkFrame(standingFootName)
vis.updateFrame(standingFootFrame, standingFootName, parent='cont debug', visible=False)
# TODO: remove the pitch and roll of this frame to support it being on uneven ground
# Step 1: filter the data down to a box in front of the robot:
polyData = self.getRecedingTerrainRegion(polyData, footstepsdriver.FootstepsDriver.getFeetMidPoint(self.robotStateModel))
if (doStereoFiltering is True):
# used for stereo data:
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
polyData = segmentation.labelOutliers(polyData, searchRadius=0.06, neighborsInSearchRadius=15) # 0.06 and 10 originally
vis.updatePolyData(polyData, 'voxel plane points', parent='cont debug', colorByName='is_outlier', visible=False)
polyData = segmentation.thresholdPoints(polyData, 'is_outlier', [0, 0])
vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='cont debug', visible=True)
# Step 2: find all the surfaces in front of the robot (about 0.75sec)
clusters = segmentation.findHorizontalSurfaces(polyData, removeGroundFirst=False, normalEstimationSearchRadius=0.05,
clusterTolerance=0.025, distanceToPlaneThreshold=0.0025, normalsDotUpRange=[0.95, 1.0])
if (clusters is None):
print "No cluster found, stop walking now!"
return
# Step 3: find the corners of the minimum bounding rectangles
blocks,groundPlane = self.extractBlocksFromSurfaces(clusters, standingFootFrame)
# Step 5: Find the two foot positions we should plan with: the next committed tool and the current standing foot
'''
if (self.committedStep is not None):
#print "i got a committedStep. is_right_foot?" , self.committedStep.is_right_foot
if (self.committedStep.is_right_foot):
standingFootTransform = self.robotStateModel.getLinkFrame(self.ikPlanner.leftFootLink)
nextDoubleSupportPose = self.getNextDoubleSupportPose(standingFootTransform, self.committedStep.transform)
else:
standingFootTransform = self.robotStateModel.getLinkFrame(self.ikPlanner.rightFootLink)
nextDoubleSupportPose = self.getNextDoubleSupportPose(self.committedStep.transform, standingFootTransform)
comm_mesh,comm_color = self.getMeshAndColor(self.committedStep.is_right_foot)
comm_color[1] = 0.75 ; comm_color[2] = 0.25
stand_mesh, stand_color = self.getMeshAndColor( not self.committedStep.is_right_foot )
stand_color[1] = 0.75 ; stand_color[2] = 0.25
vis.updateFrame(self.committedStep.transform, 'committed foot', parent='foot placements', scale=0.2, visible=False)
obj = vis.showPolyData(comm_mesh, 'committed step', color=comm_color, alpha=1.0, parent='steps')
obj.actor.SetUserTransform(self.committedStep.transform)
vis.updateFrame(standingFootTransform, 'standing foot', parent='foot placements', scale=0.2, visible=False)
obj = vis.showPolyData(stand_mesh, 'standing step', color=stand_color, alpha=1.0, parent='steps')
obj.actor.SetUserTransform(standingFootTransform)
else:
# don't have a committed footstep, assume we are standing still
nextDoubleSupportPose = self.robotStateJointController.getPose('EST_ROBOT_STATE')
'''
self.displayExpectedPose(nextDoubleSupportPose)
if not self.useManualFootstepPlacement and self.queryPlanner:
footsteps = self.computeFootstepPlanSafeRegions(blocks, nextDoubleSupportPose, standingFootName)
else:
footsteps = self.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame, removeFirstLeftStep)
if not len(footsteps):
return
if self.queryPlanner:
self.sendPlanningRequest(footsteps, nextDoubleSupportPose)
else:
self.drawFittedSteps(footsteps)
def replanFootsteps(self, polyData, standingFootName, removeFirstLeftStep=True, doStereoFiltering=True, nextDoubleSupportPose=None):
obj = om.getOrCreateContainer('continuous')
om.getOrCreateContainer('cont debug', obj)
vis.updatePolyData( polyData, 'walking snapshot', parent='cont debug', visible=False)
standingFootFrame = self.robotStateModel.getLinkFrame(standingFootName)
vis.updateFrame(standingFootFrame, standingFootName, parent='cont debug', visible=False)
# TODO: remove the pitch and roll of this frame to support it being on uneven ground
# Step 1: filter the data down to a box in front of the robot:
polyData = self.getRecedingTerrainRegion(polyData, footstepsdriver.FootstepsDriver.getFeetMidPoint(self.robotStateModel))
if (doStereoFiltering is True):
# used for stereo data:
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
polyData = segmentation.labelOutliers(polyData, searchRadius=0.06, neighborsInSearchRadius=15) # 0.06 and 10 originally
vis.updatePolyData(polyData, 'voxel plane points', parent='cont debug', colorByName='is_outlier', visible=False)
polyData = segmentation.thresholdPoints(polyData, 'is_outlier', [0, 0])
vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='cont debug', visible=True)
# Step 2: find all the surfaces in front of the robot (about 0.75sec)
clusters = segmentation.findHorizontalSurfaces(polyData, removeGroundFirst=False, normalEstimationSearchRadius=0.05,
clusterTolerance=0.025, distanceToPlaneThreshold=0.0025, normalsDotUpRange=[0.95, 1.0])
if (clusters is None):
print "No cluster found, stop walking now!"
return
# Step 3: find the corners of the minimum bounding rectangles
blocks,groundPlane = self.extractBlocksFromSurfaces(clusters, standingFootFrame)
# Step 5: Find the two foot positions we should plan with: the next committed tool and the current standing foot
'''
if (self.committedStep is not None):
#print "i got a committedStep. is_right_foot?" , self.committedStep.is_right_foot
if (self.committedStep.is_right_foot):
standingFootTransform = self.robotStateModel.getLinkFrame(self.ikPlanner.leftFootLink)
nextDoubleSupportPose = self.getNextDoubleSupportPose(standingFootTransform, self.committedStep.transform)
else:
standingFootTransform = self.robotStateModel.getLinkFrame(self.ikPlanner.rightFootLink)
nextDoubleSupportPose = self.getNextDoubleSupportPose(self.committedStep.transform, standingFootTransform)
comm_mesh,comm_color = self.getMeshAndColor(self.committedStep.is_right_foot)
comm_color[1] = 0.75 ; comm_color[2] = 0.25
stand_mesh, stand_color = self.getMeshAndColor( not self.committedStep.is_right_foot )
stand_color[1] = 0.75 ; stand_color[2] = 0.25
vis.updateFrame(self.committedStep.transform, 'committed foot', parent='foot placements', scale=0.2, visible=False)
obj = vis.showPolyData(comm_mesh, 'committed step', color=comm_color, alpha=1.0, parent='steps')
obj.actor.SetUserTransform(self.committedStep.transform)
vis.updateFrame(standingFootTransform, 'standing foot', parent='foot placements', scale=0.2, visible=False)
obj = vis.showPolyData(stand_mesh, 'standing step', color=stand_color, alpha=1.0, parent='steps')
obj.actor.SetUserTransform(standingFootTransform)
else:
# don't have a committed footstep, assume we are standing still
nextDoubleSupportPose = self.robotStateJointController.getPose('EST_ROBOT_STATE')
'''
self.displayExpectedPose(nextDoubleSupportPose)
if not self.useManualFootstepPlacement and self.queryPlanner:
footsteps = self.computeFootstepPlanSafeRegions(blocks, nextDoubleSupportPose, standingFootName)
else:
footsteps = self.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame, removeFirstLeftStep)
if not len(footsteps):
return
if self.queryPlanner:
self.sendPlanningRequest(footsteps, nextDoubleSupportPose)
else:
self.drawFittedSteps(footsteps)
