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 updateDrawIntersection(self, frame): origin = np.array(frame.transform.GetPosition()) # print "origin is now at", origin d = DebugData() sliderIdx = self.slider.value controllerType = self.getControllerTypeFromCounter(sliderIdx) colorMaxRange = self.colorMap[controllerType] # if the QValue was empty then color it green if self.emptyQValue[sliderIdx]: colorMaxRange = [1, 1, 0] # this is yellow for i in xrange(self.Sensor.numRays): ray = self.Sensor.rays[:, i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) # print "rayTransformed is", rayTransformed intersection = self.Sensor.raycast(self.locator, origin, origin + rayTransformed * self.Sensor.rayLength) if intersection is not None: d.addLine(origin, intersection, color=[1, 0, 0]) else: d.addLine(origin, origin + rayTransformed * self.Sensor.rayLength, color=colorMaxRange) vis.updatePolyData(d.getPolyData(), "rays", colorByName="RGB255")
def setupStrings(): d = DebugData() poles = [[-.36,.5,1.5],[-.36,-.5,1.5],[0,.5,1.5],[0,-.5,1.5],[.36,.5,1.5],[.36,-.5,1.5]] for pole in poles: d.addCylinder(pole, [0,0,1], 3, radius=0.021) vis.updatePolyData(d.getPolyData(), 'poles') d = DebugData() strings = [ [-.36, .5, .09, 0, -.5, 1.77], [-.36, .5, .74, -.36, -.5, .95], [-.36, .5, 1.12, -.36, -.5, 1.68], [-.36, .5, 1.33, .36, -.5, 2.29], [-.36, .5, 1.6, .36, -.5, 1.62], [-.36, .5, 1.74, .36, -.5, 1.93], [-.36, .5, 2.15, -.36, -.5, 1.46], [0, .5, .765, 0, -.5, .795], [0, .5, 1.15, .36, -.5, 1.15], [0, .5, 1.28, -.36, -.5, .11], [0, .5, 1.42, 0, -.5, 1.42], [0, .5, 1.78, .36, -.5, .12], [0, .5, 2.05, -.36, -.5, 1.835], [.36, .5, .8, -.36, -.5, 1.11], [.36, .5, 1.16, -.36, -.5, 1.47], [.36, .5, 1.61, .36, -.5, 1.19], [.36, .5, 2.0, .36, -.5, 2.1], [-.36, .3, 0, -.36, .3, 2.01], [0, -.34, 0, 0, -.34, 1.42], [.36, 0, 0, .36, 0, 2.05], ] for string in strings: p1 = string[:3] p2 = string[3:] d.addLine(p1,p2,radius=.001,color=[255,0,0]) vis.updatePolyData(d.getPolyData(), 'strings')
def setupStrings(): d = DebugData() poles = [[-0.36, 0.5, 1.5], [-0.36, -0.5, 1.5], [0, 0.5, 1.5], [0, -0.5, 1.5], [0.36, 0.5, 1.5], [0.36, -0.5, 1.5]] for pole in poles: d.addCylinder(pole, [0, 0, 1], 3, radius=0.021) vis.updatePolyData(d.getPolyData(), "poles") d = DebugData() strings = [ [-0.36, 0.5, 0.09, 0, -0.5, 1.77], [-0.36, 0.5, 0.74, -0.36, -0.5, 0.95], [-0.36, 0.5, 1.12, -0.36, -0.5, 1.68], [-0.36, 0.5, 1.33, 0.36, -0.5, 2.29], [-0.36, 0.5, 1.6, 0.36, -0.5, 1.62], [-0.36, 0.5, 1.74, 0.36, -0.5, 1.93], [-0.36, 0.5, 2.15, -0.36, -0.5, 1.46], [0, 0.5, 0.765, 0, -0.5, 0.795], [0, 0.5, 1.15, 0.36, -0.5, 1.15], [0, 0.5, 1.28, -0.36, -0.5, 0.11], [0, 0.5, 1.42, 0, -0.5, 1.42], [0, 0.5, 1.78, 0.36, -0.5, 0.12], [0, 0.5, 2.05, -0.36, -0.5, 1.835], [0.36, 0.5, 0.8, -0.36, -0.5, 1.11], [0.36, 0.5, 1.16, -0.36, -0.5, 1.47], [0.36, 0.5, 1.61, 0.36, -0.5, 1.19], [0.36, 0.5, 2.0, 0.36, -0.5, 2.1], [-0.36, 0.3, 0, -0.36, 0.3, 2.01], [0, -0.34, 0, 0, -0.34, 1.42], [0.36, 0, 0, 0.36, 0, 2.05], ] for string in strings: p1 = string[:3] p2 = string[3:] d.addLine(p1, p2, radius=0.001, color=[255, 0, 0]) vis.updatePolyData(d.getPolyData(), "strings")
def updateIntersection(frame): origin = np.array(frame.transform.GetPosition()) rayLength = 5 for i in range(0,numRays): ray = rays[:,i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) intersection = computeIntersection(locator, origin, origin + rayTransformed*rayLength) name = 'ray intersection ' + str(i) if intersection is not None: om.removeFromObjectModel(om.findObjectByName(name)) d = DebugData() d.addLine(origin, intersection) color = [1,0,0] # d.addSphere(intersection, radius=0.04) vis.updatePolyData(d.getPolyData(), name, color=color) else: om.removeFromObjectModel(om.findObjectByName(name)) d = DebugData() d.addLine(origin, origin+rayTransformed*rayLength) color = [0,1,0] # d.addSphere(intersection, radius=0.04) vis.updatePolyData(d.getPolyData(), name, color=color)
def findFarRightCorner(self, polyData, linkFrame): ''' Within a point cloud find the point to the far right from the link The input is typically the 4 corners of a minimum bounding box ''' diagonalTransform = transformUtils.frameFromPositionAndRPY([0,0,0], [0,0,45]) diagonalTransform.Concatenate(linkFrame) vis.updateFrame(diagonalTransform, 'diagonal frame', parent='cont debug', visible=False) #polyData = shallowCopy(polyData) points = vtkNumpy.getNumpyFromVtk(polyData, 'Points') #vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x') #vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y') #vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z') viewOrigin = diagonalTransform.TransformPoint([0.0, 0.0, 0.0]) viewX = diagonalTransform.TransformVector([1.0, 0.0, 0.0]) viewY = diagonalTransform.TransformVector([0.0, 1.0, 0.0]) viewZ = diagonalTransform.TransformVector([0.0, 0.0, 1.0]) #polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x') polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y') #polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z') vis.updatePolyData( polyData, 'cornerPoints', parent='cont debug', visible=False) farRightIndex = vtkNumpy.getNumpyFromVtk(polyData, 'distance_along_foot_y').argmin() points = vtkNumpy.getNumpyFromVtk(polyData, 'Points') return points[farRightIndex,:]
def findFarRightCorner(self, polyData, linkFrame): ''' Within a point cloud find the point to the far right from the link The input is typically the 4 corners of a minimum bounding box ''' diagonalTransform = transformUtils.frameFromPositionAndRPY([0,0,0], [0,0,45]) diagonalTransform.Concatenate(linkFrame) vis.updateFrame(diagonalTransform, 'diagonal frame', parent='cont debug', visible=False) #polyData = shallowCopy(polyData) points = vtkNumpy.getNumpyFromVtk(polyData, 'Points') #vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x') #vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y') #vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z') viewOrigin = diagonalTransform.TransformPoint([0.0, 0.0, 0.0]) viewX = diagonalTransform.TransformVector([1.0, 0.0, 0.0]) viewY = diagonalTransform.TransformVector([0.0, 1.0, 0.0]) viewZ = diagonalTransform.TransformVector([0.0, 0.0, 1.0]) #polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x') polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y') #polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z') vis.updatePolyData( polyData, 'cornerPoints', parent='cont debug', visible=False) farRightIndex = vtkNumpy.getNumpyFromVtk(polyData, 'distance_along_foot_y').argmin() points = vtkNumpy.getNumpyFromVtk(polyData, 'Points') return points[farRightIndex,:]
def testFindWalls(self): raycastDistance = self.sensor.raycastAllFromCurrentFrameLocation() wallsFound = self.findWalls(raycastDistance, addNoise=True) carTransform = om.findObjectByName('robot frame').transform d = DebugData() for wallData in wallsFound: intercept = wallData['ransacFit'][0] slope = wallData['ransacFit'][1] wallDirectionInCarFrame = np.array([1.0, slope, 0.0]) wallPointInCarFrame = np.array([0.0, intercept, 0.0]) # now need to transform these to world frame in order to plot them. wallPointWorldFrame = np.array( carTransform.TransformPoint(wallPointInCarFrame)) wallDirectionWorldFrame = np.array( carTransform.TransformVector(wallDirectionInCarFrame)) wallDirectionWorldFrame = 1 / np.linalg.norm( wallDirectionWorldFrame) * wallDirectionWorldFrame lineLength = 15.0 lineOrigin = wallPointWorldFrame - lineLength * wallDirectionWorldFrame lineEnd = wallPointWorldFrame + lineLength * wallDirectionWorldFrame d.addLine(lineOrigin, lineEnd, radius=0.3, color=[0, 0, 1]) vis.updatePolyData(d.getPolyData(), 'line estimate', colorByName='RGB255') return wallsFound
def __init__(self, robotSystem, view): self.robotStateModel = robotSystem.robotStateModel self.robotStateJointController = robotSystem.robotStateJointController self.robotSystem = robotSystem self.lFootFtFrameId = self.robotStateModel.model.findLinkID( self.robotSystem.ikPlanner.leftFootLink) self.rFootFtFrameId = self.robotStateModel.model.findLinkID( self.robotSystem.ikPlanner.rightFootLink) self.leftInContact = 0 self.rightInContact = 0 self.view = view self.ddDrakeWrapper = PythonQt.dd.ddDrakeWrapper() self.warningButton = QtGui.QLabel('COP Warning') self.warningButton.setStyleSheet("background-color:white") self.dialogVisible = False d = DebugData() vis.updatePolyData(d.getPolyData(), 'measured cop', view=self.view, parent='robot state model') om.findObjectByName('measured cop').setProperty('Visible', False) self.robotStateModel.connectModelChanged(self.update)
def updateDrawIntersection(self, frame): origin = np.array(frame.transform.GetPosition()) #print "origin is now at", origin d = DebugData() sliderIdx = self.slider.value controllerType = self.getControllerTypeFromCounter(sliderIdx) colorMaxRange = self.colorMap[controllerType] # if the QValue was empty then color it green if self.emptyQValue[sliderIdx]: colorMaxRange = [1, 1, 0] # this is yellow for i in xrange(self.Sensor.numRays): ray = self.Sensor.rays[:, i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) #print "rayTransformed is", rayTransformed intersection = self.Sensor.raycast( self.locator, origin, origin + rayTransformed * self.Sensor.rayLength) if intersection is not None: d.addLine(origin, intersection, color=[1, 0, 0]) else: d.addLine(origin, origin + rayTransformed * self.Sensor.rayLength, color=colorMaxRange) vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
def update(self, robotModel): name = 'camera frustum %s' % self.robotModel.getProperty('Name') obj = om.findObjectByName(name) if obj and not obj.getProperty('Visible'): return vis.updatePolyData(self.getCameraFrustumGeometry(self.rayLength), name, parent=self.robotModel, visible=False)
def updatePointcloudSnapshot(self): if (self.useLidar is True): return vis.updatePolyData(segmentation.getCurrentRevolutionData(), 'pointcloud snapshot', parent='segmentation') else: return vis.updatePolyData(segmentation.getDisparityPointCloud(4), 'pointcloud snapshot', parent='segmentation')
def updatePointcloudSnapshot(self): if (self.useLidar is True): return vis.updatePolyData(segmentation.getCurrentRevolutionData(), 'pointcloud snapshot', parent='segmentation') else: return vis.updatePolyData(segmentation.getDisparityPointCloud(4), 'pointcloud snapshot', parent='segmentation')
def update(self, robotModel): name = 'camera frustum %s' % self.robotModel.getProperty('Name') obj = om.findObjectByName(name) if obj and not obj.getProperty('Visible'): return vis.updatePolyData(self.getCameraFrustumGeometry(self.rayLength), name, parent=self.robotModel, visible=False)
def createSpheres(self, sensorValues): d = DebugData() for key in sensorValues.keys(): frame, pos, rpy = self.sensorLocations[key] t = transformUtils.frameFromPositionAndRPY(pos, rpy) t.PostMultiply() t.Concatenate(self.frames[frame]) d.addSphere(t.GetPosition(), radius=0.005, color=self.getColor(sensorValues[key], key), resolution=8) vis.updatePolyData(d.getPolyData(), self.name, colorByName='RGB255')
def doFTDraw(self, unusedrobotstate): frames = [] fts = [] vis_names = [] if (hasattr(self.robotStateJointController, 'lastRobotStateMessage') and self.robotStateJointController.lastRobotStateMessage): if self.draw_right: rft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_force + self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_torque) rft[0] = -1.*rft[0] # right FT looks to be rotated 180deg around the z axis rft[1] = -1.*rft[1] rft[3] = -1.*rft[3] rft[4] = -1.*rft[4] rft -= self.ft_right_bias fts.append(rft) frames.append(self.robotStateModel.getLinkFrame('r_hand_force_torque')) vis_names.append('ft_right') if self.draw_left: lft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_force + self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_torque) lft -= self.ft_left_bias fts.append(lft) frames.append(self.robotStateModel.getLinkFrame('l_hand_force_torque')) vis_names.append('ft_left') for i in range(0, len(frames)): frame = frames[i] ft = fts[i] offset = [0., 0., 0.] p1 = frame.TransformPoint(offset) scale = 1.0/25.0 # todo add slider for this? scalet = 1.0 / 2.5 p2f = frame.TransformPoint(offset + ft[0:3]*scale) p2t = frame.TransformPoint(offset + ft[3:6]) normalt = (np.array(p2t) - np.array(p1)) normalt = normalt / np.linalg.norm(normalt) d = DebugData() # force if np.linalg.norm(np.array(p2f) - np.array(p1)) < 0.1: d.addLine(p1, p2f, color=[1.0, 0.0, 0.0]) else: d.addArrow(p1, p2f, color=[1.,0.,0.]) # torque if self.draw_torque: d.addCircle(p1, normalt, scalet*np.linalg.norm(ft[3:6])) # frame (largely for debug) vis.updateFrame(frame, vis_names[i]+'frame', view=self.view, parent='wristft', visible=False, scale=0.2) vis.updatePolyData(d.getPolyData(), vis_names[i], view=self.view, parent='wristft')
def fitSwitchBox(self, polyData, points): boxPosition = points[0] wallPoint = points[1] # find a frame that is aligned with wall searchRadius = 0.2 planePoints, normal = segmentation.applyLocalPlaneFit(polyData, points[0], searchRadius=np.linalg.norm(points[1] - points[0]), searchRadiusEnd=1.0) obj = vis.updatePolyData(planePoints, 'wall plane points', color=[0,1,0], visible=False) obj.setProperty('Point Size', 7) viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection() if np.dot(normal, viewDirection) < 0: normal = -normal origin = segmentation.computeCentroid(planePoints) zaxis = [0,0,1] xaxis = normal yaxis = np.cross(zaxis, xaxis) yaxis /= np.linalg.norm(yaxis) zaxis = np.cross(xaxis, yaxis) zaxis /= np.linalg.norm(zaxis) t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis) # translate that frame to the box position t.PostMultiply() t.Translate(boxPosition) boxFrame = transformUtils.copyFrame(t) self.switchPlanner.spawnBoxAffordanceAtFrame(boxFrame)
def drawCenterOfMass(model): stanceFrame = footstepsDriver.getFeetMidPoint(model) com = list(model.model.getCenterOfMass()) com[2] = stanceFrame.GetPosition()[2] d = DebugData() d.addSphere(com, radius=0.015) obj = vis.updatePolyData(d.getPolyData(), 'COM %s' % model.getProperty('Name'), color=[1,0,0], visible=False, parent=model)
def draw(self): d = DebugData() points = list(self.points) if self.hoverPos is not None: points.append(self.hoverPos) # draw points for p in points: d.addSphere(p, radius=5) if self.drawLines and len(points) > 1: for a, b in zip(points, points[1:]): d.addLine(a, b) # connect end points # d.addLine(points[0], points[-1]) if self.annotationObj: self.annotationObj.setPolyData(d.getPolyData()) else: self.annotationObj = vis.updatePolyData(d.getPolyData(), 'annotation', parent='segmentation', color=[1,0,0], view=self.view) self.annotationObj.addToView(self.view) self.annotationObj.actor.SetPickable(False) self.annotationObj.actor.GetProperty().SetLineWidth(2)
def fitSwitchBox(self, polyData, points): boxPosition = points[0] wallPoint = points[1] # find a frame that is aligned with wall searchRadius = 0.2 planePoints, normal = segmentation.applyLocalPlaneFit(polyData, points[0], searchRadius=np.linalg.norm(points[1] - points[0]), searchRadiusEnd=1.0) obj = vis.updatePolyData(planePoints, 'wall plane points', color=[0,1,0], visible=False) obj.setProperty('Point Size', 7) viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection() if np.dot(normal, viewDirection) < 0: normal = -normal origin = segmentation.computeCentroid(planePoints) zaxis = [0,0,1] xaxis = normal yaxis = np.cross(zaxis, xaxis) yaxis /= np.linalg.norm(yaxis) zaxis = np.cross(xaxis, yaxis) zaxis /= np.linalg.norm(zaxis) t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis) # translate that frame to the box position t.PostMultiply() t.Translate(boxPosition) boxFrame = transformUtils.copyFrame(t) self.switchPlanner.spawnBoxAffordanceAtFrame(boxFrame)
def updatePlanFrames(self): if self.getViewMode() != 'frames': return numberOfSamples = self.getNumberOfSamples() meshes = self.planPlayback.getPlanPoseMeshes( self.plan, self.playbackJointController, self.playbackRobotModel, numberOfSamples) d = DebugData() startColor = [0.8, 0.8, 0.8] endColor = [85 / 255.0, 255 / 255.0, 255 / 255.0] colorFunc = scipy.interpolate.interp1d([0, numberOfSamples - 1], [startColor, endColor], axis=0, kind='slinear') for i, mesh in reversed(list(enumerate(meshes))): d.addPolyData(mesh, color=colorFunc(i)) pd = d.getPolyData() clean = vtk.vtkCleanPolyData() clean.SetInput(pd) clean.Update() pd = clean.GetOutput() self.planFramesObj = vis.updatePolyData(d.getPolyData(), 'robot plan', alpha=1.0, visible=False, colorByName='RGB255', parent='planning') self.showPlanFrames()
def updatePlanFrames(self): if self.getViewMode() != 'frames': return numberOfSamples = self.getNumberOfSamples() meshes = self.planPlayback.getPlanPoseMeshes(self.plan, self.playbackJointController, self.playbackRobotModel, numberOfSamples) d = DebugData() startColor = [0.8, 0.8, 0.8] endColor = [85/255.0, 255/255.0, 255/255.0] colorFunc = scipy.interpolate.interp1d([0, numberOfSamples-1], [startColor, endColor], axis=0, kind='slinear') for i, mesh in reversed(list(enumerate(meshes))): d.addPolyData(mesh, color=colorFunc(i)) pd = d.getPolyData() clean = vtk.vtkCleanPolyData() clean.SetInput(pd) clean.Update() pd = clean.GetOutput() self.planFramesObj = vis.updatePolyData(d.getPolyData(), 'robot plan', alpha=1.0, visible=False, colorByName='RGB255', parent='planning') self.showPlanFrames()
def draw(self): d = DebugData() points = list(self.points) if self.hoverPos is not None: points.append(self.hoverPos) # draw points for p in points: d.addSphere(p, radius=5) if self.drawLines and len(points) > 1: for a, b in zip(points, points[1:]): d.addLine(a, b) # connect end points # d.addLine(points[0], points[-1]) if self.annotationObj: self.annotationObj.setPolyData(d.getPolyData()) else: self.annotationObj = vis.updatePolyData(d.getPolyData(), 'annotation', parent='segmentation', color=[1, 0, 0], view=self.view) self.annotationObj.addToView(self.view) self.annotationObj.actor.SetPickable(False) self.annotationObj.actor.GetProperty().SetLineWidth(2)
def draw(self): d = DebugData() points = [p if p is not None else self.hoverPos for p in self.points] # draw points for p in points: if p is not None: d.addSphere(p, radius=0.008) if self.drawLines: # draw lines for a, b in zip(points, points[1:]): if b is not None: d.addLine(a, b) # connect end points if points[-1] is not None: d.addLine(points[0], points[-1]) self.annotationObj = vis.updatePolyData(d.getPolyData(), self.annotationName, parent=self.annotationFolder) self.annotationObj.setProperty('Color', [1, 0, 0]) self.annotationObj.actor.SetPickable(False)
def rayDebug(position, ray): d = DebugData() d.addLine(position, position + ray * 5.0) drcView = app.getViewManager().findView('DRC View') obj = vis.updatePolyData(d.getPolyData(), 'camera ray', view=drcView, color=[0, 1, 0]) obj.actor.GetProperty().SetLineWidth(2)
def updateDrawIntersection(frame): origin = np.array(frame.transform.GetPosition()) #print "origin is now at", origin d = DebugData() for i in xrange(numRays): ray = rays[:,i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) #print "rayTransformed is", rayTransformed intersection = computeIntersection(locator, origin, origin + rayTransformed*rayLength) if intersection is not None: d.addLine(origin, intersection, color=[1,0,0]) else: d.addLine(origin, origin+rayTransformed*rayLength, color=[0,1,0]) vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
def setupObjectInCamera(self, obj): imageView = self.imageView obj = vis.updatePolyData(vtk.vtkPolyData(), self.getTransformedName(obj), view=imageView.view, color=obj.getProperty('Color'), parent=self.getFolderName(), visible=obj.getProperty('Visible')) self.addActorToImageOverlay(obj, imageView) return obj
def loadMap(self, filename, transform=None): print filename pd = io.readPolyData(filename) if transform is not None: pd = filterUtils.transformPolyData(pd, self.cameraToLocalInit) pdi = vis.updatePolyData(pd, 'map') try: pdi.setProperty('Color By', 'rgb_colors') except Exception, e: print "Could not set color to RGB - not an element" #raise e
def updateDrawIntersection(self, frame): origin = np.array(frame.transform.GetPosition()) #print "origin is now at", origin d = DebugData() colorMaxRange = [0,1,0] for i in xrange(self.sensor.numRays): ray = self.sensor.rays[:,i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) #print "rayTransformed is", rayTransformed intersection = self.sensor.raycast(self.locator, origin, origin + rayTransformed*self.sensor.rayLength) if intersection is not None: d.addLine(origin, intersection, color=[1,0,0]) else: d.addLine(origin, origin+rayTransformed*self.sensor.rayLength, color=colorMaxRange) vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
def onShowMapButton(self): vis.updateFrame(self.cameraToLocalInit, 'initial cam' ) filename = os.path.expanduser('~/Kinect_Logs/Kintinuous.pcd') print filename pd = io.readPolyData(filename) pd = filterUtils.transformPolyData(pd, self.cameraToLocalInit ) #t = transformUtils.frameFromPositionAndRPY([0,0,0],[-90,0,-90]) #pd = filterUtils.transformPolyData(pd , t) pdi = vis.updatePolyData(pd,'map') pdi.setProperty('Color By', 'rgb_colors')
def updateDrawIntersection(frame): origin = np.array(frame.transform.GetPosition()) #print "origin is now at", origin d = DebugData() for i in xrange(numRays): ray = rays[:, i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) #print "rayTransformed is", rayTransformed intersection = computeIntersection(locator, origin, origin + rayTransformed * rayLength) if intersection is not None: d.addLine(origin, intersection, color=[1, 0, 0]) else: d.addLine(origin, origin + rayTransformed * rayLength, color=[0, 1, 0]) vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
def removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[-np.inf, -0.03], whichAxis=1, whichAxisLetter='y', percentile = 95): yvalues = vnp.getNumpyFromVtk(polyData, 'Points')[:, whichAxis] backY = np.percentile(yvalues, percentile) if ( percentile > 50): searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [backY - 0.1, np.inf]) else: searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [-np.inf, backY + 0.1]) vis.updatePolyData(searchRegion, 'search region', parent="segmentation", colorByName=whichAxisLetter, visible=False) # find the plane of the back wall, remove it and the points behind it: _, origin, normal = segmentation.applyPlaneFit(searchRegion, distanceThreshold=0.02, expectedNormal=expectedNormal, perpendicularAxis=expectedNormal, returnOrigin=True) points = vnp.getNumpyFromVtk(polyData, 'Points') dist = np.dot(points - origin, normal) vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane') backFrame = transformUtils.getTransformFromOriginAndNormal(origin, normal, normalAxis=2) vis.updateFrame(backFrame, 'back frame', parent='segmentation', scale=0.15 , visible=False) vis.updatePolyData(polyData, 'dist to back', parent='segmentation', visible=False) polyData = segmentation.thresholdPoints(polyData, 'dist_to_plane', filterRange) vis.updatePolyData(polyData, 'back off and all', parent='segmentation', visible=False) return polyData
def fitObjectsOnShelf(polyData, maxHeight = 0.25): # find the shelf plane: polyDataWithoutFront, _ = segmentation.removeMajorPlane(polyData, distanceThreshold=0.02) polyDataPlaneFit, origin, normal = segmentation.applyPlaneFit(polyDataWithoutFront, expectedNormal=np.array([0.0,0.0,1.0]), perpendicularAxis=np.array([0.0,0.0,1.0]), returnOrigin=True) vis.updatePolyData(polyDataPlaneFit, 'polyDataPlaneFit', parent='segmentation', visible=False) shelfSurfacePoints = segmentation.thresholdPoints(polyDataPlaneFit, 'dist_to_plane', [-0.01, 0.01]) shelfCenter = segmentation.computeCentroid(shelfSurfacePoints) shelfFrame = transformUtils.getTransformFromOriginAndNormal(shelfCenter, normal, normalAxis=2) vis.showFrame(shelfFrame, 'shelfFrame', parent='segmentation', scale=0.15 , visible=False) # find the points near to the shelf plane and find objects on it: points = vnp.getNumpyFromVtk(polyData, 'Points') dist = np.dot(points - origin, normal) vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane') shelfPoints = segmentation.thresholdPoints(polyData, 'dist_to_plane', [-0.01, maxHeight]) vis.updatePolyData(shelfPoints, 'shelf', parent='segmentation', visible=False) data = segmentation.segmentTableScene(shelfPoints, shelfCenter, filterClustering = False ) vis.showClusterObjects(data.clusters + [data.table], parent='segmentation') # remove the points that we considered from the orginal cloud dists = vnp.getNumpyFromVtk(polyData, 'dist_to_plane') diffShelf = ( ((dists > maxHeight) + (dists < -0.01))) + 0.1 -0.1 vnp.addNumpyToVtk(polyData, diffShelf, 'diff_shelf') polyData = segmentation.thresholdPoints(polyData, 'diff_shelf', [1, 1]) vis.updatePolyData(polyData, 'rest', parent='segmentation', visible=False) return polyData
def fitDrillBarrel ( drillPoints, forwardDirection, plane_origin, plane_normal): ''' Given a point cloud which ONLY contains points from a barrell drill, standing upright and the equations of a table its resting on, and the general direction of the robot Fit a barrell drill ''' if not drillPoints.GetNumberOfPoints(): return vis.updatePolyData(drillPoints, 'drill cluster', parent=getDebugFolder(), visible=False) drillBarrelPoints = thresholdPoints(drillPoints, 'dist_to_plane', [0.177, 0.30]) if not drillBarrelPoints.GetNumberOfPoints(): return # fit line to drill barrel points linePoint, lineDirection, _ = applyLineFit(drillBarrelPoints, distanceThreshold=0.5) if np.dot(lineDirection, forwardDirection) < 0: lineDirection = -lineDirection vis.updatePolyData(drillBarrelPoints, 'drill barrel points', parent=getDebugFolder(), visible=False) pts = vtkNumpy.getNumpyFromVtk(drillBarrelPoints, 'Points') dists = np.dot(pts-linePoint, lineDirection) p1 = linePoint + lineDirection*np.min(dists) p2 = linePoint + lineDirection*np.max(dists) p1 = projectPointToPlane(p1, plane_origin, plane_normal) p2 = projectPointToPlane(p2, plane_origin, plane_normal) d = DebugData() d.addSphere(p1, radius=0.01) d.addSphere(p2, radius=0.01) d.addLine(p1, p2) vis.updatePolyData(d.getPolyData(), 'drill debug points', color=[0,1,0], parent=getDebugFolder(), visible=False) drillToBasePoint = np.array([-0.07, 0.0 , -0.12]) zaxis = plane_normal xaxis = lineDirection xaxis /= np.linalg.norm(xaxis) yaxis = np.cross(zaxis, xaxis) yaxis /= np.linalg.norm(yaxis) xaxis = np.cross(yaxis, zaxis) xaxis /= np.linalg.norm(xaxis) t = getTransformFromAxes(xaxis, yaxis, zaxis) t.PreMultiply() t.Translate(-drillToBasePoint) t.PostMultiply() t.Translate(p1) return t
def __init__(self, robotSystem, view): self.robotStateModel = robotSystem.robotStateModel self.robotStateJointController = robotSystem.robotStateJointController self.robotSystem = robotSystem self.lFootFtFrameId = self.robotStateModel.model.findLinkID( self.robotSystem.ikPlanner.leftFootLink) self.rFootFtFrameId = self.robotStateModel.model.findLinkID( self.robotSystem.ikPlanner.rightFootLink) self.leftInContact = 0 self.rightInContact = 0 self.view = view self.ddDrakeWrapper = PythonQt.dd.ddDrakeWrapper() self.warningButton = QtGui.QLabel('COP Warning') self.warningButton.setStyleSheet("background-color:white") self.dialogVisible = False d = DebugData() vis.updatePolyData(d.getPolyData(), 'measured cop', view=self.view, parent='robot state model') om.findObjectByName('measured cop').setProperty('Visible', False) self.robotStateModel.connectModelChanged(self.update)
def updateCursor(self, displayPoint): center = self.displayPointToImagePoint(displayPoint, restrictToImageDimensions=False) center = np.array(center) d = DebugData() d.addLine(center + [0, -3000, 0], center + [0, 3000, 0]) d.addLine(center + [-3000, 0, 0], center + [3000, 0, 0]) self.cursorObj = vis.updatePolyData(d.getPolyData(), 'cursor', alpha=0.5, view=self.view) self.cursorObj.addToView(self.view) self.cursorObj.actor.SetUseBounds(False) self.cursorObj.actor.SetPickable(False) self.view.render()
def getRecedingTerrainRegion(self, polyData, linkFrame): ''' Find the point cloud in front of the foot frame''' #polyData = shallowCopy(polyData) points = vtkNumpy.getNumpyFromVtk(polyData, 'Points') #vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x') #vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y') #vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z') viewOrigin = linkFrame.TransformPoint([0.0, 0.0, 0.0]) viewX = linkFrame.TransformVector([1.0, 0.0, 0.0]) viewY = linkFrame.TransformVector([0.0, 1.0, 0.0]) viewZ = linkFrame.TransformVector([0.0, 0.0, 1.0]) polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x') polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y') polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z') polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_x', [0.12, 1.6]) polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_y', [-0.4, 0.4]) polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_z', [-0.4, 0.4]) vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='cont debug', visible=True) return polyData
def getRecedingTerrainRegion(self, polyData, linkFrame): ''' Find the point cloud in front of the foot frame''' #polyData = shallowCopy(polyData) points = vtkNumpy.getNumpyFromVtk(polyData, 'Points') #vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x') #vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y') #vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z') viewOrigin = linkFrame.TransformPoint([0.0, 0.0, 0.0]) viewX = linkFrame.TransformVector([1.0, 0.0, 0.0]) viewY = linkFrame.TransformVector([0.0, 1.0, 0.0]) viewZ = linkFrame.TransformVector([0.0, 0.0, 1.0]) polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x') polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y') polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z') polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_x', [0.12, 1.6]) polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_y', [-0.4, 0.4]) polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_z', [-0.4, 0.4]) vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='cont debug', visible=True) return polyData
def removePlaneAndBeyond(polyData, expectedNormal=[1, 0, 0], filterRange=[-np.inf, -0.03], whichAxis=1, whichAxisLetter='y', percentile=95): yvalues = vnp.getNumpyFromVtk(polyData, 'Points')[:, whichAxis] backY = np.percentile(yvalues, percentile) if (percentile > 50): searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [backY - 0.1, np.inf]) else: searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [-np.inf, backY + 0.1]) vis.updatePolyData(searchRegion, 'search region', parent="segmentation", colorByName=whichAxisLetter, visible=False) # find the plane of the back wall, remove it and the points behind it: _, origin, normal = segmentation.applyPlaneFit( searchRegion, distanceThreshold=0.02, expectedNormal=expectedNormal, perpendicularAxis=expectedNormal, returnOrigin=True) points = vnp.getNumpyFromVtk(polyData, 'Points') dist = np.dot(points - origin, normal) vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane') backFrame = transformUtils.getTransformFromOriginAndNormal(origin, normal, normalAxis=2) vis.updateFrame(backFrame, 'back frame', parent='segmentation', scale=0.15, visible=False) vis.updatePolyData(polyData, 'dist to back', parent='segmentation', visible=False) polyData = segmentation.thresholdPoints(polyData, 'dist_to_plane', filterRange) vis.updatePolyData(polyData, 'back off and all', parent='segmentation', visible=False) return polyData
def updateCursor(self, displayPoint): center = self.displayPointToImagePoint(displayPoint, restrictToImageDimensions=False) center = np.array(center) d = DebugData() d.addLine(center + [0, -3000, 0], center + [0, 3000, 0]) d.addLine(center + [-3000, 0, 0], center + [3000, 0, 0]) self.cursorObj = vis.updatePolyData(d.getPolyData(), 'cursor', alpha=0.5, view=self.view) self.cursorObj.addToView(self.view) self.cursorObj.actor.SetUseBounds(False) self.cursorObj.actor.SetPickable(False) self.view.render()
def onAtlasStepParams(self,msg): if (msg.desired_step_spec.foot_index ==1): is_right_foot = True else: is_right_foot = False #print msg.desired_step_spec.foot_index , " and " , is_right_foot foot = msg.desired_step_spec.foot footTransform = transformUtils.frameFromPositionAndRPY( foot.position , [0, 0, foot.yaw*180/math.pi]) mesh,color = self.getMeshAndColor(is_right_foot) #color[1] = 0.75 ; color[2] = 0.25 vis.updateFrame(footTransform, 'bdi foot', parent='foot placements', scale=0.2, visible=False) #bdi_step_mesh = om.findObjectByName('bdi step') #om.removeFromObjectModel(bdi_step_mesh) obj = vis.updatePolyData(mesh, 'bdi step', color=color, alpha=1.0, parent='foot placements', visible=False) obj.setProperty('Color',QtGui.QColor(color[0]*255.0,color[1]*255.0,color[2]*255.0)) obj.actor.SetUserTransform(footTransform)
def onAtlasStepParams(self,msg): if (msg.desired_step_spec.foot_index ==1): is_right_foot = True else: is_right_foot = False #print msg.desired_step_spec.foot_index , " and " , is_right_foot foot = msg.desired_step_spec.foot footTransform = transformUtils.frameFromPositionAndRPY( foot.position , [0, 0, foot.yaw*180/math.pi]) mesh,color = self.getMeshAndColor(is_right_foot) #color[1] = 0.75 ; color[2] = 0.25 vis.updateFrame(footTransform, 'bdi foot', parent='foot placements', scale=0.2, visible=False) #bdi_step_mesh = om.findObjectByName('bdi step') #om.removeFromObjectModel(bdi_step_mesh) obj = vis.updatePolyData(mesh, 'bdi step', color=color, alpha=1.0, parent='foot placements', visible=False) obj.setProperty('Color',QtGui.QColor(color[0]*255.0,color[1]*255.0,color[2]*255.0)) obj.actor.SetUserTransform(footTransform)
def fitObjectsOnShelf(polyData, maxHeight=0.25): # find the shelf plane: polyDataWithoutFront, _ = segmentation.removeMajorPlane( polyData, distanceThreshold=0.02) polyDataPlaneFit, origin, normal = segmentation.applyPlaneFit( polyDataWithoutFront, expectedNormal=np.array([0.0, 0.0, 1.0]), perpendicularAxis=np.array([0.0, 0.0, 1.0]), returnOrigin=True) vis.updatePolyData(polyDataPlaneFit, 'polyDataPlaneFit', parent='segmentation', visible=False) shelfSurfacePoints = segmentation.thresholdPoints(polyDataPlaneFit, 'dist_to_plane', [-0.01, 0.01]) shelfCenter = segmentation.computeCentroid(shelfSurfacePoints) shelfFrame = transformUtils.getTransformFromOriginAndNormal(shelfCenter, normal, normalAxis=2) vis.showFrame(shelfFrame, 'shelfFrame', parent='segmentation', scale=0.15, visible=False) # find the points near to the shelf plane and find objects on it: points = vnp.getNumpyFromVtk(polyData, 'Points') dist = np.dot(points - origin, normal) vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane') shelfPoints = segmentation.thresholdPoints(polyData, 'dist_to_plane', [-0.01, maxHeight]) vis.updatePolyData(shelfPoints, 'shelf', parent='segmentation', visible=False) data = segmentation.segmentTableScene(shelfPoints, shelfCenter, filterClustering=False) vis.showClusterObjects(data.clusters + [data.table], parent='segmentation') # remove the points that we considered from the orginal cloud dists = vnp.getNumpyFromVtk(polyData, 'dist_to_plane') diffShelf = (((dists > maxHeight) + (dists < -0.01))) + 0.1 - 0.1 vnp.addNumpyToVtk(polyData, diffShelf, 'diff_shelf') polyData = segmentation.thresholdPoints(polyData, 'diff_shelf', [1, 1]) vis.updatePolyData(polyData, 'rest', parent='segmentation', visible=False) return polyData
def onMouseMove(self, displayPoint, modifiers=None): om.removeFromObjectModel(om.findObjectByName('link selection')) self.linkName = None self.pickedPoint = None selection = self.getSelection(displayPoint) if selection is None: return pickedPoint, linkName, normal = selection d = DebugData() d.addSphere(pickedPoint, radius=0.01) d.addLine(pickedPoint, np.array(pickedPoint) + 0.1 * np.array(normal), radius=0.005) obj = vis.updatePolyData(d.getPolyData(), 'link selection', color=[0,1,0]) obj.actor.SetPickable(False) self.linkName = linkName self.pickedPoint = pickedPoint
def onMouseMove(self, displayPoint, modifiers=None): om.removeFromObjectModel(om.findObjectByName('link selection')) self.linkName = None self.pickedPoint = None selection = self.getSelection(displayPoint) if selection is None: return pickedPoint, linkName, normal = selection d = DebugData() d.addSphere(pickedPoint, radius=0.01) d.addLine(pickedPoint, np.array(pickedPoint) + 0.1 * np.array(normal), radius=0.005) obj = vis.updatePolyData(d.getPolyData(), 'link selection', color=[0,1,0]) obj.actor.SetPickable(False) self.linkName = linkName self.pickedPoint = pickedPoint
def draw(self): d = DebugData() points = [p if p is not None else self.hoverPos for p in self.points] # draw points for p in points: if p is not None: d.addSphere(p, radius=0.008) if self.drawLines: # draw lines for a, b in zip(points, points[1:]): if b is not None: d.addLine(a, b) # connect end points if points[-1] is not None: d.addLine(points[0], points[-1]) self.annotationObj = vis.updatePolyData(d.getPolyData(), self.annotationName, parent=self.annotationFolder) self.annotationObj.setProperty('Color', [1,0,0]) self.annotationObj.actor.SetPickable(False)
def fitDrillBarrel(drillPoints, forwardDirection, plane_origin, plane_normal): ''' Given a point cloud which ONLY contains points from a barrell drill, standing upright and the equations of a table its resting on, and the general direction of the robot Fit a barrell drill ''' if not drillPoints.GetNumberOfPoints(): return vis.updatePolyData(drillPoints, 'drill cluster', parent=getDebugFolder(), visible=False) drillBarrelPoints = thresholdPoints(drillPoints, 'dist_to_plane', [0.177, 0.30]) if not drillBarrelPoints.GetNumberOfPoints(): return # fit line to drill barrel points linePoint, lineDirection, _ = applyLineFit(drillBarrelPoints, distanceThreshold=0.5) if np.dot(lineDirection, forwardDirection) < 0: lineDirection = -lineDirection vis.updatePolyData(drillBarrelPoints, 'drill barrel points', parent=getDebugFolder(), visible=False) pts = vtkNumpy.getNumpyFromVtk(drillBarrelPoints, 'Points') dists = np.dot(pts - linePoint, lineDirection) p1 = linePoint + lineDirection * np.min(dists) p2 = linePoint + lineDirection * np.max(dists) p1 = projectPointToPlane(p1, plane_origin, plane_normal) p2 = projectPointToPlane(p2, plane_origin, plane_normal) d = DebugData() d.addSphere(p1, radius=0.01) d.addSphere(p2, radius=0.01) d.addLine(p1, p2) vis.updatePolyData(d.getPolyData(), 'drill debug points', color=[0, 1, 0], parent=getDebugFolder(), visible=False) drillToBasePoint = np.array([-0.07, 0.0, -0.12]) zaxis = plane_normal xaxis = lineDirection xaxis /= np.linalg.norm(xaxis) yaxis = np.cross(zaxis, xaxis) yaxis /= np.linalg.norm(yaxis) xaxis = np.cross(yaxis, zaxis) xaxis /= np.linalg.norm(xaxis) t = getTransformFromAxes(xaxis, yaxis, zaxis) t.PreMultiply() t.Translate(-drillToBasePoint) t.PostMultiply() t.Translate(p1) return t
def rayDebug(position, ray): d = DebugData() d.addLine(position, position+ray*5.0) drcView = app.getViewManager().findView('DRC View') obj = vis.updatePolyData(d.getPolyData(), 'camera ray', view=drcView, color=[0,1,0]) obj.actor.GetProperty().SetLineWidth(2)
def update(self, unused): if (om.findObjectByName('measured cop').getProperty('Visible') and self.robotStateJointController.lastRobotStateMessage): if self.dialogVisible == False: self.warningButton.setVisible(True) app.getMainWindow().statusBar().insertPermanentWidget( 0, self.warningButton) self.dialogVisible = True self.leftInContact = self.robotStateJointController.lastRobotStateMessage.force_torque.l_foot_force_z > 500 self.rightInContact = self.robotStateJointController.lastRobotStateMessage.force_torque.r_foot_force_z > 500 if self.rightInContact or self.leftInContact: lFootFt = [ self.robotStateJointController.lastRobotStateMessage. force_torque.l_foot_torque_x, self.robotStateJointController.lastRobotStateMessage. force_torque.l_foot_torque_y, 0.0, 0.0, 0.0, self.robotStateJointController.lastRobotStateMessage. force_torque.l_foot_force_z ] rFootFt = [ self.robotStateJointController.lastRobotStateMessage. force_torque.r_foot_torque_x, self.robotStateJointController.lastRobotStateMessage. force_torque.r_foot_torque_y, 0.0, 0.0, 0.0, self.robotStateJointController.lastRobotStateMessage. force_torque.r_foot_force_z ] lFootTransform = self.robotStateModel.getLinkFrame( self.robotSystem.ikPlanner.leftFootLink) rFootTransform = self.robotStateModel.getLinkFrame( self.robotSystem.ikPlanner.rightFootLink) rFootOrigin = np.array( rFootTransform.TransformPoint([0, 0, -0.07645])) lFootOrigin = np.array( lFootTransform.TransformPoint([0, 0, -0.07645])) # down to sole measured_cop = self.ddDrakeWrapper.resolveCenterOfPressure( self.robotStateModel.model, [self.lFootFtFrameId, self.rFootFtFrameId], lFootFt + rFootFt, [0., 0., 1.], (self.rightInContact * rFootOrigin + self.leftInContact * lFootOrigin) / (self.leftInContact + self.rightInContact)) allFootContacts = np.empty([0, 2]) if self.rightInContact: rFootContacts = np.array([ rFootTransform.TransformPoint(contact_point) for contact_point in self.LONG_FOOT_CONTACT_POINTS ]) allFootContacts = np.concatenate( (allFootContacts, rFootContacts[:, 0:2])) if self.leftInContact: lFootContacts = np.array([ lFootTransform.TransformPoint(contact_point) for contact_point in self.LONG_FOOT_CONTACT_POINTS ]) allFootContacts = np.concatenate( (allFootContacts, lFootContacts[:, 0:2])) num_pts = allFootContacts.shape[0] dist = self.ddDrakeWrapper.drakeSignedDistanceInsideConvexHull( num_pts, allFootContacts.reshape(num_pts * 2, 1), measured_cop[0:2]) inSafeSupportPolygon = dist >= 0 # map dist to color -- green if inside threshold, red if not dist = min(max(0, dist), self.DESIRED_INTERIOR_DISTANCE ) / self.DESIRED_INTERIOR_DISTANCE # nonlinear interpolation here to try to maintain saturation r = int(255. - 255. * dist**4) g = int(255. * dist**0.25) b = 0 colorStatus = [r / 255., g / 255., b / 255.] colorStatusString = 'rgb(%d, %d, %d)' % (r, g, b) self.warningButton.setStyleSheet("background-color:" + colorStatusString) d = DebugData() d.addSphere(measured_cop[0:3], radius=0.02) vis.updatePolyData(d.getPolyData(), 'measured cop', view=self.view, parent='robot state model').setProperty( 'Color', colorStatus) elif self.dialogVisible: app.getMainWindow().statusBar().removeWidget(self.warningButton) self.dialogVisible = False
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) if app.getTestingInteractiveEnabled(): view.show() app.showObjectModel() app.start()
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 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 setupObjectInCamera(self, obj): imageView = self.imageView obj = vis.updatePolyData(vtk.vtkPolyData(), self.getTransformedName(obj), view=imageView.view, color=obj.getProperty('Color'), parent=self.getFolderName(), visible=obj.getProperty('Visible')) self.addActorToImageOverlay(obj, imageView) return obj
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) if app.getTestingInteractiveEnabled(): view.show() app.showObjectModel() app.start()