def draw(self): d = DebugData() points = list(self.points) if self.hoverPos is not None: points.append(self.hoverPos) # draw points radius = 5 scale = (2 * self.view.camera().GetParallelScale()) / ( self.view.renderer().GetSize()[1]) for p in points: d.addSphere(p, radius=radius * scale) if self.drawLines and len(points) > 1: for a, b in zip(points, points[1:]): d.addLine(a, b) # connect end points # d.addLine(points[0], points[-1]) if self.annotationObj: self.annotationObj.setPolyData(d.getPolyData()) else: self.annotationObj = vis.updatePolyData( d.getPolyData(), "annotation", parent="segmentation", color=[1, 0, 0], view=self.view, ) self.annotationObj.addToView(self.view) self.annotationObj.actor.SetPickable(False) self.annotationObj.actor.GetProperty().SetLineWidth(2)
def draw(self): d = DebugData() points = [p if p is not None else self.hoverPos for p in self.points] # draw points for p in points: if p is not None: d.addSphere(p, radius=0.008) if self.drawLines: # draw lines for a, b in zip(points, points[1:]): if b is not None: d.addLine(a, b) # connect end points if points[-1] is not None and self.drawClosedLoop: d.addLine(points[0], points[-1]) self.annotationObj = vis.updatePolyData( d.getPolyData(), self.annotationName, parent=self.annotationFolder, view=self.view, ) self.annotationObj.setProperty("Color", [1, 0, 0]) self.annotationObj.actor.SetPickable(False)
def drawCenterOfMass(model): stanceFrame = robotSystem.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 update(self): t = self.frameProvider.getFrame("MULTISENSE_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 makeSphere(self, position, radius=0.0075): d = DebugData() d.addSphere(position, radius=radius) return d.getPolyData()
def updateGeometryFromProperties(self): d = DebugData() d.addSphere((0, 0, 0), self.getProperty("Radius")) self.setPolyData(d.getPolyData())
def newWalkingGoal(self, displayPoint, view): # put walking goal at robot's base mainLink = drcargs.getRobotConfig(self.robotName)["pelvisLink"] footFrame = self.robotModel.getLinkFrame(mainLink) if not footFrame: print( "ERROR: The link '{}' provided for the key 'pelvisLink' in the configuration file does not exist in " "the robot's URDF. Cannot place walking goal.".format(mainLink) ) return worldPt1, worldPt2 = vis.getRayFromDisplayPoint(view, displayPoint) groundOrigin = footFrame.GetPosition() groundNormal = [0.0, 0.0, 1.0] selectedGroundPoint = [0.0, 0.0, 0.0] t = vtk.mutable(0.0) vtk.vtkPlane.IntersectWithLine( worldPt1, worldPt2, groundNormal, groundOrigin, t, selectedGroundPoint ) walkingTarget = transformUtils.frameFromPositionAndRPY( selectedGroundPoint, np.array(footFrame.GetOrientation()) ) frameObj = vis.updateFrame( walkingTarget, self.robotName + " 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() 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 " + self.robotName, parent=frameObj, visible=True, color=[1, 1, 0], ) handle.actor.SetUserTransform(frameObj.transform) placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain) def onFramePropertyModified(propertySet, propertyName): if propertyName == "Edit": if propertySet.getProperty(propertyName): placer.start() else: placer.stop() frameObj.properties.connectPropertyChanged(onFramePropertyModified) onFramePropertyModified(frameObj, "Edit") frameObj.connectFrameModified(self.onWalkingGoalModified)