def processSnippet(): obj = om.getOrCreateContainer('continuous') om.getOrCreateContainer('cont debug', obj) if (continuouswalkingDemo.processContinuousStereo): polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/block_snippet_stereo.vtp')) polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01) else: polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/block_snippet.vtp')) vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='continuous') standingFootName = 'l_foot' standingFootFrame = robotStateModel.getLinkFrame(standingFootName) vis.updateFrame(standingFootFrame, standingFootName, parent='continuous', visible=False) # Step 2: find all the surfaces in front of the robot (about 0.75sec) clusters = segmentation.findHorizontalSurfaces(polyData) if (clusters is None): print "No cluster found, stop walking now!" return # Step 3: find the corners of the minimum bounding rectangles blocks,groundPlane = cwdemo.extractBlocksFromSurfaces(clusters, standingFootFrame) footsteps = cwdemo.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame) cwdemo.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)
def findMinimumBoundingRectangle(self, polyData, linkFrame): ''' Find minimum bounding rectangle. The input is assumed to be a rectangular point cloud of cinder blocks Returns transform of far right corner (pointing away from robot) ''' # TODO: for non-z up surfaces, this needs work # TODO: return other parameters # Originally From: https://github.com/dbworth/minimum-area-bounding-rectangle polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.02) #vis.updatePolyData( polyData, 'block top', parent='cont debug', visible=False) polyDataCentroid = segmentation.computeCentroid(polyData) pts =vtkNumpy.getNumpyFromVtk( polyData , 'Points' ) xy_points = pts[:,[0,1]] vis.updatePolyData( get2DAsPolyData(xy_points) , 'xy_points', parent='cont debug', visible=False) hull_points = qhull_2d.qhull2D(xy_points) vis.updatePolyData( get2DAsPolyData(hull_points) , 'hull_points', parent='cont debug', visible=False) # Reverse order of points, to match output from other qhull implementations hull_points = hull_points[::-1] # print 'Convex hull points: \n', hull_points, "\n" # Find minimum area bounding rectangle (rot_angle, rectArea, rectDepth, rectWidth, center_point, corner_points_ground) = min_bounding_rect.minBoundingRect(hull_points) vis.updatePolyData( get2DAsPolyData(corner_points_ground) , 'corner_points_ground', parent='cont debug', visible=False) cornerPoints = np.vstack((corner_points_ground.T, polyDataCentroid[2]*np.ones( corner_points_ground.shape[0]) )).T cornerPolyData = vtkNumpy.getVtkPolyDataFromNumpyPoints(cornerPoints) # Create a frame at the far right point - which points away from the robot farRightCorner = self.findFarRightCorner(cornerPolyData , linkFrame) viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection() robotYaw = math.atan2( viewDirection[1], viewDirection[0] )*180.0/np.pi blockAngle = rot_angle*(180/math.pi) #print "robotYaw ", robotYaw #print "blockAngle ", blockAngle blockAngleAll = np.array([blockAngle , blockAngle+90 , blockAngle+180, blockAngle+270]) #print blockAngleAll for i in range(0,4): if(blockAngleAll[i]>180): blockAngleAll[i]=blockAngleAll[i]-360 #print blockAngleAll values = abs(blockAngleAll - robotYaw) #print values min_idx = np.argmin(values) if ( (min_idx==1) or (min_idx==3) ): #print "flip rectDepth and rectWidth as angle is not away from robot" temp = rectWidth ; rectWidth = rectDepth ; rectDepth = temp #print "best angle", blockAngleAll[min_idx] rot_angle = blockAngleAll[min_idx]*math.pi/180.0 # Optional: overwrite all of the above with the yaw of the robt when it was standing in front of the blocks: if (self.fixBlockYawWithInitial): rot_angle = self.initialRobotYaw cornerTransform = transformUtils.frameFromPositionAndRPY( farRightCorner , [0,0, np.rad2deg(rot_angle) ] ) #print "Minimum area bounding box:" #print "Rotation angle:", rot_angle, "rad (", rot_angle*(180/math.pi), "deg )" #print "rectDepth:", rectDepth, " rectWidth:", rectWidth, " Area:", rectArea #print "Center point: \n", center_point # numpy array #print "Corner points: \n", cornerPoints, "\n" # numpy array return cornerTransform, rectDepth, rectWidth, rectArea
def fitRunningBoardAtFeet(self): # get stance frame startPose = self.getPlanningStartPose() stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(self.robotSystem.robotStateModel, useWorldZ=False) stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame) # get pointcloud and extract search region covering the running board polyData = segmentation.getCurrentRevolutionData() polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01) _, polyData = segmentation.removeGround(polyData) polyData = segmentation.cropToBox(polyData, stanceFrame, [1.0, 1.0, 0.1]) if not polyData.GetNumberOfPoints(): print 'empty search region point cloud' return vis.updatePolyData(polyData, 'running board search points', parent=segmentation.getDebugFolder(), color=[0,1,0], visible=False) # extract maximal points along the stance x axis perpAxis = stanceFrameAxes[0] edgeAxis = stanceFrameAxes[1] edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis) edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints) vis.updatePolyData(edgePoints, 'edge points', parent=segmentation.getDebugFolder(), visible=True) # ransac fit a line to the edge points linePoint, lineDirection, fitPoints = segmentation.applyLineFit(edgePoints) if np.dot(lineDirection, stanceFrameAxes[1]) < 0: lineDirection = -lineDirection linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels', [1.0, 1.0]) dists = np.dot(vnp.getNumpyFromVtk(linePoints, 'Points')-linePoint, lineDirection) p1 = linePoint + lineDirection*np.min(dists) p2 = linePoint + lineDirection*np.max(dists) vis.updatePolyData(fitPoints, 'line fit points', parent=segmentation.getDebugFolder(), colorByName='ransac_labels', visible=False) # compute a new frame that is in plane with the stance frame # and matches the orientation and position of the detected edge origin = np.array(stanceFrame.GetPosition()) normal = np.array(stanceFrameAxes[2]) # project stance origin to edge, then back to foot frame originProjectedToEdge = linePoint + lineDirection*np.dot(origin - linePoint, lineDirection) originProjectedToPlane = segmentation.projectPointToPlane(originProjectedToEdge, origin, normal) zaxis = np.array(stanceFrameAxes[2]) yaxis = np.array(lineDirection) xaxis = np.cross(yaxis, zaxis) xaxis /= np.linalg.norm(xaxis) yaxis = np.cross(zaxis, xaxis) yaxis /= np.linalg.norm(yaxis) d = DebugData() d.addSphere(p1, radius=0.005) d.addSphere(p2, radius=0.005) d.addLine(p1, p2) d.addSphere(originProjectedToEdge, radius=0.001, color=[1,0,0]) d.addSphere(originProjectedToPlane, radius=0.001, color=[0,1,0]) d.addLine(originProjectedToPlane, origin, color=[0,1,0]) d.addLine(originProjectedToEdge, origin, color=[1,0,0]) vis.updatePolyData(d.getPolyData(), 'running board edge', parent=segmentation.getDebugFolder(), colorByName='RGB255', visible=False) # update the running board box affordance position and orientation to # fit the detected edge box = self.spawnRunningBoardAffordance() boxDimensions = box.getProperty('Dimensions') t = transformUtils.getTransformFromAxesAndOrigin(xaxis, yaxis, zaxis, originProjectedToPlane) t.PreMultiply() t.Translate(-boxDimensions[0]/2.0, 0.0, -boxDimensions[2]/2.0) box.getChildFrame().copyFrame(t) self.initialize()
def fitRunningBoardAtFeet(self): # get stance frame startPose = self.getPlanningStartPose() stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint( self.robotSystem.robotStateModel, useWorldZ=False) stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame) # get pointcloud and extract search region covering the running board polyData = segmentation.getCurrentRevolutionData() polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01) _, polyData = segmentation.removeGround(polyData) polyData = segmentation.cropToBox(polyData, stanceFrame, [1.0, 1.0, 0.1]) if not polyData.GetNumberOfPoints(): print 'empty search region point cloud' return vis.updatePolyData(polyData, 'running board search points', parent=segmentation.getDebugFolder(), color=[0, 1, 0], visible=False) # extract maximal points along the stance x axis perpAxis = stanceFrameAxes[0] edgeAxis = stanceFrameAxes[1] edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis) edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints) vis.updatePolyData(edgePoints, 'edge points', parent=segmentation.getDebugFolder(), visible=True) # ransac fit a line to the edge points linePoint, lineDirection, fitPoints = segmentation.applyLineFit( edgePoints) if np.dot(lineDirection, stanceFrameAxes[1]) < 0: lineDirection = -lineDirection linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels', [1.0, 1.0]) dists = np.dot( vnp.getNumpyFromVtk(linePoints, 'Points') - linePoint, lineDirection) p1 = linePoint + lineDirection * np.min(dists) p2 = linePoint + lineDirection * np.max(dists) vis.updatePolyData(fitPoints, 'line fit points', parent=segmentation.getDebugFolder(), colorByName='ransac_labels', visible=False) # compute a new frame that is in plane with the stance frame # and matches the orientation and position of the detected edge origin = np.array(stanceFrame.GetPosition()) normal = np.array(stanceFrameAxes[2]) # project stance origin to edge, then back to foot frame originProjectedToEdge = linePoint + lineDirection * np.dot( origin - linePoint, lineDirection) originProjectedToPlane = segmentation.projectPointToPlane( originProjectedToEdge, origin, normal) zaxis = np.array(stanceFrameAxes[2]) yaxis = np.array(lineDirection) xaxis = np.cross(yaxis, zaxis) xaxis /= np.linalg.norm(xaxis) yaxis = np.cross(zaxis, xaxis) yaxis /= np.linalg.norm(yaxis) d = DebugData() d.addSphere(p1, radius=0.005) d.addSphere(p2, radius=0.005) d.addLine(p1, p2) d.addSphere(originProjectedToEdge, radius=0.001, color=[1, 0, 0]) d.addSphere(originProjectedToPlane, radius=0.001, color=[0, 1, 0]) d.addLine(originProjectedToPlane, origin, color=[0, 1, 0]) d.addLine(originProjectedToEdge, origin, color=[1, 0, 0]) vis.updatePolyData(d.getPolyData(), 'running board edge', parent=segmentation.getDebugFolder(), colorByName='RGB255', visible=False) # update the running board box affordance position and orientation to # fit the detected edge box = self.spawnRunningBoardAffordance() boxDimensions = box.getProperty('Dimensions') t = transformUtils.getTransformFromAxesAndOrigin( xaxis, yaxis, zaxis, originProjectedToPlane) t.PreMultiply() t.Translate(-boxDimensions[0] / 2.0, 0.0, -boxDimensions[2] / 2.0) box.getChildFrame().copyFrame(t) self.initialize()