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 newMesh(): d = DebugData() d.addArrow((0,0,0), (0,0,0.3)) pd = d.getPolyData() meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd) desc = dict(classname='MeshAffordanceItem', Name='test mesh', Filename=meshId, pose=((0.5,0.0,1.0), (1,0,0,0))) return affordanceManager.newAffordanceFromDescription(desc)
def updateGeometryFromProperties(self): d = DebugData() d.addCube( self.getProperty("Dimensions"), (0, 0, 0), subdivisions=self.getProperty("Subdivisions"), ) self.setPolyData(d.getPolyData())
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 updateGeometryFromProperties(self): d = DebugData() length = self.getProperty("Length") d.addCapsule( center=(0, 0, 0), axis=(0, 0, 1), length=self.getProperty("Length"), radius=self.getProperty("Radius"), ) self.setPolyData(d.getPolyData())
def 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 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 updateGeometryFromProperties(self): radius = self.getProperty("Radius") circlePoints = np.linspace(0, 2 * np.pi, self.getProperty("Segments") + 1) spokes = [(0.0, np.sin(x), np.cos(x)) for x in circlePoints] spokes = [radius * np.array(x) / np.linalg.norm(x) for x in spokes] d = DebugData() for a, b in zip(spokes, spokes[1:]): d.addCapsule( center=(a + b) / 2.0, axis=(b - a), length=np.linalg.norm(b - a), radius=self.getProperty("Tube Radius"), ) self.setPolyData(d.getPolyData())
def onSpawnMesh(self): d = DebugData() d.addArrow((0, 0, 0), (0, 0, 0.3)) pd = d.getPolyData() meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd) pose = transformUtils.poseFromTransform(self.getSpawnFrame()) desc = dict( classname="MeshAffordanceItem", Name="mesh", Filename=meshId, uuid=newUUID(), pose=pose, ) return self.affordanceManager.newAffordanceFromDescription(desc)
def updateCursor(self, displayPoint): center = self.displayPointToImagePoint(displayPoint, restrictToImageDimensions=False) center = np.array(center) d = DebugData() d.addLine(center + [0, -3000, 0], center + [0, 3000, 0]) d.addLine(center + [-3000, 0, 0], center + [3000, 0, 0]) self.cursorObj = vis.updatePolyData(d.getPolyData(), "cursor", alpha=0.5, view=self.view) self.cursorObj.addToView(self.view) self.cursorObj.actor.SetUseBounds(False) self.cursorObj.actor.SetPickable(False) self.view.render()
def updateGeometryFromProperties(self): filename = self.getProperty("Filename") if not filename: polyData = vtk.vtkPolyData() else: polyData = self.getMeshManager().get(filename) if not polyData: if not os.path.isabs(filename): filename = os.path.join(director.getDRCBaseDir(), filename) if os.path.isfile(filename): polyData = ioutils.readPolyData(filename) else: # use axes as a placeholder mesh d = DebugData() d.addFrame(vtk.vtkTransform(), scale=0.1, tubeRadius=0.005) polyData = d.getPolyData() self.setPolyData(polyData)
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 updatePlanFrames(self): if self.getViewMode() != "frames": return numberOfSamples = self.getNumberOfSamples() meshes = self.planPlayback.getPlanPoseMeshes( self.plan, self.playbackJointController, self.playbackRobotModel, numberOfSamples, ) d = DebugData() startColor = [0.8, 0.8, 0.8] endColor = [85 / 255.0, 255 / 255.0, 255 / 255.0] colorFunc = scipy.interpolate.interp1d( [0, numberOfSamples - 1], [startColor, endColor], axis=0, kind="slinear" ) for i, mesh in reversed(list(enumerate(meshes))): d.addPolyData(mesh, color=colorFunc(i)) pd = d.getPolyData() clean = vtk.vtkCleanPolyData() clean.SetInput(pd) clean.Update() pd = clean.GetOutput() self.planFramesObj = vis.updatePolyData( d.getPolyData(), "robot plan", alpha=1.0, visible=False, colorByName="RGB255", parent="planning", ) self.showPlanFrames()
planePoints = segmentation.thresholdPoints(polyData, 'plane_segmentation_labels', [i, i]) if reorientNormals: normals = vnp.getNumpyFromVtk(planePoints, 'original_normals') expectedNormal = normals[0] else: expectedNormal = None chulls.append(computePlanarConvexHull(planePoints, expectedNormal=expectedNormal)) if removeGround: chulls.append(computePlanarConvexHull(groundPoints, expectedNormal=[0,0,1])) d = DebugData() 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)
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)
def makeSphere(self, position, radius=0.0075): d = DebugData() d.addSphere(position, radius=radius) return d.getPolyData()
def makeRobotPolyData(): d = DebugData() d.addCone(origin=(0,0,0), normal=(1,0,0), radius=0.2, height=0.3) return d.getPolyData()
def updateGeometryFromProperties(self): d = DebugData() d.addSphere((0, 0, 0), self.getProperty("Radius")) self.setPolyData(d.getPolyData())
def getMergedConvexHullsMesh(chulls): d = DebugData() for i, chull in enumerate(chulls): d.addPolyData(chull.convexHull, color=segmentation.getRandomColor()) return d.getPolyData()