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 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 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, locator="None"): if locator == "None": locator = self.locator 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] 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]) else: d.addLine(origin, origin + rayTransformed * self.Sensor.rayLength, color=colorMaxRange) vis.updatePolyData(d.getPolyData(), "rays", colorByName="RGB255")
def drawActionSetFull(self, go_nowhere=False): # print "I am drawing the action set" d = DebugData() for index, value in enumerate(self.pos_trajectories): for time_step_index in xrange(2 * self.numPointsToDraw - 1): firstX = value[0, time_step_index] firstY = value[1, time_step_index] firstZ = value[2, time_step_index] secondX = value[0, time_step_index + 1] secondY = value[1, time_step_index + 1] secondZ = value[2, time_step_index + 1] firstEndpt = (firstX, firstY, firstZ) secondEndpt = (secondX, secondY, secondZ) if time_step_index >= 10: color = [0.8, 0, 0.8] else: color = [0.1, 0.1, 1.0] if go_nowhere: firstEndpt = [0, 0, 0] secondEndpt = [0, 0, 0.001] d.addLine(firstEndpt, secondEndpt, radius=0.02, color=color) obj = vis.updatePolyData(d.getPolyData(), "action_set", colorByName="RGB255")
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 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 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 = [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 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() # Iterate over all triangles, drawing the contact surface as a triangle # outlined in blue. for surface in msg.hydroelastic_contacts: 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]]) d.addLine(p1=va, p2=vb, radius=0, color=[0, 0, 1]) d.addLine(p1=vb, p2=vc, radius=0, color=[0, 0, 1]) d.addLine(p1=va, p2=vc, radius=0, color=[0, 0, 1]) key = (str(surface.body1_name), str(surface.body2_name)) vis.showPolyData( d.getPolyData(), str(key), parent=folder, color=[0, 0, 1])
def buildGlobalGoal(): #print "building circle world" d = DebugData() worldXmin = 25 worldXmax = 50 worldYmin = -20 worldYmax = 20 firstX = worldXmin + np.random.rand()*(worldXmax-worldXmin) firstY = worldYmin + np.random.rand()*(worldYmax-worldYmin) firstEndpt = (firstX,firstY,0.2) secondEndpt = (firstX,firstY,-0.2) #d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn()) d.addLine(firstEndpt, secondEndpt, radius=0.3, color=[0.5,1,0]) obj = vis.updatePolyData(d.getPolyData(), 'global_goal', colorByName='RGB255') world = World() world.visObj = obj world.global_goal_x = firstX world.global_goal_y = firstY return world
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 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 updateDrawPolyApprox(self, frame): distances = self.Sensor.raycastAll(frame) polyCoefficients = self.SensorApproximator.polyFitConstrainedLP(distances) if polyCoefficients == None: polyCoefficients = [0,0] d = DebugData() x = self.SensorApproximator.approxThetaVector y = x * 0.0 for index,val in enumerate(y): y[index] = self.horner(x[index],polyCoefficients) origin = np.array(frame.transform.GetPosition()) origin[2] = -0.001 for i in xrange(self.SensorApproximator.numApproxPoints): if y[i] > 0: ray = self.SensorApproximator.approxRays[:,i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) intersection = origin + rayTransformed * y[i] intersection[2] = -0.001 d.addLine(origin, intersection, color=[0,0,1]) vis.updatePolyData(d.getPolyData(), 'polyApprox', colorByName='RGB255')
def to_polydata(self, bottle_detected=False): """Converts the sensor to polydata.""" d = DebugData() origin = np.array([self._state[0], self._state[1], self.z_distance]) for hit, intersection, ray in zip(self._hit, self._intersections, self._rays): if hit: color = [1., 0.45882353, 0.51372549] endpoint = intersection else: color = [0., 0.6, 0.58823529] endpoint = origin + ray d.addLine(origin, endpoint, color=color, radius=0.05) # add vision sensor center = [self._state[0], self._state[1], 0] axis = [0, 0, 1] if bottle_detected: color = [0., 0.8, 0.] else: color = [0., 0.6, 0.58823529] d.addCircle(center, axis, 5, color=color) return d.getPolyData()
def updateDrawNormals(self, frame): leftDistances = self.raycastLeftOrRight(frame, self.leftRays) rightDistances = self.raycastLeftOrRight(frame, self.rightRays) d = DebugData() x = self.SensorApproximator.approxThetaVector y = x * 0.0 for index,val in enumerate(y): y[index] = self.horner(x[index],polyCoefficients) origin = np.array(frame.transform.GetPosition()) origin[2] = -0.001 for i in xrange(self.numRays): #ray = self.SensorApproximator.approxRays[:,i] #rayTransformed = np.array(frame.transform.TransformNormal(ray)) #intersection = origin + rayTransformed * y[i] #intersection[2] = -0.001 p1 = leftDistances[i] p2 = rightDistances[i] d.addLine(p1, p2, color=[0,0.1,1]) vis.updatePolyData(d.getPolyData(), 'polyApprox', colorByName='RGB255')
def buildGlobalGoal(scale): #print "building circle world" d = DebugData() worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d, scale=scale, boundaryType="Square", withData=False) #print "boundaries done" firstX = worldXmin + np.random.rand()*(worldXmax-worldXmin) firstY = worldYmin + np.random.rand()*(worldYmax-worldYmin) firstEndpt = (firstX,firstY,0.2) secondEndpt = (firstX,firstY,-0.2) #d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn()) d.addLine(firstEndpt, secondEndpt, radius=1.0, color=[0.5,1,0]) obj = vis.updatePolyData(d.getPolyData(), 'global_goal', colorByName='RGB255') world = World() world.visObj = obj world.global_goal_x = firstX world.global_goal_y = firstY print "I should have actually built a goal" return world
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 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 _get_line(self, x, y, num, color): data = DebugData() data.addLine(x, y, radius=0.7, color=[1, 1, 1]) polydata = data.getPolyData() self._add_polydata(polydata, "line" + str(num), color) return polydata
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 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 buildLineSegmentTestWorld(percentObsDensity, nonRandom=False, circleRadius=3, scale=1.0, randomSeed=5, obstaclesInnerFraction=1.0): #print "building circle world" if nonRandom: np.random.seed(randomSeed) d = DebugData() worldXmin = 3 worldXmax = 15 worldYmin = -10 worldYmax = 10 #print "boundaries done" worldArea = (worldXmax-worldXmin)*(worldYmax-worldYmin) #print worldArea obsScalingFactor = 5.0/12.0 maxNumObstacles = obsScalingFactor * worldArea numObstacles = int(obstaclesInnerFraction**2 * percentObsDensity/100.0 * maxNumObstacles) print numObstacles, " is num obstacles" # 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) secondX = obsXmin + np.random.rand()*(obsXmax-obsXmin) secondY = 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=0.5) obj = vis.updatePolyData(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 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 drawActionSetEmpty(self): # print "I am drawing the action set" d = DebugData() for index, value in enumerate(self.pos_trajectories): for time_step_index in xrange(2 * self.numPointsToDraw - 1): d.addLine([0, 0, 0], [0, 0, 0], radius=0.01, color=[1, 1, 1]) obj = vis.updatePolyData(d.getPolyData(), "action_set", colorByName="RGB255")
def buildCircleWorld(percentObsDensity, nonRandom=False, circleRadius=3, scale=None, randomSeed=5, obstaclesInnerFraction=1.0, alpha=0.0): #print "building circle world" list_of_circles = [] if nonRandom: np.random.seed(randomSeed) d = DebugData() worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d, scale=scale, boundaryType="Square", alpha=alpha) #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) list_of_circles.append( (firstX, firstY) ) 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', alpha=alpha) world = World() world.visObj = obj world.Xmax = worldXmax world.Xmin = worldXmin world.Ymax = worldYmax world.Ymin = worldYmin world.numObstacles = numObstacles world.percentObsDensity = percentObsDensity world.list_of_circles = list_of_circles return world
def drawModel(self, model): modelFolder = om.getOrCreateContainer(model.name, parentObj=self.getRootFolder()) markerFolder = om.getOrCreateContainer('markers', parentObj=modelFolder) modelName = model.name markerObjects = markerFolder.children() if len(markerObjects) != model.nummarkers: for obj in markerObjects: om.removeFromObjectModel(obj) modelColor = vis.getRandomColor() markerObjects = self.createMarkerObjects(model.nummarkers, markerFolder, modelName, modelColor) self.models[modelName] = markerObjects if len(markerObjects): modelColor = markerObjects[0].getProperty('Color') for i, marker in enumerate(model.markers): xyz = np.array(marker.xyz) * self.unitConversion markerFrame = vtk.vtkTransform() markerFrame.Translate(xyz) markerObjects[i].getChildFrame().copyFrame(markerFrame) if self.drawEdges: d = DebugData() for m1 in model.markers: xyz = np.array(m1.xyz) * self.unitConversion for m2 in model.markers: xyz2 = np.array(m2.xyz) * self.unitConversion d.addLine(xyz, xyz2) edges = shallowCopy(d.getPolyData()) vis.updatePolyData(edges, modelName + ' edges', color=modelColor, parent=modelFolder) else: edgesObj = om.findObjectByName(modelName + ' edges') om.removeFromObjectModel(edgesObj) computeModelFrame = True if computeModelFrame: pos = np.array(model.segments[0].T) * self.unitConversion rpy = np.array(model.segments[0].A) modelFrame = transformUtils.frameFromPositionAndRPY( pos, np.degrees(rpy)) vis.updateFrame(modelFrame, modelName + ' frame', parent=modelFolder, scale=0.1)
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 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 drawActionSetFinal(self): # print "I am drawing the action set" d = DebugData() for x_index, x_value in enumerate(self.p_x_final): for y_index, y_value in enumerate(self.p_y_final): firstEndpt = (0.0, 0.0, 0.0) secondEndpt = (x_value, y_value, 0.0) d.addLine(firstEndpt, secondEndpt, radius=0.02, color=[0.8, 0, 0.8]) obj = vis.updatePolyData(d.getPolyData(), "action_set", colorByName="RGB255")
def getCameraFrustumMesh(view, rayLength=1.0): origin = np.array(view.camera().GetPosition()) def getCameraRay(displayPoint): _, ray = vis.getRayFromDisplayPoint(view, displayPoint) ray = ray-origin ray /= np.linalg.norm(ray) return ray viewWidth, viewHeight = view.renderWindow().GetSize() rays = [getCameraRay(x) for x in [[0.0, 0.0], [viewWidth, 0.0], [viewWidth, viewHeight], [0.0, viewHeight]]] rays = [rayLength*r for r in rays] camPos = origin lineRadius = 0.0 color = [1.0, 1.0, 1.0] d = DebugData() d.addLine(camPos, camPos+rays[0], radius=lineRadius, color=color) d.addLine(camPos, camPos+rays[1], radius=lineRadius, color=color) d.addLine(camPos, camPos+rays[2], radius=lineRadius, color=color) d.addLine(camPos, camPos+rays[3], radius=lineRadius, color=color) d.addLine(camPos+rays[0], camPos+rays[1], radius=lineRadius, color=color) d.addLine(camPos+rays[1], camPos+rays[2], radius=lineRadius, color=color) d.addLine(camPos+rays[2], camPos+rays[3], radius=lineRadius, color=color) d.addLine(camPos+rays[3], camPos+rays[0], radius=lineRadius, color=color) pd = d.getPolyData() return pd
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 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 updateDrawIntersectionManual(self, frame): print "I am getting called too" d = DebugData() originHigher = np.array(frame.transform.GetPosition()) originHigher[2] = 1.0 sliderIdx = self.slider.value for i in xrange(self.Sensor.numRays): ray = self.Sensor.rays[:,i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) distance = self.raycastDataManual[sliderIdx,i] d.addLine(originHigher, originHigher+rayTransformed*distance, color=[0,1,1]) vis.updatePolyData(d.getPolyData(), 'raysManual', colorByName='RGB255')
def debugFill(self): d = DebugData() print "got here" for i in xrange(self.numX): print "and here" for j in xrange(self.numY): print "and even here" for k in xrange(self.numZ): firstEndpt = (i, j, k - 0.1) secondEndpt = (i, j, k + 0.1) d.addLine(firstEndpt, secondEndpt, radius=0.2) obj = vis.showPolyData(d.getPolyData(), "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 to_polydata(self): """Converts the sensor to polydata.""" d = DebugData() origin = np.array([self._state[0], self._state[1], 0]) for hit, intersection, ray in zip(self._hit, self._intersections, self._rays): if hit: color = [1., 0.45882353, 0.51372549] endpoint = intersection else: color = [0., 0.6, 0.58823529] endpoint = origin + ray d.addLine(origin, endpoint, color=color, radius=0.05) return d.getPolyData()
def updateCameraMesh(): scale = cameraObj.getChildFrame().getProperty('Scale') * 10.0 rayLength = scale d = DebugData() d.addCube(dimensions=[0.04, 0.08, 0.06], center=[-0.02, 0.0, 0.0], color=[1,0.5,0]) d.addLine([0.0, 0.0, 0.0], [0.01, 0.0, 0.0], radius=0.023, color=[1,0.5,0]) cameraModelMesh = d.getPolyData() t = vtk.vtkTransform() t.Scale(scale, scale, scale) cameraModelMesh = filterUtils.transformPolyData(cameraModelMesh, t) cameraMesh = getCameraFrustumMesh(depthScanner.view, rayLength) cameraMesh = filterUtils.transformPolyData(cameraMesh, getCameraTransform(depthScanner.view.camera()).GetLinearInverse()) cameraMesh = filterUtils.appendPolyData([cameraMesh, cameraModelMesh]) cameraObj.setPolyData(cameraMesh)
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 drawModel(self, model): modelFolder = om.getOrCreateContainer(model.name, parentObj=self.getRootFolder()) markerFolder = om.getOrCreateContainer("markers", parentObj=modelFolder) modelName = model.name markerObjects = markerFolder.children() if len(markerObjects) != model.nummarkers: for obj in markerObjects: om.removeFromObjectModel(obj) modelColor = vis.getRandomColor() markerObjects = self.createMarkerObjects(model.nummarkers, markerFolder, modelName, modelColor) self.models[modelName] = markerObjects if len(markerObjects): modelColor = markerObjects[0].getProperty("Color") for i, marker in enumerate(model.markers): xyz = np.array(marker.xyz) * self.unitConversion markerFrame = vtk.vtkTransform() markerFrame.Translate(xyz) markerObjects[i].getChildFrame().copyFrame(markerFrame) if self.drawEdges: d = DebugData() for m1 in model.markers: xyz = np.array(m1.xyz) * self.unitConversion for m2 in model.markers: xyz2 = np.array(m2.xyz) * self.unitConversion d.addLine(xyz, xyz2) edges = shallowCopy(d.getPolyData()) vis.updatePolyData(edges, modelName + " edges", color=modelColor, parent=modelFolder) else: edgesObj = om.findObjectByName(modelName + " edges") om.removeFromObjectModel(edgesObj) computeModelFrame = True if computeModelFrame: pos = np.array(model.segments[0].T) * self.unitConversion rpy = np.array(model.segments[0].A) modelFrame = transformUtils.frameFromPositionAndRPY(pos, np.degrees(rpy)) vis.updateFrame(modelFrame, modelName + " frame", parent=modelFolder, scale=0.1)
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 drawFirstIntersections(self, frame, firstRaycast): origin = np.array(frame.transform.GetPosition()) d = DebugData() firstRaycastLocations = self.Sensor.invertRaycastsToLocations(self.frame, firstRaycast) for i in xrange(self.Sensor.numRays): endpoint = firstRaycastLocations[i,:] if firstRaycast[i] == 20.0: d.addLine(origin, endpoint, color=[0,1,0]) else: d.addLine(origin, endpoint, color=[1,0,0]) vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255') self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations) self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData) self.Sensor.setLocator(self.LineSegmentLocator)
def buildFixedTriangleWorld(percentObsDensity): print "building fixed triangle 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.1) 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 drawEndEffectorTrajFromPlan(plan=None): if plan is None: plan = robotSystem.ikPlanner.lastManipPlan linkName = getEndEffectorLinkName() frames = getLinkFrameSamplesFromPlan(plan, linkName) pointInFrame = np.array(getGraspToHandLink().GetPosition()) pts = [] for f in frames: p = np.array(f.TransformPoint(pointInFrame)) pts.append(p) d = DebugData() for p1, p2 in zip(pts, pts[1:]): d.addLine(p1, p2) vis.updatePolyData(d.getPolyData(), 'end effector traj', parent='debug')
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 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 visualizeTwoStepEstimateSingleContact(self, data): # draw the contact ray if that option is set if self.options['twoStepEstimator']['showContactRay']: d = DebugData() d.addLine(data['contactRay']['rayOriginInWorld'], data['contactRay']['rayEndInWorld'], radius=0.005) color = data['contactRay']['color'] visName = data['contactRay']['visObjectName'] obj = vis.updatePolyData(d.getPolyData(), visName, color=color) self.visObjectDrawTime[visName] = time.time() # draw the actual contact point if it exists if data['pt'] is not None: visName = data['linkName'] + " active link estimated external force" self.drawForce(visName, data['linkName'], data['pt'], data['force'], color=[1, 0, 0]) self.visObjectDrawTime[visName] = time.time()
def updateCameraMesh(): scale = cameraObj.getChildFrame().getProperty('Scale') * 10.0 rayLength = scale d = DebugData() d.addCube(dimensions=[0.04, 0.08, 0.06], center=[-0.02, 0.0, 0.0], color=[1, 0.5, 0]) d.addLine([0.0, 0.0, 0.0], [0.01, 0.0, 0.0], radius=0.023, color=[1, 0.5, 0]) cameraModelMesh = d.getPolyData() t = vtk.vtkTransform() t.Scale(scale, scale, scale) cameraModelMesh = filterUtils.transformPolyData(cameraModelMesh, t) cameraMesh = getCameraFrustumMesh(depthScanner.view, rayLength) cameraMesh = filterUtils.transformPolyData( cameraMesh, getCameraTransform(depthScanner.view.camera()).GetLinearInverse()) cameraMesh = filterUtils.appendPolyData([cameraMesh, cameraModelMesh]) cameraObj.setPolyData(cameraMesh)
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 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()
for i, result in enumerate(chulls): planePoints, chull, plane = result.points, result.convexHull, result.plane c = segmentation.getRandomColor() vis.showPolyData(planePoints, 'plane %d' % i, color=c) chull = vis.showPolyData(chull, 'convex hull %d' % i, color=c) chull.setProperty('Surface Mode', 'Surface with edges') chull.actor.GetProperty().SetLineWidth(3) center = segmentation.computeCentroid(chull.polyData) chullPoints = vnp.getNumpyFromVtk(chull.polyData, 'Points') d.addLine(plane.GetOrigin(), np.array(plane.GetOrigin()) + 0.005 * np.array(plane.GetNormal()), radius=0.0001, color=[0, 0, 0]) #d.addArrow(plane.GetOrigin(), np.array(plane.GetOrigin()) + 0.01 * np.array(plane.GetNormal()), headRadius=0.001, tubeRadius=0.0002) #d.addSphere(chullPoints[0], radius=0.001, color=[1,0,0]) #d.addSphere(chullPoints[1], radius=0.001, color=[0,1,0]) vis.showPolyData(d.getPolyData(), 'plane normals', colorByName='RGB255') #saveConvexHulls(chulls, name) #vis.showPolyData(ioUtils.readPolyData(os.path.join(name, 'merged_planes.ply')), 'merged_planes') applogic.resetCamera([1, 1, 0]) applogic.setBackgroundColor([1, 1, 1]) view.orientationMarkerWidget().Off() app.gridObj.setProperty('Color', [0, 0, 0])
x = theta z = np.sin(theta) y = np.cos(theta) pts = np.vstack((x, y, z)).T.copy() pts /= np.max(pts) return pts app = consoleapp.ConsoleApp() view = app.createView() view.show() d = DebugData() 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)