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 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, pickedCellId = 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 self.normal = normal self.pickedCellId = pickedCellId modifiers = QtGui.QApplication.keyboardModifiers() if modifiers == QtCore.Qt.ControlModifier and self.cellCaptureMode: self.provisionallyCaptureCell(linkName, pickedCellId)
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 and self.drawClosedLoop: d.addLine(points[0], points[-1]) self.annotationObj = vis.updatePolyData(d.getPolyData(), self.annotationName, parent=self.annotationFolder, view=self.view) 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 getMarkerGeometry(self): if self.markerGeometry is None: d = DebugData() d.addSphere(np.zeros(3), radius=0.007, resolution=8) self.markerGeometry = shallowCopy(d.getPolyData()) return self.markerGeometry
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 and self.drawClosedLoop: d.addLine(points[0], points[-1]) self.annotationObj = vis.updatePolyData(d.getPolyData(), self.annotationName, parent=self.annotationFolder, view=self.view) self.annotationObj.setProperty('Color', [1,0,0]) self.annotationObj.actor.SetPickable(False)
def getMarkerGeometry(self): if self.markerGeometry is None: d = DebugData() d.addSphere(np.zeros(3), radius=0.007, resolution=12) self.markerGeometry = shallowCopy(d.getPolyData()) return self.markerGeometry
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 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 showDebugPoint(p, name='debug point', update=False, visible=True): d = DebugData() d.addSphere(p, radius=0.01, color=[1, 0, 0]) if update: vis.updatePolyData(d.getPolyData(), name) else: vis.showPolyData(d.getPolyData(), name, colorByName='RGB255', visible=visible)
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 createPlunger(self): forceLocationInWorld = np.array([0, 0, 0]) forceDirectionInWorld = np.array([0, 0, 1]) point = forceLocationInWorld - 0.1 * forceDirectionInWorld color = [1, 0, 0] d = DebugData() # d.addArrow(point, forceLocationInWorld, headRadius=0.025, tubeRadius=0.005, color=color) d.addSphere(forceLocationInWorld, radius=0.01) d.addLine(point, forceLocationInWorld, radius=0.005) self.plungerPolyData = d.getPolyData()
def createCapsule(params): d = DebugData() radius = params["radius"] length = params["length"] 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()]
def createCapsule(params): d = DebugData() radius = params["radius"] length = params["length"] color = params.get("color", DEFAULT_COLOR)[:3] d.addCylinder(center=(0, 0, 0), axis=(0, 0, 1), radius=radius, length=length, color=color) d.addSphere(center=(0, 0, length / 2.0), radius=radius, color=color) d.addSphere(center=(0, 0, -length / 2.0), radius=radius, color=color) return [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 createSpheres(self, sensorValues): d = DebugData() for key in list(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 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 buildAccelSphere(center, a_max, color=[1,0.3,0.0], alpha=0.5): d = DebugData() d.addSphere([0,0,0], radius=1) polyData = d.getPolyData() t = vtk.vtkTransform() t.Scale(a_max, a_max, a_max) polyData = filterUtils.transformPolyData(polyData, t) t = vtk.vtkTransform() t.Translate(center) polyData = filterUtils.transformPolyData(polyData, t) obj = vis.updatePolyData(polyData, 'accel_sphere', color=color, alpha=alpha) return polyData
def buildEllipse(i, center, x_scale, y_scale, z_scale, color=[1,1,1], alpha=1.0): d = DebugData() d.addSphere([0,0,0], radius=1) polyData = d.getPolyData() t = vtk.vtkTransform() t.Scale(x_scale, y_scale, z_scale) polyData = filterUtils.transformPolyData(polyData, t) t = vtk.vtkTransform() t.Translate(center) polyData = filterUtils.transformPolyData(polyData, t) obj = vis.updatePolyData(polyData, str(i), color=color, alpha=alpha)
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 drawResult(self, result): radius = 0.01 parent = om.getOrCreateContainer('Hand Eye Calibration') d = DebugData() for cameraLocation in result['cameraLocations']: d.addSphere(cameraLocation, radius=radius) vis.updatePolyData(d.getPolyData(), 'All Camera Locations', parent=parent, color=[1,0,0]) d = DebugData() for data in result['feasiblePoses']: d.addSphere(data['cameraLocation'], radius=radius) vis.updatePolyData(d.getPolyData(), 'Feasible Camera Locations', parent=parent, color=[0,1,0])
def drawCenterOfMass(model): #stanceFrame = footstepsDriver.getFeetMidPoint(model) com = list(model.model.getCenterOfMass()) com[2] = 0.0 #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) for linkName in [robotSystem.ikPlanner.leftFootLink, robotSystem.ikPlanner.rightFootLink]: d = DebugData() contactPts = robotSystem.ikPlanner.robotModel.getLinkContactPoints(linkName) linkFrame = model.getLinkFrame(linkName) for p in contactPts: p = linkFrame.TransformPoint(p) d.addSphere(p, radius=0.01) obj = vis.updatePolyData(d.getPolyData(), '%s %s contact points' % (model.getProperty('Name'), linkName), color=[1,0,0], visible=False, parent=model)
def __init__(self, view): vieweventfilter.ViewEventFilter.__init__(self, view) d = DebugData() d.addSphere((0, 0, 0), radius=1.0) self.cameraCenterObj = vis.PolyDataItem('mouse point', d.getPolyData(), view) self.cameraCenterObj.setProperty('Visible', False) self.cameraCenterObj.setProperty('Color', [1, 1, 0]) self.cameraCenterObj.actor.SetUserTransform(vtk.vtkTransform()) self.cameraCenterObj.actor.SetPickable(False) self.renderWindowobserver = view.renderWindow().AddObserver('StartEvent', self.onStartRender) self.renderWindowobserver = view.renderWindow().AddObserver('EndEvent', self.onEndRender) self.style = vtk.vtkPickCenteredInteractorStyle() self.style.SetMotionFactor(3) self.showOnMove = False self.showCenter = False self.lastHitPoint = None self._enabled = False self.setEnabled(True)
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 == lcmrl.viewer_geometry_data_t.BOX: d = DebugData() d.addCube(dimensions=geom.float_data[0:3], center=[0,0,0]) return d.getPolyData() elif geom.type == lcmrl.viewer_geometry_data_t.SPHERE: d = DebugData() d.addSphere(center=(0,0,0), radius=geom.float_data[0]) return d.getPolyData() elif geom.type == lcmrl.viewer_geometry_data_t.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 == lcmrl.viewer_geometry_data_t.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() elif hasattr(lcmrl.viewer_geometry_data_t, "ELLIPSOID") and geom.type == lcmrl.viewer_geometry_data_t.ELLIPSOID: d = DebugData() radii = geom.float_data[0:3] d.addEllipsoid(center=(0,0,0), radii=radii) return d.getPolyData() raise Exception('Unsupported geometry type: %s' % geom.type)
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 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 createPolyDataFromPrimitive(geom): if geom.type == lcmrl.viewer_geometry_data_t.BOX: d = DebugData() d.addCube(dimensions=geom.float_data[0:3], center=[0, 0, 0]) return d.getPolyData() elif geom.type == lcmrl.viewer_geometry_data_t.SPHERE: d = DebugData() d.addSphere(center=(0, 0, 0), radius=geom.float_data[0]) return d.getPolyData() elif geom.type == lcmrl.viewer_geometry_data_t.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 == lcmrl.viewer_geometry_data_t.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() elif hasattr(lcmrl.viewer_geometry_data_t, "ELLIPSOID") and geom.type == lcmrl.viewer_geometry_data_t.ELLIPSOID: d = DebugData() radii = geom.float_data[0:3] d.addEllipsoid(center=(0, 0, 0), radii=radii) 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 updateDrawIntersection(self, frame, locator="None"): if locator=="None": locator = self.locator if self.sphere_toggle: sphere_radius=self.sigma_sensor else: sphere_radius=0.0 origin = np.array(frame.transform.GetPosition()) #print "origin is now at", origin d = DebugData() d_sphere=DebugData() sliderIdx = self.slider.value controllerType = self.getControllerTypeFromCounter(sliderIdx) colorMaxRange = self.colorMap[controllerType] 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(locator, origin, origin + rayTransformed*self.Sensor.rayLength) if intersection is not None: d.addLine(origin, intersection, color=[1,0,0]) d_sphere.addSphere(intersection, radius=sphere_radius, color=[1,0,0]) else: d.addLine(origin, origin+rayTransformed*self.Sensor.rayLength, color=colorMaxRange) d_sphere.addSphere(origin, radius=0.0, color=[0,1,0]) vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255') vis.updatePolyData(d_sphere.getPolyData(), 'spheres', colorByName='RGB255', alpha=0.3)
def createSphere(params): d = DebugData() color = params.get("color", DEFAULT_COLOR)[:3] d.addSphere(center=(0, 0, 0), radius=params["radius"], color=color) return [d.getPolyData()]
from director.consoleapp import ConsoleApp from director.debugVis import DebugData import director.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 _makeMarkers(self, points, radius=0.01): d = DebugData() for p in points: d.addSphere(p, radius=radius, resolution=8) return d.getPolyData()
def makeDebugPoints(points, radius=0.01): d = DebugData() for p in points: d.addSphere(p, radius=radius) return shallowCopy(d.getPolyData())
d.addLine((0,0,0), (1,0,0), radius=0.03) show(d, (0, 0, 0)) d = DebugData() d.addPolygon([[0,0,0], [0.8, 0, 0], [1, 0.6, 0], [0.4, 1, 0], [-0.2, 0.6, 0]]) show(d, (2, 0, 0)) d = DebugData() d.addPolyLine(getHelixPoints(), radius=0.01) show(d, (4, 0, 0)) d = DebugData() d.addSphere([0, 0, 0], radius=0.3) show(d, (6, 0, 0)) d = DebugData() d.addFrame(vtk.vtkTransform(), scale=0.5, tubeRadius=0.03) show(d, (0, 2, 0)) d = DebugData() d.addArrow((0, 0, 0), (0, 1, 0)) show(d, (2, 2, 0)) d = DebugData() d.addEllipsoid((0, 0, 0), radii=(0.5, 0.35, 0.2))
def handle_message(self, msg): # Limits the rate of message handling, since redrawing is done in the # message handler. self._sub.setSpeedLimit(30) # Removes the folder completely. om.removeFromObjectModel(om.findObjectByName(self._folder_name)) # Recreates folder. folder = om.getOrCreateContainer(self._folder_name) # Though strangely named, DebugData() is the object through which # drawing is done in DrakeVisualizer. d = DebugData() # Set the color map. color_map = self.create_color_map() # The scale value attributable to auto-scale. auto_force_scale = 1.0 auto_moment_scale = 1.0 auto_traction_scale = 1.0 auto_slip_velocity_scale = 1.0 max_force = -1 max_moment = -1 max_traction = -1 max_slip = -1 # TODO(sean-curtis-TRI) Remove the following comment when this # code can be exercised. # The following code is not exercised presently because the # magnitude mode is always set to kFixedLength. # Determine scaling magnitudes if autoscaling is activated. if self.magnitude_mode == ContactVisModes.kAutoScale: if self.show_spatial_force: for surface in msg.hydroelastic_contacts: force = np.array([surface.force_C_W[0], surface.force_C_W[1], surface.force_C_W[2]]) moment = np.array([surface.moment_C_W[0], surface.moment_C_W[1], surface.moment_C_W[2]]) force_mag = np.linalg.norm(force) moment_mag = np.linalg.norm(moment) if force_mag > max_force: max_force = force_mag if moment_mag > max_moment: max_moment = moment_mag # Prepare scaling information for the traction vectors. if self.show_traction_vectors: for quad_point_data in surface.quadrature_point_data: traction = np.array([quad_point_data.traction_Aq_W[0], quad_point_data.traction_Aq_W[1], quad_point_data.traction_Aq_W[2]]) max_traction = max(max_traction, np.linalg.norm(traction)) # Prepare scaling information for the slip velocity vectors. if self.show_slip_velocity_vectors: for quad_point_data in surface.quadrature_point_data: slip_speed = np.array([quad_point_data.vt_BqAq_W[0], quad_point_data.vt_BqAq_W[1], quad_point_data.vt_BqAq_W[2]]) max_slip_speed = max(max_slip_speed, np.linalg.norm(slip_speed)) # Compute scaling factors. auto_force_scale = 1.0 / max_force auto_moment_scale = 1.0 / max_moment auto_traction_scale = 1.0 / max_traction auto_slip_velocity_scale = 1.0 / max_slip_speed # TODO(drum) Consider exiting early if no visualization options are # enabled. for surface in msg.hydroelastic_contacts: view = applogic.getCurrentRenderView() # Keep track if any DebugData is written to. # Necessary to keep DrakeVisualizer from spewing messages to the # console when no DebugData is sent to director. has_debug_data = False # Draw the spatial force. if self.show_spatial_force: point = np.array([surface.centroid_W[0], surface.centroid_W[1], surface.centroid_W[2]]) force = np.array([surface.force_C_W[0], surface.force_C_W[1], surface.force_C_W[2]]) moment = np.array([surface.moment_C_W[0], surface.moment_C_W[1], surface.moment_C_W[2]]) force_mag = np.linalg.norm(force) moment_mag = np.linalg.norm(moment) # Draw the force arrow if it's of sufficient magnitude. if force_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode == ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this force would be # skipped. scale /= force_mag d.addArrow(start=point, end=point + auto_force_scale * force * scale, tubeRadius=0.005, headRadius=0.01, color=[1, 0, 0]) has_debug_data = True # Draw the moment arrow if it's of sufficient magnitude. if moment_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode == ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this moment would be # skipped. scale /= moment_mag d.addArrow(start=point, end=point + auto_moment_scale * moment * scale, tubeRadius=0.005, headRadius=0.01, color=[0, 0, 1]) has_debug_data = True # Iterate over all quadrature points, drawing traction and slip # velocity vectors. if self.show_traction_vectors or self.show_slip_velocity_vectors: # Arrows and/or spheres are drawn through debug data if there # exists a quadrature point. if surface.num_quadrature_points > 0: has_debug_data = True for quad_point_data in surface.quadrature_point_data: origin = np.array([quad_point_data.p_WQ[0], quad_point_data.p_WQ[1], quad_point_data.p_WQ[2]]) if self.show_traction_vectors: traction = np.array([quad_point_data.traction_Aq_W[0], quad_point_data.traction_Aq_W[1], quad_point_data.traction_Aq_W[2]]) traction_mag = np.linalg.norm(traction) # Draw the arrow only if it's of sufficient magnitude. if traction_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode ==\ ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this traction # would be skipped. scale /= traction_mag offset = auto_traction_scale * traction * scale d.addArrow(start=origin, end=origin + offset, tubeRadius=0.000125, headRadius=0.00025, color=[1, 0, 1]) else: d.addSphere(center=origin, radius=0.000125, color=[1, 0, 1]) if self.show_slip_velocity_vectors: slip = np.array([quad_point_data.vt_BqAq_W[0], quad_point_data.vt_BqAq_W[1], quad_point_data.vt_BqAq_W[2]]) slip_mag = np.linalg.norm(slip) # Draw the arrow only if it's of sufficient magnitude. if slip_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode ==\ ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this slip # vector would be skipped. scale /= slip_mag offset = auto_slip_velocity_scale * slip * scale d.addArrow(start=origin, end=origin + offset, tubeRadius=0.000125, headRadius=0.00025, color=[0, 1, 1]) else: d.addSphere(center=origin, radius=0.000125, color=[0, 1, 1]) # Send everything except pressure and contact edges to director. if has_debug_data: item_name = '{}, {}'.format( surface.body1_name, surface.body2_name) cls = vis.PolyDataItem item = cls(item_name, d.getPolyData(), view) om.addToObjectModel(item, folder) item.setProperty('Visible', True) item.setProperty('Alpha', 1.0) # Coloring for force and moment vectors. item.colorBy('RGB255') if self.show_pressure or self.show_contact_edges: pos, pos_above, pos_below, uvs, tri_mesh, seg_mesh = \ self.process_triangles(surface) if self.show_pressure and len(tri_mesh) > 0: # Copy data to VTK objects. vtk_uvs = vnp.getVtkFromNumpy(uvs) vtk_tris_above = vtk.vtkCellArray() vtk_tris_below = vtk.vtkCellArray() vtk_tris_above.Allocate(len(tri_mesh)) vtk_tris_below.Allocate(len(tri_mesh)) for tri in tri_mesh: vtk_tris_above.InsertNextCell(3, tri) vtk_tris_below.InsertNextCell(3, tri) vtk_polydata_tris_above = vtk.vtkPolyData() vtk_polydata_tris_above.SetPoints( vnp.getVtkPointsFromNumpy(pos_above)) vtk_polydata_tris_above.SetPolys(vtk_tris_above) vtk_polydata_tris_above.GetPointData().SetTCoords(vtk_uvs) vtk_polydata_tris_below = vtk.vtkPolyData() vtk_polydata_tris_below.SetPoints( vnp.getVtkPointsFromNumpy(pos_below)) vtk_polydata_tris_below.SetPolys(vtk_tris_below) vtk_polydata_tris_below.GetPointData().SetTCoords(vtk_uvs) vtk_mapper_above = vtk.vtkPolyDataMapper() vtk_mapper_above.SetInputData(vtk_polydata_tris_above) vtk_mapper_below = vtk.vtkPolyDataMapper() vtk_mapper_below.SetInputData(vtk_polydata_tris_below) # Feed VTK objects into director. item_name = 'Pressure between {}, {}'.format( surface.body1_name, surface.body2_name) polydata_item_above = vis.PolyDataItem( item_name, vtk_polydata_tris_above, view) polydata_item_above.actor.SetMapper(vtk_mapper_above) polydata_item_above.actor.SetTexture(self.texture) om.addToObjectModel(polydata_item_above, folder) item_name = 'Pressure between {}, {}'.format( surface.body1_name, surface.body2_name) polydata_item_below = vis.PolyDataItem( item_name, vtk_polydata_tris_below, view) polydata_item_below.actor.SetMapper(vtk_mapper_below) polydata_item_below.actor.SetTexture(self.texture) om.addToObjectModel(polydata_item_below, folder) if self.show_contact_edges and len(seg_mesh) > 0: # Copy data to VTK objects. vtk_segs = vtk.vtkCellArray() vtk_segs.Allocate(len(seg_mesh)) for seg in seg_mesh: vtk_segs.InsertNextCell(2, seg) vtk_polydata_segs = vtk.vtkPolyData() vtk_polydata_segs.SetPoints( vnp.getVtkPointsFromNumpy(pos)) vtk_polydata_segs.SetLines(vtk_segs) vtk_mapper = vtk.vtkPolyDataMapper() vtk_mapper.SetInputData(vtk_polydata_segs) vtk_mapper.Update() # Feed VTK objects into director. item_name = 'Contact edges between {}, {}'.format( surface.body1_name, surface.body2_name) polydata_item = vis.PolyDataItem( item_name, vtk_polydata_segs, view) polydata_item.actor.SetMapper(vtk_mapper) [r, g, b] = color_map.get_contrasting_color() contrasting_color = [r*255, g*255, b*255] polydata_item.actor.GetProperty().SetColor(contrasting_color) om.addToObjectModel(polydata_item, folder)
def updateGeometryFromProperties(self): d = DebugData() d.addSphere((0, 0, 0), self.getProperty('Radius')) self.setPolyData(d.getPolyData())
def updateGeometryFromProperties(self): d = DebugData() d.addSphere((0,0,0), self.getProperty('Radius')) self.setPolyData(d.getPolyData())
def handle_message(self, msg): # Limits the rate of message handling, since redrawing is done in the # message handler. self._sub.setSpeedLimit(30) # Removes the folder completely. om.removeFromObjectModel(om.findObjectByName(self._folder_name)) # Recreates folder. folder = om.getOrCreateContainer(self._folder_name) # Set the color map. color_map = self.create_color_map() # The scale value attributable to auto-scale. auto_force_scale = 1.0 auto_moment_scale = 1.0 auto_traction_scale = 1.0 auto_slip_velocity_scale = 1.0 max_force = -1 max_moment = -1 max_traction = -1 max_slip_speed = -1 # Determine scaling magnitudes if autoscaling is activated. if self.magnitude_mode == ContactVisModes.kAutoScale: for surface in msg.hydroelastic_contacts: if self.show_spatial_force: force = np.array([ surface.force_C_W[0], surface.force_C_W[1], surface.force_C_W[2] ]) moment = np.array([ surface.moment_C_W[0], surface.moment_C_W[1], surface.moment_C_W[2] ]) force_mag = np.linalg.norm(force) moment_mag = np.linalg.norm(moment) if force_mag > max_force: max_force = force_mag if moment_mag > max_moment: max_moment = moment_mag # Prepare scaling information for the traction vectors. if self.show_traction_vectors: for quad_point_data in surface.quadrature_point_data: traction = np.array([ quad_point_data.traction_Aq_W[0], quad_point_data.traction_Aq_W[1], quad_point_data.traction_Aq_W[2] ]) max_traction = max(max_traction, np.linalg.norm(traction)) # Prepare scaling information for the slip velocity vectors. if self.show_slip_velocity_vectors: for quad_point_data in surface.quadrature_point_data: slip_speed = np.array([ quad_point_data.vt_BqAq_W[0], quad_point_data.vt_BqAq_W[1], quad_point_data.vt_BqAq_W[2] ]) max_slip_speed = max(max_slip_speed, np.linalg.norm(slip_speed)) # Compute scaling factors. We don't want division by zero. # We don't want division by negative numbers. if max_force > 0: auto_force_scale = 1.0 / max_force if max_moment > 0: auto_moment_scale = 1.0 / max_moment if max_traction > 0: auto_traction_scale = 1.0 / max_traction if max_slip_speed > 0: auto_slip_velocity_scale = 1.0 / max_slip_speed # TODO(drum) Consider exiting early if no visualization options are # enabled. view = applogic.getCurrentRenderView() for surface in msg.hydroelastic_contacts: contact_data_folder = om.getOrCreateContainer( f'Contact data between {surface.body1_name} and ' f'{surface.body2_name}', folder) # Adds a collection of debug data to the console with the given # item name. def add_contact_data(data, item_name): # Exploit the fact that data.append is a vtkAppendPolyData # instance. The number of input connections on port zero is the # number of *actual* geometries added. If zero have been added, # do no work. if (data is None or data.append.GetNumberOfInputConnections(0) == 0): return item = vis.PolyDataItem(item_name, data.getPolyData(), view) om.addToObjectModel(item, contact_data_folder) item.setProperty('Visible', True) item.setProperty('Alpha', 1.0) item.colorBy('RGB255') # Draw the spatial force. if self.show_spatial_force: force_data = DebugData() point = np.array([ surface.centroid_W[0], surface.centroid_W[1], surface.centroid_W[2] ]) force = np.array([ surface.force_C_W[0], surface.force_C_W[1], surface.force_C_W[2] ]) moment = np.array([ surface.moment_C_W[0], surface.moment_C_W[1], surface.moment_C_W[2] ]) force_mag = np.linalg.norm(force) moment_mag = np.linalg.norm(moment) # Draw the force arrow if it's of sufficient magnitude. if force_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode == ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this force would be # skipped. scale /= force_mag force_data.addArrow(start=point, end=point + auto_force_scale * force * scale, tubeRadius=0.001, headRadius=0.002, color=[1, 0, 0]) # Draw the moment arrow if it's of sufficient magnitude. if moment_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode == ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this moment would be # skipped. scale /= moment_mag force_data.addArrow(start=point, end=point + auto_moment_scale * moment * scale, tubeRadius=0.001, headRadius=0.002, color=[0, 0, 1]) add_contact_data(force_data, "Spatial force") # Iterate over all quadrature points, drawing traction and slip # velocity vectors. if self.show_traction_vectors or self.show_slip_velocity_vectors: traction_data = DebugData() slip_data = DebugData() for quad_point_data in surface.quadrature_point_data: origin = np.array([ quad_point_data.p_WQ[0], quad_point_data.p_WQ[1], quad_point_data.p_WQ[2] ]) if self.show_traction_vectors: traction = np.array([ quad_point_data.traction_Aq_W[0], quad_point_data.traction_Aq_W[1], quad_point_data.traction_Aq_W[2] ]) traction_mag = np.linalg.norm(traction) # Draw the arrow only if it's of sufficient magnitude. if traction_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode ==\ ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this traction # would be skipped. scale /= traction_mag offset = auto_traction_scale * traction * scale traction_data.addArrow(start=origin, end=origin + offset, tubeRadius=0.000125, headRadius=0.00025, color=[1, 0, 1]) else: traction_data.addSphere(center=origin, radius=0.000125, color=[1, 0, 1]) if self.show_slip_velocity_vectors: slip = np.array([ quad_point_data.vt_BqAq_W[0], quad_point_data.vt_BqAq_W[1], quad_point_data.vt_BqAq_W[2] ]) slip_mag = np.linalg.norm(slip) # Draw the arrow only if it's of sufficient magnitude. if slip_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode ==\ ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this slip # vector would be skipped. scale /= slip_mag offset = auto_slip_velocity_scale * slip * scale slip_data.addArrow(start=origin, end=origin + offset, tubeRadius=0.000125, headRadius=0.00025, color=[0, 1, 1]) else: slip_data.addSphere(center=origin, radius=0.000125, color=[0, 1, 1]) add_contact_data(traction_data, "Traction") add_contact_data(slip_data, "Slip velocity") if self.show_pressure or self.show_contact_edges: pos, uvs, tri_mesh, seg_mesh = \ self.process_triangles(surface) if self.show_pressure and len(tri_mesh) > 0: # Copy data to VTK objects. vtk_uvs = vnp.getVtkFromNumpy(uvs) vtk_tris = vtk.vtkCellArray() vtk_tris.Allocate(len(tri_mesh)) for tri in tri_mesh: vtk_tris.InsertNextCell(3, tri) vtk_polydata_tris = vtk.vtkPolyData() vtk_polydata_tris.SetPoints(vnp.getVtkPointsFromNumpy(pos)) vtk_polydata_tris.SetPolys(vtk_tris) vtk_polydata_tris.GetPointData().SetTCoords(vtk_uvs) vtk_mapper = vtk.vtkPolyDataMapper() vtk_mapper.SetInputData(vtk_polydata_tris) # Feed VTK objects into director. item_name = 'Contact surface' polydata_item = vis.PolyDataItem(item_name, vtk_polydata_tris, view) polydata_item.actor.SetMapper(vtk_mapper) polydata_item.actor.SetTexture(self.texture) om.addToObjectModel(polydata_item, contact_data_folder) if self.show_contact_edges and len(seg_mesh) > 0: # Copy data to VTK objects. vtk_segs = vtk.vtkCellArray() vtk_segs.Allocate(len(seg_mesh)) for seg in seg_mesh: vtk_segs.InsertNextCell(2, seg) vtk_polydata_segs = vtk.vtkPolyData() vtk_polydata_segs.SetPoints(vnp.getVtkPointsFromNumpy(pos)) vtk_polydata_segs.SetLines(vtk_segs) vtk_mapper = vtk.vtkPolyDataMapper() vtk_mapper.SetInputData(vtk_polydata_segs) vtk_mapper.Update() # Feed VTK objects into director. item_name = 'Mesh edges' polydata_item = vis.PolyDataItem(item_name, vtk_polydata_segs, view) polydata_item.actor.SetMapper(vtk_mapper) [r, g, b] = color_map.get_contrasting_color() contrasting_color = [r * 255, g * 255, b * 255] polydata_item.actor.GetProperty().SetColor(contrasting_color) om.addToObjectModel(polydata_item, contact_data_folder)
def createSphere(params): d = DebugData() d.addSphere(center=(0, 0, 0), radius=params["radius"]) return [d.getPolyData()]
def drawShape(self, currShape, next_loc, msg, rotation_matrix=None): ''' Function for drawing shapes. Currently this supports lines, points, and 3D axes currShape: the current shape to be drawn next_loc: the location where to draw the shape msg: the message from the LCM hendler that called this function rotation_matrix: the rotation matrix to be used for the axis shape. Mainly used by the abstract handler ''' # draw a continuous line if (currShape.type == "line"): # check if the duration has been initialized if (currShape.duration == None or len(currShape.points) == 0): currShape.duration = msg.utime / 1000000 # visualize and trace line for 'history' seconds, adding points at a distance at least 10e-5 if (((msg.utime / 1000000) - currShape.duration <= currShape.history) or currShape.history <= 0): # make sure to add at least 2 points before starting to check for the distance between points if (len(currShape.points) < 2): currShape.points.append(next_loc) d = DebugData() d.addPolyLine(currShape.points, radius=currShape.thickness, color=currShape.color) if (currShape.object == None): currShape.object = vis.showPolyData( d.getPolyData(), currShape.name) currShape.object.setProperty('Color', currShape.color) else: currShape.object.setPolyData(d.getPolyData()) else: if (np.linalg.norm( np.array(next_loc) - np.array(currShape.points[-1])) >= 10e-5): currShape.points.append(next_loc) d = DebugData() d.addPolyLine(currShape.points, radius=currShape.thickness, color=currShape.color) if (currShape.object == None): currShape.object = vis.showPolyData( d.getPolyData(), currShape.name) else: currShape.object.setPolyData(d.getPolyData()) elif (currShape.history > 0): if (len(currShape.points) == 0): currShape.duration = msg.utime / 1000000 else: # visualize and trace line for 'history' seconds, adding points at a distance at least 10e-5 if (np.linalg.norm( np.array(next_loc) - np.array(currShape.points[-1])) >= 10e-5): currShape.points.popleft() currShape.points.append(next_loc) d = DebugData() d.addPolyLine(currShape.points, radius=currShape.thickness, color=currShape.color) if (currShape.object == None): currShape.object = vis.showPolyData( d.getPolyData(), currShape.name) else: currShape.object.setPolyData(d.getPolyData()) # draw a point elif (currShape.type == "point"): d = DebugData() d.addSphere(next_loc, radius=currShape.radius) # create a new point if (currShape.created == True): currShape.object = vis.showPolyData(d.getPolyData(), currShape.name) # set color and transparency of point currShape.object.setProperty('Color', currShape.color) currShape.object.setProperty('Alpha', currShape.alpha) currShape.created = False else: # update the location of the last point currShape.object.setPolyData(d.getPolyData()) # draw a set of axes elif (currShape.type == "axes"): # get the rotation matrix rot_matrix = None if (currShape.category != "lcm"): rigTrans = self.plant.EvalBodyPoseInWorld( self.context, self.plant.GetBodyByName(currShape.frame)) rot_matrix = rigTrans.rotation().matrix().transpose() else: rot_matrix = rotation_matrix colors = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] d = DebugData() for i in range(3): d.addArrow(next_loc, next_loc + (rot_matrix[i] * currShape.length), headRadius=0.03, color=colors[i]) # create the 3 axes if (currShape.created == True): currShape.object = vis.showPolyData(d.getPolyData(), currShape.name, colorByName='RGB255') currShape.object.setProperty('Alpha', currShape.alpha) currShape.created = False else: # update the location of the last point currShape.object.setPolyData(d.getPolyData())
def handle_message(self, msg): # Limits the rate of message handling, since redrawing is done in the # message handler. self._sub.setSpeedLimit(30) # Removes the folder completely. om.removeFromObjectModel(om.findObjectByName(self._folder_name)) # Recreates folder. folder = om.getOrCreateContainer(self._folder_name) # Though strangely named, DebugData() is the object through which # drawing is done in DrakeVisualizer. d = DebugData() # Set the color map. color_map = self.create_color_map() # The scale value attributable to auto-scale. auto_force_scale = 1.0 auto_moment_scale = 1.0 auto_traction_scale = 1.0 auto_slip_velocity_scale = 1.0 max_force = -1 max_moment = -1 max_traction = -1 max_slip = -1 # TODO(sean-curtis-TRI) Remove the following comment when this # code can be exercised. # The following code is not exercised presently because the # magnitude mode is always set to kFixedLength. # Determine scaling magnitudes if autoscaling is activated. if self.magnitude_mode == ContactVisModes.kAutoScale: if self.show_spatial_force: for surface in msg.hydroelastic_contacts: force = np.array([ surface.force_C_W[0], surface.force_C_W[1], surface.force_C_W[2] ]) moment = np.array([ surface.moment_C_W[0], surface.moment_C_W[1], surface.moment_C_W[2] ]) force_mag = np.linalg.norm(force) moment_mag = np.linalg.norm(moment) if force_mag > max_force: max_force = force_mag if moment_mag > max_moment: max_moment = moment_mag # Prepare scaling information for the traction vectors. if self.show_traction_vectors: for quad_point_data in surface.quadrature_point_data: traction = np.array([ quad_point_data.traction_Aq_W[0], quad_point_data.traction_Aq_W[1], quad_point_data.traction_Aq_W[2] ]) max_traction = max(max_traction, np.linalg.norm(traction)) # Prepare scaling information for the slip velocity vectors. if self.show_slip_velocity_vectors: for quad_point_data in surface.quadrature_point_data: slip_speed = np.array([ quad_point_data.vt_BqAq_W[0], quad_point_data.vt_BqAq_W[1], quad_point_data.vt_BqAq_W[2] ]) max_slip_speed = max(max_slip_speed, np.linalg.norm(slip_speed)) # Compute scaling factors. auto_force_scale = 1.0 / max_force auto_moment_scale = 1.0 / max_moment auto_traction_scale = 1.0 / max_traction auto_slip_velocity_scale = 1.0 / max_slip_speed # TODO(drum) Consider exiting early if no visualization options are # enabled. for surface in msg.hydroelastic_contacts: # Draw the spatial force. if self.show_spatial_force: point = np.array([ surface.centroid_W[0], surface.centroid_W[1], surface.centroid_W[2] ]) force = np.array([ surface.force_C_W[0], surface.force_C_W[1], surface.force_C_W[2] ]) moment = np.array([ surface.moment_C_W[0], surface.moment_C_W[1], surface.moment_C_W[2] ]) force_mag = np.linalg.norm(force) moment_mag = np.linalg.norm(moment) # Draw the force arrow if it's of sufficient magnitude. if force_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode == ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this force would be # skipped. scale /= force_mag d.addArrow(start=point, end=point + auto_force_scale * force * scale, tubeRadius=0.005, headRadius=0.01, color=[1, 0, 0]) # Draw the moment arrow if it's of sufficient magnitude. if moment_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode == ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this moment would be # skipped. scale /= moment_mag d.addArrow(start=point, end=point + auto_moment_scale * moment * scale, tubeRadius=0.005, headRadius=0.01, color=[0, 0, 1]) # Iterate over all quadrature points, drawing traction and slip # velocity vectors. if self.show_traction_vectors or self.show_slip_velocity_vectors: for quad_point_data in surface.quadrature_point_data: origin = np.array([ quad_point_data.p_WQ[0], quad_point_data.p_WQ[1], quad_point_data.p_WQ[2] ]) if self.show_traction_vectors: traction = np.array([ quad_point_data.traction_Aq_W[0], quad_point_data.traction_Aq_W[1], quad_point_data.traction_Aq_W[2] ]) traction_mag = np.linalg.norm(traction) # Draw the arrow only if it's of sufficient magnitude. if traction_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode ==\ ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this traction # would be skipped. scale /= traction_mag offset = auto_traction_scale * traction * scale d.addArrow(start=origin, end=origin + offset, tubeRadius=0.000125, headRadius=0.00025, color=[1, 0, 1]) else: d.addSphere(center=origin, radius=0.000125, color=[1, 0, 1]) if self.show_slip_velocity_vectors: slip = np.array([ quad_point_data.vt_BqAq_W[0], quad_point_data.vt_BqAq_W[1], quad_point_data.vt_BqAq_W[2] ]) slip_mag = np.linalg.norm(slip) # Draw the arrow only if it's of sufficient magnitude. if slip_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode ==\ ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this slip # vector would be skipped. scale /= slip_mag offset = auto_slip_velocity_scale * slip * scale d.addArrow(start=origin, end=origin + offset, tubeRadius=0.000125, headRadius=0.00025, color=[0, 1, 1]) else: d.addSphere(center=origin, radius=0.000125, color=[0, 1, 1]) # Iterate over all triangles. for tri in surface.triangles: va = np.array([tri.p_WA[0], tri.p_WA[1], tri.p_WA[2]]) vb = np.array([tri.p_WB[0], tri.p_WB[1], tri.p_WB[2]]) vc = np.array([tri.p_WC[0], tri.p_WC[1], tri.p_WC[2]]) # Save the maximum pressure. self.max_pressure_observed = max(self.max_pressure_observed, tri.pressure_A) self.max_pressure_observed = max(self.max_pressure_observed, tri.pressure_B) self.max_pressure_observed = max(self.max_pressure_observed, tri.pressure_C) # TODO(drum) Vertex color interpolation may be insufficiently # granular if a single triangle spans too large a range of # pressures. Suggested solution is to use a texture map. # Get the colors at the vertices. color_a = color_map.get_color(tri.pressure_A) color_b = color_map.get_color(tri.pressure_B) color_c = color_map.get_color(tri.pressure_C) if self.show_pressure: # TODO(drum) Use a better method for this; the current # approach is susceptible to z-fighting under certain # zoom levels. # Compute a normal to the triangle. We need this normal # because the visualized pressure surface can be coplanar # with parts of the visualized geometry, in which case a # dithering type effect would appear. So we use the normal # to draw two triangles slightly offset to both sides of # the contact surface. # Note that if the area of this triangle is very small, we # won't waste time visualizing it, which also means that # won't have to worry about degenerate triangles). # TODO(edrumwri) Consider allowing the user to set these # next two values programmatically. min_area = 1e-8 offset_scalar = 1e-4 normal = np.cross(vb - va, vc - vb) norm_normal = np.linalg.norm(normal) if norm_normal >= min_area: unit_normal = normal / np.linalg.norm(normal) offset = unit_normal * offset_scalar d.addPolygon([va + offset, vb + offset, vc + offset], color=[color_a, color_b, color_c]) d.addPolygon([va - offset, vb - offset, vc - offset], color=[color_a, color_b, color_c]) # TODO(drum) Consider drawing shared edges just once. if self.show_contact_edges: contrasting_color = color_map.get_contrasting_color() d.addPolyLine(points=(va, vb, vc), isClosed=True, color=contrasting_color) item_name = '{}, {}'.format(surface.body1_name, surface.body2_name) cls = vis.PolyDataItem view = applogic.getCurrentRenderView() item = cls(item_name, d.getPolyData(), view) om.addToObjectModel(item, folder) item.setProperty('Visible', True) item.setProperty('Alpha', 1.0) # Conditional necessary to keep DrakeVisualizer from spewing # messages to the console when the contact surface is empty. if len(msg.hydroelastic_contacts) > 0: item.colorBy('RGB255')
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 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 makeSphere(self, position, radius=0.0075): d = DebugData() d.addSphere(position, radius=radius) return d.getPolyData()