def updatePlanFrames(self): if self.getViewMode() != 'frames': return numberOfSamples = self.getNumberOfSamples() meshes = self.planPlayback.getPlanPoseMeshes( self.plan, self.playbackJointController, self.playbackRobotModel, numberOfSamples) d = DebugData() startColor = [0.8, 0.8, 0.8] endColor = [85 / 255.0, 255 / 255.0, 255 / 255.0] colorFunc = scipy.interpolate.interp1d([0, numberOfSamples - 1], [startColor, endColor], axis=0, kind='slinear') for i, mesh in reversed(list(enumerate(meshes))): d.addPolyData(mesh, color=colorFunc(i)) pd = d.getPolyData() clean = vtk.vtkCleanPolyData() clean.SetInput(pd) clean.Update() pd = clean.GetOutput() self.planFramesObj = vis.updatePolyData(d.getPolyData(), 'robot plan', alpha=1.0, visible=False, colorByName='RGB255', parent='planning') self.showPlanFrames()
def 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 loadFeet(): meshDir = os.path.join(app.getDRCBase(), 'software/models/atlas_v3/meshes') meshes = [] for foot in ['l', 'r']: d = DebugData() d.addPolyData( io.readPolyData(os.path.join(meshDir, '%s_talus.stl' % foot))) d.addPolyData( io.readPolyData(os.path.join(meshDir, '%s_foot.stl' % foot))) meshes.append(d.getPolyData()) return meshes
def loadFootMeshes(): meshes = [] for i in range(0,2): d = DebugData() for footMeshFile in _footMeshFiles[i]: d.addPolyData(ioUtils.readPolyData( footMeshFile , computeNormals=True)) t = vtk.vtkTransform() t.Scale(0.98, 0.98, 0.98) pd = filterUtils.transformPolyData(d.getPolyData(), t) meshes.append(pd) return meshes
def drawPolytope(msg): _id = msg.id; if msg.remove: om.removeFromObjectModel(om.findObjectByName('IRIS polytopes')) return if msg.highlighted: color = [.8,.2,.2] else: color = [.8,.8,.8] name = 'polytope {:d}'.format(_id) obj = om.findObjectByName(name) if obj: om.removeFromObjectModel(obj) V = np.array(msg.V) polyData = vnp.numpyToPolyData(V.T.copy()) vol_mesh = filterUtils.computeDelaunay3D(polyData) debug = DebugData() debug.addPolyData(vol_mesh) vis.showPolyData(debug.getPolyData(), name, color=color, alpha=0.4, parent='IRIS polytopes')