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 = [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 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 main(): global app, view, nav_data nav_data = np.array([[0, 0, 0]]) lcmUtils.addSubscriber(".*_NAV$", node_nav_t, handleNavData) app = ConsoleApp() app.setupGlobals(globals()) app.showPythonConsole() view = app.createView() view.show() global d d = DebugData() d.addLine([0, 0, 0], [1, 0, 0], color=[0, 1, 0]) d.addSphere((0, 0, 0), radius=0.02, color=[1, 0, 0]) #vis.showPolyData(d.getPolyData(), 'my debug geometry', colorByName='RGB255') startSwarmVisualization() app.start()
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 main(): global app, view, nav_data nav_data = np.array([[0, 0, 0]]) lcmUtils.addSubscriber(".*_NAV$", node_nav_t, handleNavData) app = ConsoleApp() app.setupGlobals(globals()) app.showPythonConsole() view = app.createView() view.show() global d d = DebugData() d.addLine([0,0,0], [1,0,0], color=[0,1,0]) d.addSphere((0,0,0), radius=0.02, color=[1,0,0]) #vis.showPolyData(d.getPolyData(), 'my debug geometry', colorByName='RGB255') startSwarmVisualization() app.start()
def onNewWalkingGoal(self, walkingGoal=None): walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel) frameObj = vis.updateFrame(walkingGoal, 'walking goal', parent='planning', scale=0.25) frameObj.setProperty('Edit', True) rep = frameObj.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) frameObj.widget.HandleRotationEnabledOff() if self.placer: self.placer.stop() terrain = om.findObjectByName('HEIGHT_MAP_SCENE') if terrain: pos = np.array(frameObj.transform.GetPosition()) polyData = filterUtils.removeNonFinitePoints(terrain.polyData) if polyData.GetNumberOfPoints(): polyData = segmentation.labelDistanceToLine( polyData, pos, pos + [0, 0, 1]) polyData = segmentation.thresholdPoints( polyData, 'distance_to_line', [0.0, 0.1]) if polyData.GetNumberOfPoints(): pos[2] = np.nanmax( vnp.getNumpyFromVtk(polyData, 'Points')[:, 2]) frameObj.transform.Translate( pos - np.array(frameObj.transform.GetPosition())) d = DebugData() d.addSphere((0, 0, 0), radius=0.03) handle = vis.showPolyData(d.getPolyData(), 'walking goal terrain handle', parent=frameObj, visible=True, color=[1, 1, 0]) handle.actor.SetUserTransform(frameObj.transform) self.placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain) def onFramePropertyModified(propertySet, propertyName): if propertyName == 'Edit': if propertySet.getProperty(propertyName): self.placer.start() else: self.placer.stop() frameObj.properties.connectPropertyChanged(onFramePropertyModified) onFramePropertyModified(frameObj, 'Edit') frameObj.connectFrameModified(self.onWalkingGoalModified) self.onWalkingGoalModified(frameObj)
def drawContactPts(self, obj, footstep, **kwargs): leftPoints, rightPoints = FootstepsDriver.getContactPts(footstep.params.support_contact_groups) contact_pts = rightPoints if footstep.is_right_foot else leftPoints d = DebugData() for pt in contact_pts: d.addSphere(pt, radius=0.01) d_obj = vis.showPolyData(d.getPolyData(), "contact points", parent=obj, **kwargs) d_obj.actor.SetUserTransform(obj.actor.GetUserTransform())
def drawContactPts(self, obj, footstep, **kwargs): if footstep.is_right_foot: _, contact_pts = FootstepsDriver.getContactPts() else: contact_pts, _ = FootstepsDriver.getContactPts() d = DebugData() for pt in contact_pts: d.addSphere(pt, radius=0.01) d_obj = vis.showPolyData(d.getPolyData(), "contact points", parent=obj, **kwargs) d_obj.actor.SetUserTransform(obj.actor.GetUserTransform())
def showBoardDebug(affs=None): referenceFrame = vtk.vtkTransform() referenceFrame.Translate(0, 0, 5.0) affs = affs or om.getObjects() for obj in affs: if isinstance(obj, BlockAffordanceItem): d = DebugData() d.addSphere(computeClosestCorner(obj, referenceFrame), radius=0.015) showPolyData(d.getPolyData(), 'closest corner', parent='board debug', visible=True) showFrame(computeGroundFrame(obj, referenceFrame), 'ground frame', parent='board debug', visible=True)
def __init__(self, uid, view, seed_pose, irisDriver, existing_region=None): d = DebugData() self.uid = uid vis.PolyDataItem.__init__(self, "IRIS region {:d}".format(uid), d.getPolyData(), view) self.transform = seed_pose d.addSphere((0,0,0), radius=0.02) self.seedObj = vis.showPolyData(d.getPolyData(), 'region seed', parent=om.getOrCreateContainer('IRIS region seeds')) self.seedObj.actor.SetUserTransform(self.transform) self.frameObj = vis.showFrame(self.transform, 'region seed frame', scale=0.2, visible=False, parent=self.seedObj) self.frameObj.setProperty('Edit', True) self.frameObj.widget.HandleRotationEnabledOff() terrain = om.findObjectByName('HEIGHT_MAP_SCENE') if terrain: rep = self.frameObj.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) pos = np.array(self.frameObj.transform.GetPosition()) polyData = filterUtils.removeNonFinitePoints(terrain.polyData) if polyData.GetNumberOfPoints(): polyData = segmentation.labelDistanceToLine(polyData, pos, pos+[0,0,1]) polyData = segmentation.thresholdPoints(polyData, 'distance_to_line', [0.0, 0.1]) if polyData.GetNumberOfPoints(): pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, 'Points')[:,2]) self.frameObj.transform.Translate(pos - np.array(self.frameObj.transform.GetPosition())) self.placer = PlacerWidget(view, self.seedObj, terrain) self.placer.start() else: self.frameObj.setProperty('Edit', True) self.frameObj.setProperty('Visible', True) self.driver = irisDriver self.safe_region = None self.addProperty('Visible', True) self.addProperty('Enabled for Walking', True) self.addProperty('Alpha', 1.0) self.addProperty('Color', QtGui.QColor(200,200,20)) self.frameObj.connectFrameModified(self.onFrameModified) if existing_region is None: self.onFrameModified(self.frameObj) else: self.setRegion(existing_region) self.setProperty('Alpha', 0.5) self.setProperty('Color', QtGui.QColor(220,220,220))
def update(self): t = self.frameProvider.getFrame('SCAN') p1 = [0.0, 0.0, 0.0] p2 = [2.0, 0.0, 0.0] p1 = t.TransformPoint(p1) p2 = t.TransformPoint(p2) d = DebugData() d.addSphere(p1, radius=0.01, color=[0,1,0]) d.addLine(p1, p2, color=[0,1,0]) self.setPolyData(d.getPolyData())
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 onNewWalkingGoal(self, walkingGoal=None): walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel) frameObj = vis.updateFrame(walkingGoal, 'walking goal', parent='planning', scale=0.25) frameObj.setProperty('Edit', True) rep = frameObj.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) frameObj.widget.HandleRotationEnabledOff() if self.placer: self.placer.stop() terrain = om.findObjectByName('HEIGHT_MAP_SCENE') if terrain: pos = np.array(frameObj.transform.GetPosition()) polyData = filterUtils.removeNonFinitePoints(terrain.polyData) if polyData.GetNumberOfPoints(): polyData = segmentation.labelDistanceToLine(polyData, pos, pos+[0,0,1]) polyData = segmentation.thresholdPoints(polyData, 'distance_to_line', [0.0, 0.1]) if polyData.GetNumberOfPoints(): pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, 'Points')[:,2]) frameObj.transform.Translate(pos - np.array(frameObj.transform.GetPosition())) d = DebugData() d.addSphere((0,0,0), radius=0.03) handle = vis.showPolyData(d.getPolyData(), 'walking goal terrain handle', parent=frameObj, visible=True, color=[1,1,0]) handle.actor.SetUserTransform(frameObj.transform) self.placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain) def onFramePropertyModified(propertySet, propertyName): if propertyName == 'Edit': if propertySet.getProperty(propertyName): self.placer.start() else: self.placer.stop() frameObj.properties.connectPropertyChanged(onFramePropertyModified) onFramePropertyModified(frameObj, 'Edit') frameObj.connectFrameModified(self.onWalkingGoalModified) self.onWalkingGoalModified(frameObj)
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 createPolyDataFromPrimitive(geom): if geom.type == lcmdrake.lcmt_viewer_geometry_data.BOX: d = DebugData() d.addCube(dimensions=geom.float_data[0:3], center=[0,0,0]) return d.getPolyData() elif geom.type == lcmdrake.lcmt_viewer_geometry_data.SPHERE: d = DebugData() d.addSphere(center=(0,0,0), radius=geom.float_data[0]) return d.getPolyData() elif geom.type == lcmdrake.lcmt_viewer_geometry_data.CYLINDER: d = DebugData() d.addCylinder(center=(0,0,0), axis=(0,0,1), radius=geom.float_data[0], length=geom.float_data[1]) return d.getPolyData() elif geom.type == lcmdrake.lcmt_viewer_geometry_data.CAPSULE: d = DebugData() radius = geom.float_data[0] length = geom.float_data[1] d.addCylinder(center=(0,0,0), axis=(0,0,1), radius=radius, length=length) d.addSphere(center=(0,0,length/2.0), radius=radius) d.addSphere(center=(0,0,-length/2.0), radius=radius) return d.getPolyData() raise Exception('Unsupported geometry type: %s' % geom.type)
def debugDrawFootPoints(model): pts_left, pts_right = FootstepsDriver.getContactPts() d = DebugData() for linkName in [_leftFootLink, _rightFootLink]: t = model.getLinkFrame(linkName) d.addFrame(t, scale=0.2) if (linkName is _leftFootLink): pts = pts_left else: pts = pts_right footMidPoint = np.mean(pts, axis=0) for p in pts.tolist() + [footMidPoint.tolist()]: t.TransformPoint(p, p) d.addSphere(p, radius=0.015) midpt = FootstepsDriver.getFeetMidPoint(model) d.addFrame(midpt, scale=0.2) vis.showPolyData(d.getPolyData(), 'foot points debug', parent='debug', colorByName='RGB255')
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 updateGeometryFromProperties(self): d = DebugData() d.addSphere((0, 0, 0), self.getProperty('Radius')) self.setPolyData(d.getPolyData())
from ddapp.consoleapp import ConsoleApp from ddapp.debugVis import DebugData import ddapp.visualization as vis # initialize application components app = ConsoleApp() view = app.createView() view.showMaximized() # create a sphere primitive d = DebugData() d.addSphere(center=(0,0,0), radius=0.5) # show the sphere in the visualization window sphere = vis.showPolyData(d.getPolyData(), 'sphere') sphere.setProperty('Color', [0,1,0]) # start the application app.start()
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 __init__(self, uid, view, seed_pose, irisDriver, existing_region=None): d = DebugData() self.uid = uid vis.PolyDataItem.__init__(self, "IRIS region {:d}".format(uid), d.getPolyData(), view) self.transform = seed_pose d.addSphere((0, 0, 0), radius=0.02) self.seedObj = vis.showPolyData( d.getPolyData(), 'region seed', parent=om.getOrCreateContainer('IRIS region seeds')) self.seedObj.actor.SetUserTransform(self.transform) self.frameObj = vis.showFrame(self.transform, 'region seed frame', scale=0.2, visible=False, parent=self.seedObj) self.frameObj.setProperty('Edit', True) self.frameObj.widget.HandleRotationEnabledOff() terrain = om.findObjectByName('HEIGHT_MAP_SCENE') if terrain: rep = self.frameObj.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) pos = np.array(self.frameObj.transform.GetPosition()) polyData = filterUtils.removeNonFinitePoints(terrain.polyData) if polyData.GetNumberOfPoints(): polyData = segmentation.labelDistanceToLine( polyData, pos, pos + [0, 0, 1]) polyData = segmentation.thresholdPoints( polyData, 'distance_to_line', [0.0, 0.1]) if polyData.GetNumberOfPoints(): pos[2] = np.nanmax( vnp.getNumpyFromVtk(polyData, 'Points')[:, 2]) self.frameObj.transform.Translate( pos - np.array(self.frameObj.transform.GetPosition())) self.placer = PlacerWidget(view, self.seedObj, terrain) self.placer.start() else: self.frameObj.setProperty('Edit', True) self.frameObj.setProperty('Visible', True) self.driver = irisDriver self.safe_region = None self.addProperty('Visible', True) self.addProperty('Enabled for Walking', True) self.addProperty('Alpha', 1.0) self.addProperty('Color', QtGui.QColor(200, 200, 20)) self.frameObj.connectFrameModified(self.onFrameModified) if existing_region is None: self.onFrameModified(self.frameObj) else: self.setRegion(existing_region) self.setProperty('Alpha', 0.5) self.setProperty('Color', QtGui.QColor(220, 220, 220))
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
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 updateGeometryFromProperties(self): d = DebugData() d.addSphere((0,0,0), self.getProperty('Radius')) self.setPolyData(d.getPolyData())
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
from ddapp.consoleapp import ConsoleApp from ddapp.debugVis import DebugData import ddapp.visualization as vis # initialize application components app = ConsoleApp() view = app.createView() view.showMaximized() # create a sphere primitive d = DebugData() d.addSphere(center=(0, 0, 0), radius=0.5) # show the sphere in the visualization window sphere = vis.showPolyData(d.getPolyData(), 'sphere') sphere.setProperty('Color', [0, 1, 0]) # start the application app.start()