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 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 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 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 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 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 main(): global app, view, nav_data nav_data = np.array([[0, 0, 0]]) lcmUtils.addSubscriber(".*_NAV$", node_nav_t, handleNavData) app = ConsoleApp() app.setupGlobals(globals()) app.showPythonConsole() view = app.createView() view.show() global d d = DebugData() d.addLine([0,0,0], [1,0,0], color=[0,1,0]) d.addSphere((0,0,0), radius=0.02, color=[1,0,0]) #vis.showPolyData(d.getPolyData(), 'my debug geometry', colorByName='RGB255') startSwarmVisualization() app.start()
def 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 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 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 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 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 main(): global app, view, nav_data nav_data = np.array([[0, 0, 0]]) lcmUtils.addSubscriber(".*_NAV$", node_nav_t, handleNavData) app = ConsoleApp() app.setupGlobals(globals()) app.showPythonConsole() view = app.createView() view.show() global d d = DebugData() d.addLine([0, 0, 0], [1, 0, 0], color=[0, 1, 0]) d.addSphere((0, 0, 0), radius=0.02, color=[1, 0, 0]) #vis.showPolyData(d.getPolyData(), 'my debug geometry', colorByName='RGB255') startSwarmVisualization() app.start()
def 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 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 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 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 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 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 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 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 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 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 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 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 updateDrawIntersection(frame): origin = np.array(frame.transform.GetPosition()) #print "origin is now at", origin d = DebugData() for i in xrange(numRays): ray = rays[:,i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) #print "rayTransformed is", rayTransformed intersection = computeIntersection(locator, origin, origin + rayTransformed*rayLength) if intersection is not None: d.addLine(origin, intersection, color=[1,0,0]) else: d.addLine(origin, origin+rayTransformed*rayLength, color=[0,1,0]) vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
def buildStickWorld(percentObsDensity): print "building stick world" d = DebugData() worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d) print "boundaries done" worldArea = (worldXmax-worldXmin)*(worldYmax-worldYmin) print worldArea obsScalingFactor = 1.0/12.0 maxNumObstacles = obsScalingFactor * worldArea numObstacles = int(percentObsDensity/100.0 * maxNumObstacles) print numObstacles # draw random stick obstacles obsLength = 2.0 for i in xrange(numObstacles): firstX = worldXmin + np.random.rand()*(worldXmax-worldXmin) firstY = worldYmin + np.random.rand()*(worldYmax-worldYmin) firstEndpt = (firstX,firstY,0) randTheta = np.random.rand() * 2.0*np.pi secondEndpt = (firstX+obsLength*np.cos(randTheta), firstY+obsLength*np.sin(randTheta), 0) d.addLine(firstEndpt, secondEndpt, radius=0.2) 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 updateDrawIntersection(self, frame): origin = np.array(frame.transform.GetPosition()) #print "origin is now at", origin d = DebugData() colorMaxRange = [0,1,0] for i in xrange(self.sensor.numRays): ray = self.sensor.rays[:,i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) #print "rayTransformed is", rayTransformed intersection = self.sensor.raycast(self.locator, origin, origin + rayTransformed*self.sensor.rayLength) if intersection is not None: d.addLine(origin, intersection, color=[1,0,0]) else: d.addLine(origin, origin+rayTransformed*self.sensor.rayLength, color=colorMaxRange) vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
def buildStickWorld(percentObsDensity): print "building stick world" d = DebugData() worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d) print "boundaries done" worldArea = (worldXmax - worldXmin) * (worldYmax - worldYmin) print worldArea obsScalingFactor = 1.0 / 12.0 maxNumObstacles = obsScalingFactor * worldArea numObstacles = int(percentObsDensity / 100.0 * maxNumObstacles) print numObstacles # draw random stick obstacles obsLength = 2.0 for i in xrange(numObstacles): firstX = worldXmin + np.random.rand() * (worldXmax - worldXmin) firstY = worldYmin + np.random.rand() * (worldYmax - worldYmin) firstEndpt = (firstX, firstY, 0) randTheta = np.random.rand() * 2.0 * np.pi secondEndpt = (firstX + obsLength * np.cos(randTheta), firstY + obsLength * np.sin(randTheta), 0) d.addLine(firstEndpt, secondEndpt, radius=0.2) 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 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 buildHallwayWorld(): print "building hallway world" lineRadius = 0.2 d = DebugData() xMin = -10 xMax = 10 yMin = 0 yMax = 50 for x in [xMin, xMax]: lineStart = (x, yMin, 0) lineEnd = (x, yMax, 0) d.addLine(lineStart, lineEnd, radius=lineRadius) obj = vis.showPolyData(d.getPolyData(), 'world') world = World() world.visObj = obj return world
def updateDrawIntersection(frame): origin = np.array(frame.transform.GetPosition()) #print "origin is now at", origin d = DebugData() for i in xrange(numRays): ray = rays[:, i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) #print "rayTransformed is", rayTransformed intersection = computeIntersection(locator, origin, origin + rayTransformed * rayLength) if intersection is not None: d.addLine(origin, intersection, color=[1, 0, 0]) else: d.addLine(origin, origin + rayTransformed * rayLength, color=[0, 1, 0]) vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
def convertStepToSafeRegion(self, step, rpySeed): assert step.shape[0] >= 3 assert step.shape[1] == 3 shapeVertices = np.array(step).transpose()[:2,:] s = ddapp.terrain.PolygonSegmentationNonIRIS(shapeVertices, bot_pts=self.footContactPoints) stepCenter = np.mean(step, axis=0) startSeed = np.hstack([stepCenter, rpySeed]) r = s.findSafeRegion(startSeed) if r is not None: # draw step d = DebugData() for p1, p2 in zip(step, step[1:]): d.addLine(p1, p2) d.addLine(step[-1], step[0]) folder = om.getOrCreateContainer('Safe terrain regions') obj = vis.showPolyData(d.getPolyData(), 'step region %d' % len(folder.children()), parent=folder) obj.properties.addProperty('Enabled for Walking', True) obj.safe_region = r
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 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 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 spawnTargetAffordance(self): for obj in om.getObjects(): if obj.getProperty('Name') == 'target': om.removeFromObjectModel(obj) targetFrame = transformUtils.frameFromPositionAndRPY([0.6, 0.2, 0.6], [180, 0, 90]) folder = om.getOrCreateContainer('affordances') z = DebugData() z.addLine(np.array([0, 0, 0]), np.array([-self.boxLength, 0, 0]), radius=0.02) # main bar z.addLine(np.array([-self.boxLength, 0, 0]), np.array([-self.boxLength, 0, self.boxLength]), radius=0.02) # main bar z.addLine(np.array([-self.boxLength, 0, self.boxLength]), np.array([0, 0, self.boxLength]), radius=0.02) # main bar z.addLine(np.array([0, 0, self.boxLength]), np.array([0, 0, 0]), radius=0.02) # main bar targetMesh = z.getPolyData() self.targetAffordance = vis.showPolyData( targetMesh, 'target', color=[0.0, 1.0, 0.0], cls=affordanceitems.FrameAffordanceItem, parent=folder, alpha=0.3) self.targetAffordance.actor.SetUserTransform(targetFrame) self.targetFrame = vis.showFrame(targetFrame, 'target frame', parent=self.targetAffordance, visible=False, scale=0.2) self.targetFrame = self.targetFrame.transform params = dict(length=self.boxLength, otdf_type='target', friendly_name='target') self.targetAffordance.setAffordanceParams(params) self.targetAffordance.updateParamsFromActorTransform()
def spawnTargetAffordance(self): for obj in om.getObjects(): if obj.getProperty('Name') == 'target': om.removeFromObjectModel(obj) targetFrame = transformUtils.frameFromPositionAndRPY([0.6,0.2,0.6],[180,0,90]) folder = om.getOrCreateContainer('affordances') z = DebugData() z.addLine(np.array([0,0,0]), np.array([-self.boxLength,0,0]), radius=0.02) # main bar z.addLine(np.array([-self.boxLength,0,0]), np.array([-self.boxLength,0,self.boxLength]), radius=0.02) # main bar z.addLine(np.array([-self.boxLength,0,self.boxLength]), np.array([0,0,self.boxLength]), radius=0.02) # main bar z.addLine(np.array([0,0,self.boxLength]), np.array([0,0,0]), radius=0.02) # main bar targetMesh = z.getPolyData() self.targetAffordance = vis.showPolyData(targetMesh, 'target', color=[0.0, 1.0, 0.0], cls=affordanceitems.FrameAffordanceItem, parent=folder, alpha=0.3) self.targetAffordance.actor.SetUserTransform(targetFrame) self.targetFrame = vis.showFrame(targetFrame, 'target frame', parent=self.targetAffordance, visible=False, scale=0.2) self.targetFrame = self.targetFrame.transform params = dict(length=self.boxLength, otdf_type='target', friendly_name='target') self.targetAffordance.setAffordanceParams(params) self.targetAffordance.updateParamsFromActorTransform()
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 drawFootstepPlan(self, msg, folder, left_color=None, right_color=None, alpha=1.0): for step in folder.children(): om.removeFromObjectModel(step) allTransforms = [] volFolder = getWalkingVolumesFolder() map(om.removeFromObjectModel, volFolder.children()) slicesFolder = getTerrainSlicesFolder() map(om.removeFromObjectModel, slicesFolder.children()) for i, footstep in enumerate(msg.footsteps): trans = footstep.pos.translation trans = [trans.x, trans.y, trans.z] quat = footstep.pos.rotation quat = [quat.w, quat.x, quat.y, quat.z] footstepTransform = transformUtils.transformFromPose(trans, quat) allTransforms.append(footstepTransform) if i < 2: continue if footstep.is_right_foot: mesh = getRightFootMesh() if (right_color is None): color = getRightFootColor() else: color = right_color else: mesh = getLeftFootMesh() if (left_color is None): color = getLeftFootColor() else: color = left_color # add gradual shading to steps to indicate destination frac = float(i)/ float(msg.num_steps-1) this_color = [0,0,0] this_color[0] = 0.25*color[0] + 0.75*frac*color[0] this_color[1] = 0.25*color[1] + 0.75*frac*color[1] this_color[2] = 0.25*color[2] + 0.75*frac*color[2] if self.show_contact_slices: self.drawContactVolumes(footstepTransform, color) contact_pts_left, contact_pts_right = FootstepsDriver.getContactPts() if footstep.is_right_foot: sole_offset = np.mean(contact_pts_right, axis=0) else: sole_offset = np.mean(contact_pts_left, axis=0) t_sole_prev = transformUtils.frameFromPositionMessage(msg.footsteps[i-2].pos) t_sole_prev.PreMultiply() t_sole_prev.Translate(sole_offset) t_sole = transformUtils.copyFrame(footstepTransform) t_sole.Translate(sole_offset) yaw = np.arctan2(t_sole.GetPosition()[1] - t_sole_prev.GetPosition()[1], t_sole.GetPosition()[0] - t_sole_prev.GetPosition()[0]) T_terrain_to_world = transformUtils.frameFromPositionAndRPY([t_sole_prev.GetPosition()[0], t_sole_prev.GetPosition()[1], 0], [0, 0, math.degrees(yaw)]) path_dist = np.array(footstep.terrain_path_dist) height = np.array(footstep.terrain_height) # if np.any(height >= trans[2]): terrain_pts_in_local = np.vstack((path_dist, np.zeros(len(footstep.terrain_path_dist)), height)) d = DebugData() for j in range(terrain_pts_in_local.shape[1]-1): d.addLine(terrain_pts_in_local[:,j], terrain_pts_in_local[:,j+1], radius=0.01) obj = vis.showPolyData(d.getPolyData(), 'terrain slice', parent=slicesFolder, visible=slicesFolder.getProperty('Visible'), color=[.8,.8,.3]) obj.actor.SetUserTransform(T_terrain_to_world) renderInfeasibility = False if renderInfeasibility and footstep.infeasibility > 1e-6: d = DebugData() start = allTransforms[i-1].GetPosition() end = footstepTransform.GetPosition() d.addArrow(start, end, 0.02, 0.005, startHead=True, endHead=True) vis.showPolyData(d.getPolyData(), 'infeasibility %d -> %d' % (i-2, i-1), parent=folder, color=[1, 0.2, 0.2]) stepName = 'step %d' % (i-1) obj = vis.showPolyData(mesh, stepName, color=this_color, alpha=alpha, parent=folder) obj.setIcon(om.Icons.Feet) frameObj = vis.showFrame(footstepTransform, stepName + ' frame', parent=obj, scale=0.3, visible=False) obj.actor.SetUserTransform(footstepTransform) obj.addProperty('Support Contact Groups', footstep.params.support_contact_groups, attributes=om.PropertyAttributes(enumNames=['Whole Foot', 'Front 2/3', 'Back 2/3'])) obj.properties.setPropertyIndex('Support Contact Groups', 0) obj.footstep_index = i obj.footstep_property_callback = obj.properties.connectPropertyChanged(functools.partial(self.onFootstepPropertyChanged, obj)) self.drawContactPts(obj, footstep, color=this_color)
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 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)