def updatePlanFrames(self): if self.getViewMode() != 'frames': return numberOfSamples = self.getNumberOfSamples() meshes = self.planPlayback.getPlanPoseMeshes(self.plan, self.playbackJointController, self.playbackRobotModel, numberOfSamples) d = DebugData() startColor = [0.8, 0.8, 0.8] endColor = [85/255.0, 255/255.0, 255/255.0] colorFunc = scipy.interpolate.interp1d([0, numberOfSamples-1], [startColor, endColor], axis=0, kind='slinear') for i, mesh in reversed(list(enumerate(meshes))): d.addPolyData(mesh, color=colorFunc(i)) pd = d.getPolyData() clean = vtk.vtkCleanPolyData() clean.SetInput(pd) clean.Update() pd = clean.GetOutput() self.planFramesObj = vis.updatePolyData(d.getPolyData(), 'robot plan', alpha=1.0, visible=False, colorByName='RGB255', parent='planning') self.showPlanFrames()
def draw(self): d = DebugData() points = list(self.points) if self.hoverPos is not None: points.append(self.hoverPos) # draw points for p in points: d.addSphere(p, radius=5) if self.drawLines and len(points) > 1: for a, b in zip(points, points[1:]): d.addLine(a, b) # connect end points # d.addLine(points[0], points[-1]) if self.annotationObj: self.annotationObj.setPolyData(d.getPolyData()) else: self.annotationObj = vis.updatePolyData(d.getPolyData(), 'annotation', parent='segmentation', color=[1,0,0], view=self.view) self.annotationObj.addToView(self.view) self.annotationObj.actor.SetPickable(False) self.annotationObj.actor.GetProperty().SetLineWidth(2)
def draw(self): d = DebugData() points = 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 setupStrings(): d = DebugData() poles = [[-0.36, 0.5, 1.5], [-0.36, -0.5, 1.5], [0, 0.5, 1.5], [0, -0.5, 1.5], [0.36, 0.5, 1.5], [0.36, -0.5, 1.5]] for pole in poles: d.addCylinder(pole, [0, 0, 1], 3, radius=0.021) vis.updatePolyData(d.getPolyData(), "poles") d = DebugData() strings = [ [-0.36, 0.5, 0.09, 0, -0.5, 1.77], [-0.36, 0.5, 0.74, -0.36, -0.5, 0.95], [-0.36, 0.5, 1.12, -0.36, -0.5, 1.68], [-0.36, 0.5, 1.33, 0.36, -0.5, 2.29], [-0.36, 0.5, 1.6, 0.36, -0.5, 1.62], [-0.36, 0.5, 1.74, 0.36, -0.5, 1.93], [-0.36, 0.5, 2.15, -0.36, -0.5, 1.46], [0, 0.5, 0.765, 0, -0.5, 0.795], [0, 0.5, 1.15, 0.36, -0.5, 1.15], [0, 0.5, 1.28, -0.36, -0.5, 0.11], [0, 0.5, 1.42, 0, -0.5, 1.42], [0, 0.5, 1.78, 0.36, -0.5, 0.12], [0, 0.5, 2.05, -0.36, -0.5, 1.835], [0.36, 0.5, 0.8, -0.36, -0.5, 1.11], [0.36, 0.5, 1.16, -0.36, -0.5, 1.47], [0.36, 0.5, 1.61, 0.36, -0.5, 1.19], [0.36, 0.5, 2.0, 0.36, -0.5, 2.1], [-0.36, 0.3, 0, -0.36, 0.3, 2.01], [0, -0.34, 0, 0, -0.34, 1.42], [0.36, 0, 0, 0.36, 0, 2.05], ] for string in strings: p1 = string[:3] p2 = string[3:] d.addLine(p1, p2, radius=0.001, color=[255, 0, 0]) vis.updatePolyData(d.getPolyData(), "strings")
def updateIntersection(frame): origin = np.array(frame.transform.GetPosition()) rayLength = 5 for i in range(0,numRays): ray = rays[:,i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) intersection = computeIntersection(locator, origin, origin + rayTransformed*rayLength) name = 'ray intersection ' + str(i) if intersection is not None: om.removeFromObjectModel(om.findObjectByName(name)) d = DebugData() d.addLine(origin, intersection) color = [1,0,0] # d.addSphere(intersection, radius=0.04) vis.updatePolyData(d.getPolyData(), name, color=color) else: om.removeFromObjectModel(om.findObjectByName(name)) d = DebugData() d.addLine(origin, origin+rayTransformed*rayLength) color = [0,1,0] # d.addSphere(intersection, radius=0.04) vis.updatePolyData(d.getPolyData(), name, color=color)
def 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 setupStrings(): d = DebugData() poles = [[-.36,.5,1.5],[-.36,-.5,1.5],[0,.5,1.5],[0,-.5,1.5],[.36,.5,1.5],[.36,-.5,1.5]] for pole in poles: d.addCylinder(pole, [0,0,1], 3, radius=0.021) vis.updatePolyData(d.getPolyData(), 'poles') d = DebugData() strings = [ [-.36, .5, .09, 0, -.5, 1.77], [-.36, .5, .74, -.36, -.5, .95], [-.36, .5, 1.12, -.36, -.5, 1.68], [-.36, .5, 1.33, .36, -.5, 2.29], [-.36, .5, 1.6, .36, -.5, 1.62], [-.36, .5, 1.74, .36, -.5, 1.93], [-.36, .5, 2.15, -.36, -.5, 1.46], [0, .5, .765, 0, -.5, .795], [0, .5, 1.15, .36, -.5, 1.15], [0, .5, 1.28, -.36, -.5, .11], [0, .5, 1.42, 0, -.5, 1.42], [0, .5, 1.78, .36, -.5, .12], [0, .5, 2.05, -.36, -.5, 1.835], [.36, .5, .8, -.36, -.5, 1.11], [.36, .5, 1.16, -.36, -.5, 1.47], [.36, .5, 1.61, .36, -.5, 1.19], [.36, .5, 2.0, .36, -.5, 2.1], [-.36, .3, 0, -.36, .3, 2.01], [0, -.34, 0, 0, -.34, 1.42], [.36, 0, 0, .36, 0, 2.05], ] for string in strings: p1 = string[:3] p2 = string[3:] d.addLine(p1,p2,radius=.001,color=[255,0,0]) vis.updatePolyData(d.getPolyData(), 'strings')
def updatePlanFrames(self): if self.getViewMode() != 'frames': return numberOfSamples = self.getNumberOfSamples() meshes = self.planPlayback.getPlanPoseMeshes( self.plan, self.playbackJointController, self.playbackRobotModel, numberOfSamples) d = DebugData() startColor = [0.8, 0.8, 0.8] endColor = [85 / 255.0, 255 / 255.0, 255 / 255.0] colorFunc = scipy.interpolate.interp1d([0, numberOfSamples - 1], [startColor, endColor], axis=0, kind='slinear') for i, mesh in reversed(list(enumerate(meshes))): d.addPolyData(mesh, color=colorFunc(i)) pd = d.getPolyData() clean = vtk.vtkCleanPolyData() clean.SetInput(pd) clean.Update() pd = clean.GetOutput() self.planFramesObj = vis.updatePolyData(d.getPolyData(), 'robot plan', alpha=1.0, visible=False, colorByName='RGB255', parent='planning') self.showPlanFrames()
def __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 buildDonutWorld(): print "building donut world" lineRadius = 0.2 hallwayWidth = 10 numSegments = 6 circleRadius = 50 d = DebugData() angleList = np.linspace(0, 2 * np.pi, numSegments + 1) for i in range(numSegments): theta = angleList[i] theta_next = angleList[i + 1] for radius in [circleRadius, circleRadius + hallwayWidth]: lineStart = [radius * np.cos(theta), radius * np.sin(theta), 0] lineEnd = [ radius * np.cos(theta_next), radius * np.sin(theta_next), 0 ] d.addLine(lineStart, lineEnd, radius=lineRadius) obj = vis.showPolyData(d.getPolyData(), 'world') world = World() world.visObj = obj return world
def buildSimpleWorld(): print "this is a test" d = DebugData() d.addLine((2, -1, 0), (2, 1, 0), radius=0.1) d.addLine((2, -1, 0), (1, -2, 0), radius=0.1) obj = vis.showPolyData(d.getPolyData(), 'world') return obj
def updateDrawIntersection(self, frame): origin = np.array(frame.transform.GetPosition()) #print "origin is now at", origin d = DebugData() sliderIdx = self.slider.value controllerType = self.getControllerTypeFromCounter(sliderIdx) colorMaxRange = self.colorMap[controllerType] # if the QValue was empty then color it green if self.emptyQValue[sliderIdx]: colorMaxRange = [1, 1, 0] # this is yellow for i in xrange(self.Sensor.numRays): ray = self.Sensor.rays[:, i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) #print "rayTransformed is", rayTransformed intersection = self.Sensor.raycast( self.locator, origin, origin + rayTransformed * self.Sensor.rayLength) if intersection is not None: d.addLine(origin, intersection, color=[1, 0, 0]) else: d.addLine(origin, origin + rayTransformed * self.Sensor.rayLength, color=colorMaxRange) vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
def updateDrawIntersection(self, frame): origin = np.array(frame.transform.GetPosition()) # print "origin is now at", origin d = DebugData() sliderIdx = self.slider.value controllerType = self.getControllerTypeFromCounter(sliderIdx) colorMaxRange = self.colorMap[controllerType] # if the QValue was empty then color it green if self.emptyQValue[sliderIdx]: colorMaxRange = [1, 1, 0] # this is yellow for i in xrange(self.Sensor.numRays): ray = self.Sensor.rays[:, i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) # print "rayTransformed is", rayTransformed intersection = self.Sensor.raycast(self.locator, origin, origin + rayTransformed * self.Sensor.rayLength) if intersection is not None: d.addLine(origin, intersection, color=[1, 0, 0]) else: d.addLine(origin, origin + rayTransformed * self.Sensor.rayLength, color=colorMaxRange) vis.updatePolyData(d.getPolyData(), "rays", colorByName="RGB255")
def __init__(self, robotSystem, view): self.robotStateModel = robotSystem.robotStateModel self.robotStateJointController = robotSystem.robotStateJointController self.robotSystem = robotSystem self.lFootFtFrameId = self.robotStateModel.model.findLinkID( self.robotSystem.ikPlanner.leftFootLink) self.rFootFtFrameId = self.robotStateModel.model.findLinkID( self.robotSystem.ikPlanner.rightFootLink) self.leftInContact = 0 self.rightInContact = 0 self.view = view self.ddDrakeWrapper = PythonQt.dd.ddDrakeWrapper() self.warningButton = QtGui.QLabel('COP Warning') self.warningButton.setStyleSheet("background-color:white") self.dialogVisible = False d = DebugData() vis.updatePolyData(d.getPolyData(), 'measured cop', view=self.view, parent='robot state model') om.findObjectByName('measured cop').setProperty('Visible', False) self.robotStateModel.connectModelChanged(self.update)
def buildRobot(): d = DebugData() d.addCone((0,0,0), (1,0,0), height=0.2, radius=0.1) obj = vis.showPolyData(d.getPolyData(), 'robot') frame = vis.addChildFrame(obj) return obj
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 testFindWalls(self): raycastDistance = self.sensor.raycastAllFromCurrentFrameLocation() wallsFound = self.findWalls(raycastDistance, addNoise=True) carTransform = om.findObjectByName('robot frame').transform d = DebugData() for wallData in wallsFound: intercept = wallData['ransacFit'][0] slope = wallData['ransacFit'][1] wallDirectionInCarFrame = np.array([1.0, slope, 0.0]) wallPointInCarFrame = np.array([0.0, intercept, 0.0]) # now need to transform these to world frame in order to plot them. wallPointWorldFrame = np.array( carTransform.TransformPoint(wallPointInCarFrame)) wallDirectionWorldFrame = np.array( carTransform.TransformVector(wallDirectionInCarFrame)) wallDirectionWorldFrame = 1 / np.linalg.norm( wallDirectionWorldFrame) * wallDirectionWorldFrame lineLength = 15.0 lineOrigin = wallPointWorldFrame - lineLength * wallDirectionWorldFrame lineEnd = wallPointWorldFrame + lineLength * wallDirectionWorldFrame d.addLine(lineOrigin, lineEnd, radius=0.3, color=[0, 0, 1]) vis.updatePolyData(d.getPolyData(), 'line estimate', colorByName='RGB255') return wallsFound
def run(self): radius = self.properties.getProperty('Radius') thickness = 0.03 folder = om.getOrCreateContainer('affordances') frame = self.computeValveFrame() d = DebugData() d.addLine(np.array([0, 0, -thickness / 2.0]), np.array([0, 0, thickness / 2.0]), radius=radius) mesh = d.getPolyData() params = dict(radius=radius, length=thickness, xwidth=radius, ywidth=radius, zwidth=thickness, otdf_type='steering_cyl', friendly_name='valve') affordance = vis.showPolyData(mesh, 'valve', color=[0.0, 1.0, 0.0], cls=affordanceitems.FrameAffordanceItem, parent=folder, alpha=1.0) frame = vis.showFrame(frame, 'valve frame', parent=affordance, visible=False, scale=radius) affordance.actor.SetUserTransform(frame.transform) affordance.setAffordanceParams(params) affordance.updateParamsFromActorTransform()
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 setRegion(self, safe_region): debug = DebugData() pos = safe_region.point try: xy_verts = safe_region.xy_polytope() if xy_verts.shape[1] == 0: raise QhullError("No points returned") xyz_verts = np.vstack((xy_verts, pos[2] + 0.02 + np.zeros( (1, xy_verts.shape[1])))) xyz_verts = np.hstack((xyz_verts, np.vstack( (xy_verts, pos[2] + 0.015 + np.zeros( (1, xy_verts.shape[1])))))) # print xyz_verts.shape polyData = vnp.getVtkPolyDataFromNumpyPoints(xyz_verts.T.copy()) vol_mesh = filterUtils.computeDelaunay3D(polyData) for j in range(xy_verts.shape[1]): z = pos[2] + 0.005 p1 = np.hstack((xy_verts[:, j], z)) if j < xy_verts.shape[1] - 1: p2 = np.hstack((xy_verts[:, j + 1], z)) else: p2 = np.hstack((xy_verts[:, 0], z)) debug.addLine(p1, p2, color=[.7, .7, .7], radius=0.003) debug.addPolyData(vol_mesh) # self.setPolyData(vol_mesh) self.setPolyData(debug.getPolyData()) self.safe_region = safe_region except QhullError: print "Could not generate convex hull (polytope is likely unbounded)."
def setRegion(self, safe_region): debug = DebugData() pos = safe_region.point try: xy_verts = safe_region.xy_polytope() if xy_verts.shape[1] == 0: raise QhullError("No points returned") xyz_verts = np.vstack((xy_verts, pos[2] + 0.02 + np.zeros((1, xy_verts.shape[1])))) xyz_verts = np.hstack((xyz_verts, np.vstack((xy_verts, pos[2] + 0.015 + np.zeros((1, xy_verts.shape[1])))))) # print xyz_verts.shape polyData = vnp.getVtkPolyDataFromNumpyPoints(xyz_verts.T.copy()) vol_mesh = filterUtils.computeDelaunay3D(polyData) for j in range(xy_verts.shape[1]): z = pos[2] + 0.005 p1 = np.hstack((xy_verts[:,j], z)) if j < xy_verts.shape[1] - 1: p2 = np.hstack((xy_verts[:,j+1], z)) else: p2 = np.hstack((xy_verts[:,0], z)) debug.addLine(p1, p2, color=[.7,.7,.7], radius=0.003) debug.addPolyData(vol_mesh) # self.setPolyData(vol_mesh) self.setPolyData(debug.getPolyData()) self.safe_region = safe_region except QhullError: print "Could not generate convex hull (polytope is likely unbounded)."
def buildWorld(): d = DebugData() d.addLine((2,-1,0), (2,1,0), radius=0.1) d.addLine((2,-1,0), (1,-2,0), radius=0.1) obj = vis.showPolyData(d.getPolyData(), 'world') return obj
def buildWarehouseWorld(percentObsDensity, nonRandom=False, circleRadius=0.1, scale=None, randomSeed=5, obstaclesInnerFraction=1.0): if nonRandom: np.random.seed(randomSeed) d = DebugData() worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries( d, scale=scale, boundaryType="Warehouse") numObstacles = 8 obsLength = 2.0 worldLength = worldXmax - worldXmin print worldXmin print worldXmax obstacleZone = [ worldXmin + 0.2 * worldLength, worldXmax - 0.2 * worldLength ] print obstacleZone obstacleLength = obstacleZone[1] - obstacleZone[0] incrementSize = obstacleLength * 1.0 / numObstacles print incrementSize leftOrRight = -1.0 for i in xrange(numObstacles): firstX = obstacleZone[0] + incrementSize * i leftOrRight = leftOrRight * -1.0 firstEndpt = (firstX, leftOrRight * worldYmax, 0.0) secondEndpt = (firstX, 0.0, 0.0) #d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn()) d.addLine(firstEndpt, secondEndpt, radius=circleRadius) obj = vis.showPolyData(d.getPolyData(), 'world') world = World() world.visObj = obj world.Xmax = worldXmax world.Xmin = worldXmin world.Ymax = worldYmax world.Ymin = worldYmin world.numObstacles = numObstacles world.percentObsDensity = percentObsDensity return world
def updateGeometryFromProperties(self): d = DebugData() length = self.getProperty('Length') d.addCapsule(center=(0, 0, 0), axis=(0, 0, 1), length=self.getProperty('Length'), radius=self.getProperty('Radius')) self.setPolyData(d.getPolyData())
def buildCircleWorld(percentObsDensity, nonRandom=False, circleRadius=3, scale=None, randomSeed=5, obstaclesInnerFraction=1.0): #print "building circle world" if nonRandom: np.random.seed(randomSeed) d = DebugData() worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries( d, scale=scale, boundaryType="Square") #print "boundaries done" worldArea = (worldXmax - worldXmin) * (worldYmax - worldYmin) #print worldArea obsScalingFactor = 1.0 / 12.0 maxNumObstacles = obsScalingFactor * worldArea numObstacles = int(obstaclesInnerFraction**2 * percentObsDensity / 100.0 * maxNumObstacles) #print numObstacles # draw random stick obstacles obsLength = 2.0 obsXmin = worldXmin + (1 - obstaclesInnerFraction) / 2.0 * (worldXmax - worldXmin) obsXmax = worldXmax - (1 - obstaclesInnerFraction) / 2.0 * (worldXmax - worldXmin) obsYmin = worldYmin + (1 - obstaclesInnerFraction) / 2.0 * (worldYmax - worldYmin) obsYmax = worldYmax - (1 - obstaclesInnerFraction) / 2.0 * (worldYmax - worldYmin) for i in xrange(numObstacles): firstX = obsXmin + np.random.rand() * (obsXmax - obsXmin) firstY = obsYmin + np.random.rand() * (obsYmax - obsYmin) firstEndpt = (firstX, firstY, +0.2) secondEndpt = (firstX, firstY, -0.2) #d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn()) d.addLine(firstEndpt, secondEndpt, radius=circleRadius) obj = vis.showPolyData(d.getPolyData(), 'world') world = World() world.visObj = obj world.Xmax = worldXmax world.Xmin = worldXmin world.Ymax = worldYmax world.Ymin = worldYmin world.numObstacles = numObstacles world.percentObsDensity = percentObsDensity return world
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 rayDebug(position, ray): d = DebugData() d.addLine(position, position + ray * 5.0) drcView = app.getViewManager().findView('DRC View') obj = vis.updatePolyData(d.getPolyData(), 'camera ray', view=drcView, color=[0, 1, 0]) obj.actor.GetProperty().SetLineWidth(2)
def newMesh(): d = DebugData() d.addArrow((0,0,0), (0,0,0.3)) pd = d.getPolyData() meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd) desc = dict(classname='MeshAffordanceItem', Name='test mesh', Filename=meshId, uuid=newUUID(), pose=((0.5,0.0,1.0), (1,0,0,0))) return affordanceFromDescription(desc)
def updateGeometryFromProperties(self): radius = self.getProperty('Radius') circlePoints = np.linspace(0, 2*np.pi, self.getProperty('Segments')+1) spokes = [(0.0, np.sin(x), np.cos(x)) for x in circlePoints] spokes = [radius*np.array(x)/np.linalg.norm(x) for x in spokes] d = DebugData() for a, b in zip(spokes, spokes[1:]): d.addCapsule(center=(a+b)/2.0, axis=(b-a), length=np.linalg.norm(b-a), radius=self.getProperty('Tube Radius')) self.setPolyData(d.getPolyData())
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 loadFeet(): meshDir = os.path.join(app.getDRCBase(), 'software/models/atlas_v3/meshes') meshes = [] for foot in ['l', 'r']: d = DebugData() d.addPolyData(io.readPolyData(os.path.join(meshDir, '%s_talus.stl' % foot))) d.addPolyData(io.readPolyData(os.path.join(meshDir, '%s_foot.stl' % foot))) meshes.append(d.getPolyData()) return meshes
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 onSpawnMesh(self): d = DebugData() d.addArrow((0,0,0), (0,0,0.3)) pd = d.getPolyData() meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd) pose = transformUtils.poseFromTransform(self.getSpawnFrame()) desc = dict(classname='MeshAffordanceItem', Name='mesh', Filename=meshId, uuid=newUUID(), pose=pose) return self.affordanceFromDescription(desc)
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 onSpawnMesh(self): d = DebugData() d.addArrow((0,0,0), (0,0,0.3)) pd = d.getPolyData() meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd) pose = transformUtils.poseFromTransform(self.getSpawnFrame()) desc = dict(classname='MeshAffordanceItem', Name='mesh', Filename=meshId, uuid=newUUID(), pose=pose) return self.affordanceManager.newAffordanceFromDescription(desc)
def drawTargetPath(self): path = DebugData() for i in range(1,len(self.targetPath)): p0 = self.targetPath[i-1].GetPosition() p1 = self.targetPath[i].GetPosition() path.addLine ( np.array( p0 ) , np.array( p1 ), radius= 0.005) pathMesh = path.getPolyData() self.targetPathMesh = vis.showPolyData(pathMesh, 'target frame desired path', color=[0.0, 0.3, 1.0], parent=self.targetAffordance, alpha=0.6) self.targetPathMesh.actor.SetUserTransform(self.targetFrame)
def loadFeet(): meshDir = os.path.join(app.getDRCBase(), 'software/models/atlas_v3/meshes') meshes = [] for foot in ['l', 'r']: d = DebugData() d.addPolyData( io.readPolyData(os.path.join(meshDir, '%s_talus.stl' % foot))) d.addPolyData( io.readPolyData(os.path.join(meshDir, '%s_foot.stl' % foot))) meshes.append(d.getPolyData()) return meshes
def buildWarehouseWorld(percentObsDensity, nonRandom=False, circleRadius=0.1, scale=None, randomSeed=5, obstaclesInnerFraction=1.0): if nonRandom: np.random.seed(randomSeed) d = DebugData() worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d, scale=scale, boundaryType="Warehouse") numObstacles = 8 obsLength = 2.0 worldLength = worldXmax - worldXmin print worldXmin print worldXmax obstacleZone = [worldXmin + 0.2 * worldLength, worldXmax - 0.2 * worldLength ] print obstacleZone obstacleLength = obstacleZone[1] - obstacleZone[0] incrementSize = obstacleLength * 1.0 / numObstacles print incrementSize leftOrRight = -1.0 for i in xrange(numObstacles): firstX = obstacleZone[0] + incrementSize * i leftOrRight = leftOrRight * -1.0 firstEndpt = (firstX, leftOrRight * worldYmax,0.0) secondEndpt = (firstX, 0.0, 0.0) #d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn()) d.addLine(firstEndpt, secondEndpt, radius=circleRadius) obj = vis.showPolyData(d.getPolyData(), 'world') world = World() world.visObj = obj world.Xmax = worldXmax world.Xmin = worldXmin world.Ymax = worldYmax world.Ymin = worldYmin world.numObstacles = numObstacles world.percentObsDensity = percentObsDensity return world
def loadFootMeshes(): meshes = [] for i in range(0,2): d = DebugData() for footMeshFile in _footMeshFiles[i]: d.addPolyData(ioUtils.readPolyData( footMeshFile , computeNormals=True)) t = vtk.vtkTransform() t.Scale(0.98, 0.98, 0.98) pd = filterUtils.transformPolyData(d.getPolyData(), t) meshes.append(pd) return meshes
def updateCursor(self, displayPoint): center = self.displayPointToImagePoint(displayPoint, restrictToImageDimensions=False) center = np.array(center) d = DebugData() d.addLine(center + [0, -3000, 0], center + [0, 3000, 0]) d.addLine(center + [-3000, 0, 0], center + [3000, 0, 0]) self.cursorObj = vis.updatePolyData(d.getPolyData(), 'cursor', alpha=0.5, view=self.view) self.cursorObj.addToView(self.view) self.cursorObj.actor.SetUseBounds(False) self.cursorObj.actor.SetPickable(False) self.view.render()
def updateGeometryFromProperties(self): radius = self.getProperty('Radius') circlePoints = np.linspace(0, 2 * np.pi, self.getProperty('Segments') + 1) spokes = [(0.0, np.sin(x), np.cos(x)) for x in circlePoints] spokes = [radius * np.array(x) / np.linalg.norm(x) for x in spokes] d = DebugData() for a, b in zip(spokes, spokes[1:]): d.addCapsule(center=(a + b) / 2.0, axis=(b - a), length=np.linalg.norm(b - a), radius=self.getProperty('Tube Radius')) self.setPolyData(d.getPolyData())
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 buildCircleWorld(percentObsDensity, nonRandom=False, circleRadius=3, scale=None, randomSeed=5, obstaclesInnerFraction=1.0): #print "building circle world" if nonRandom: np.random.seed(randomSeed) d = DebugData() worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d, scale=scale) #print "boundaries done" worldArea = (worldXmax-worldXmin)*(worldYmax-worldYmin) #print worldArea obsScalingFactor = 1.0/12.0 maxNumObstacles = obsScalingFactor * worldArea numObstacles = int(obstaclesInnerFraction**2 * percentObsDensity/100.0 * maxNumObstacles) #print numObstacles # draw random stick obstacles obsLength = 2.0 obsXmin = worldXmin + (1-obstaclesInnerFraction)/2.0*(worldXmax - worldXmin) obsXmax = worldXmax - (1-obstaclesInnerFraction)/2.0*(worldXmax - worldXmin) obsYmin = worldYmin + (1-obstaclesInnerFraction)/2.0*(worldYmax - worldYmin) obsYmax = worldYmax - (1-obstaclesInnerFraction)/2.0*(worldYmax - worldYmin) for i in xrange(numObstacles): firstX = obsXmin + np.random.rand()*(obsXmax-obsXmin) firstY = obsYmin + np.random.rand()*(obsYmax-obsYmin) firstEndpt = (firstX,firstY,+0.2) secondEndpt = (firstX,firstY,-0.2) #d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn()) d.addLine(firstEndpt, secondEndpt, radius=circleRadius) obj = vis.showPolyData(d.getPolyData(), 'world') world = World() world.visObj = obj world.Xmax = worldXmax world.Xmin = worldXmin world.Ymax = worldYmax world.Ymin = worldYmin world.numObstacles = numObstacles world.percentObsDensity = percentObsDensity return world
def doFTDraw(self, unusedrobotstate): frames = [] fts = [] vis_names = [] if (hasattr(self.robotStateJointController, 'lastRobotStateMessage') and self.robotStateJointController.lastRobotStateMessage): if self.draw_right: rft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_force + self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_torque) rft[0] = -1.*rft[0] # right FT looks to be rotated 180deg around the z axis rft[1] = -1.*rft[1] rft[3] = -1.*rft[3] rft[4] = -1.*rft[4] rft -= self.ft_right_bias fts.append(rft) frames.append(self.robotStateModel.getLinkFrame('r_hand_force_torque')) vis_names.append('ft_right') if self.draw_left: lft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_force + self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_torque) lft -= self.ft_left_bias fts.append(lft) frames.append(self.robotStateModel.getLinkFrame('l_hand_force_torque')) vis_names.append('ft_left') for i in range(0, len(frames)): frame = frames[i] ft = fts[i] offset = [0., 0., 0.] p1 = frame.TransformPoint(offset) scale = 1.0/25.0 # todo add slider for this? scalet = 1.0 / 2.5 p2f = frame.TransformPoint(offset + ft[0:3]*scale) p2t = frame.TransformPoint(offset + ft[3:6]) normalt = (np.array(p2t) - np.array(p1)) normalt = normalt / np.linalg.norm(normalt) d = DebugData() # force if np.linalg.norm(np.array(p2f) - np.array(p1)) < 0.1: d.addLine(p1, p2f, color=[1.0, 0.0, 0.0]) else: d.addArrow(p1, p2f, color=[1.,0.,0.]) # torque if self.draw_torque: d.addCircle(p1, normalt, scalet*np.linalg.norm(ft[3:6])) # frame (largely for debug) vis.updateFrame(frame, vis_names[i]+'frame', view=self.view, parent='wristft', visible=False, scale=0.2) vis.updatePolyData(d.getPolyData(), vis_names[i], view=self.view, parent='wristft')
def getCameraFrustumGeometry(self, rayLength): camPos, rays = self.getCameraFrustumRays() rays = [rayLength*r for r in rays] d = DebugData() d.addLine(camPos, camPos+rays[0]) d.addLine(camPos, camPos+rays[1]) d.addLine(camPos, camPos+rays[2]) d.addLine(camPos, camPos+rays[3]) d.addLine(camPos+rays[0], camPos+rays[1]) d.addLine(camPos+rays[1], camPos+rays[2]) d.addLine(camPos+rays[2], camPos+rays[3]) d.addLine(camPos+rays[3], camPos+rays[0]) return d.getPolyData()
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 run(self): radius = self.properties.getProperty('Radius') thickness = 0.03 folder = om.getOrCreateContainer('affordances') frame = self.computeValveFrame() d = DebugData() d.addLine(np.array([0, 0, -thickness/2.0]), np.array([0, 0, thickness/2.0]), radius=radius) mesh = d.getPolyData() params = dict(radius=radius, length=thickness, xwidth=radius, ywidth=radius, zwidth=thickness, otdf_type='steering_cyl', friendly_name='valve') affordance = vis.showPolyData(mesh, 'valve', color=[0.0, 1.0, 0.0], cls=affordanceitems.FrameAffordanceItem, parent=folder, alpha=1.0) frame = vis.showFrame(frame, 'valve frame', parent=affordance, visible=False, scale=radius) affordance.actor.SetUserTransform(frame.transform) affordance.setAffordanceParams(params) affordance.updateParamsFromActorTransform()
def drawPolytope(msg): _id = msg.id if msg.remove: om.removeFromObjectModel(om.findObjectByName("IRIS polytopes")) return if msg.highlighted: color = [0.8, 0.2, 0.2] else: color = [0.8, 0.8, 0.8] name = "polytope {:d}".format(_id) obj = om.findObjectByName(name) if obj: om.removeFromObjectModel(obj) V = np.array(msg.V) polyData = vnp.numpyToPolyData(V.T.copy()) vol_mesh = filterUtils.computeDelaunay3D(polyData) debug = DebugData() debug.addPolyData(vol_mesh) vis.showPolyData(debug.getPolyData(), name, color=color, alpha=0.4, parent="IRIS polytopes")