def transformPolyData(polyData, transform):
t = vtk.vtkTransformPolyDataFilter()
t.SetTransform(transform)
t.SetInput(shallowCopy(polyData))
t.Update()
return shallowCopy(t.GetOutput())
def transformPolyData(polyData, transform):
t = vtk.vtkTransformPolyDataFilter()
t.SetTransform(transform)
t.SetInput(shallowCopy(polyData))
t.Update()
return shallowCopy(t.GetOutput())
def createTexturedPlane():
source = vtk.vtkPlaneSource()
textureMap = vtk.vtkTextureMapToPlane()
textureMap.SetInput(source.GetOutput())
textureMap.Update()
return shallowCopy(textureMap.GetOutput())
def createTexturedPlane():
source = vtk.vtkPlaneSource()
textureMap = vtk.vtkTextureMapToPlane()
textureMap.SetInput(source.GetOutput())
textureMap.Update()
return shallowCopy(textureMap.GetOutput())
def appendPolyData(polyDataList):
assert len(polyDataList)
append = vtk.vtkAppendPolyData()
for polyData in polyDataList:
append.AddInput(polyData)
append.Update()
return shallowCopy(append.GetOutput())
def toggleFootstepWidget(displayPoint, view, useHorizontalWidget=False):
obj, _ = vis.findPickedObject(displayPoint, view)
if not obj:
return False
name = obj.getProperty('Name')
if name in ('footstep widget', 'footstep widget frame'):
om.removeFromObjectModel(om.findObjectByName('footstep widget'))
return True
match = re.match('^step (\d+)$', name)
if not match:
return False
stepIndex = int(match.group(1))
existingWidget = om.findObjectByName('footstep widget')
if existingWidget:
previousStep = existingWidget.stepIndex
print 'have existing widget for step:', stepIndex
om.removeFromObjectModel(existingWidget)
if previousStep == stepIndex:
print 'returning because widget was for selected step'
return True
footMesh = shallowCopy(obj.polyData)
footFrame = transformUtils.copyFrame(obj.getChildFrame().transform)
if useHorizontalWidget:
rpy = [0.0, 0.0, transformUtils.rollPitchYawFromTransform(footFrame)[2]]
footFrame = transformUtils.frameFromPositionAndRPY(footFrame.GetPosition(), np.degrees(rpy))
footObj = vis.showPolyData(footMesh, 'footstep widget', parent='planning', alpha=0.2)
footObj.stepIndex = stepIndex
frameObj = vis.showFrame(footFrame, 'footstep widget frame', parent=footObj, scale=0.2)
footObj.actor.SetUserTransform(frameObj.transform)
footObj.setProperty('Color', obj.getProperty('Color'))
frameObj.setProperty('Edit', True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
walkGoal = om.findObjectByName('walking goal')
if walkGoal:
walkGoal.setProperty('Edit', False)
def onFootWidgetChanged(frame):
footstepsDriver.onStepModified(stepIndex - 1, frame)
frameObj.connectFrameModified(onFootWidgetChanged)
return True
def appendPolyData(polyDataList):
assert len(polyDataList)
append = vtk.vtkAppendPolyData()
for polyData in polyDataList:
append.AddInput(polyData)
append.Update()
return shallowCopy(append.GetOutput())
def thresholdPoints(polyData, arrayName, thresholdRange):
assert(polyData.GetPointData().GetArray(arrayName))
f = vtk.vtkThresholdPoints()
f.SetInput(polyData)
f.ThresholdBetween(thresholdRange[0], thresholdRange[1])
f.SetInputArrayToProcess(0,0,0, vtk.vtkDataObject.FIELD_ASSOCIATION_POINTS, arrayName)
f.Update()
return shallowCopy(f.GetOutput())
def makeSphere(radius, resolution):
s = vtk.vtkSphereSource()
s.SetThetaResolution(resolution)
s.SetPhiResolution(resolution)
s.SetRadius(radius)
s.SetEndPhi(85)
s.Update()
return shallowCopy(s.GetOutput())
def makeSphere(radius, resolution):
s = vtk.vtkSphereSource()
s.SetThetaResolution(resolution)
s.SetPhiResolution(resolution)
s.SetRadius(radius)
s.SetEndPhi(85)
s.Update()
return shallowCopy(s.GetOutput())
def updateSwarm():
global nav_cloud
if not nav_cloud_obj.initialized:
nav_cloud_obj.mapper.SetColorModeToMapScalars()
nav_cloud_obj.initialized = True
#print nav_data.shape[0], nav_cloud.GetNumberOfPoints()
nav_cloud = vtknp.getVtkPolyDataFromNumpyPoints(nav_data)
nav_cloud_obj.setPolyData(shallowCopy(nav_cloud))
def updateSwarm():
global nav_cloud
if not nav_cloud_obj.initialized:
nav_cloud_obj.mapper.SetColorModeToMapScalars()
nav_cloud_obj.initialized = True
#print nav_data.shape[0], nav_cloud.GetNumberOfPoints()
nav_cloud = vtknp.getVtkPolyDataFromNumpyPoints(nav_data)
nav_cloud_obj.setPolyData(shallowCopy(nav_cloud))
def thresholdPoints(polyData, arrayName, thresholdRange):
assert (polyData.GetPointData().GetArray(arrayName))
f = vtk.vtkThresholdPoints()
f.SetInput(polyData)
f.ThresholdBetween(thresholdRange[0], thresholdRange[1])
f.SetInputArrayToProcess(0, 0, 0,
vtk.vtkDataObject.FIELD_ASSOCIATION_POINTS,
arrayName)
f.Update()
return shallowCopy(f.GetOutput())
def computeDelaunay3D(polyData):
f = vtk.vtkDelaunay3D()
f.SetInput(polyData)
f.SetOffset(100.0)
f.Update()
surface = vtk.vtkGeometryFilter()
surface.SetInput(f.GetOutput())
surface.Update()
clean = vtk.vtkCleanPolyData()
clean.SetInput(surface.GetOutput())
clean.Update()
return shallowCopy(clean.GetOutput())
def loadMultiBlockMeshes(filename):
reader = vtk.vtkXMLMultiBlockDataReader()
reader.SetFileName(filename)
reader.Update()
polyDataList = []
mb = reader.GetOutput()
for i in xrange(mb.GetNumberOfBlocks()):
polyData = vtk.vtkPolyData.SafeDownCast(mb.GetBlock(i))
if polyData and polyData.GetNumberOfPoints():
polyDataList.append(shallowCopy(polyData))
Geometry.loadTextureForMesh(polyData, filename)
return polyDataList
def computeDelaunay3D(polyData):
f = vtk.vtkDelaunay3D()
f.SetInput(polyData)
f.SetOffset(100.0)
f.Update()
surface = vtk.vtkGeometryFilter()
surface.SetInput(f.GetOutput())
surface.Update()
clean = vtk.vtkCleanPolyData()
clean.SetInput(surface.GetOutput())
clean.Update()
return shallowCopy(clean.GetOutput())
def decodePolyData(data):
'''Given a numpy int8 array, deserializes the data to construct a new
vtkPolyData object and returns the result.'''
if not hasattr(vtk, 'vtkCommunicator'):
r = vtk.vtkPolyDataReader()
r.ReadFromInputStringOn()
r.SetInputString(str(data.data))
r.Update()
return shallowCopy(r.GetOutput())
charArray = vnp.getVtkFromNumpy(data)
assert isinstance(charArray, vtk.vtkCharArray)
polyData = vtk.vtkPolyData()
vtk.vtkCommunicator.UnMarshalDataObject(charArray, polyData)
return polyData
def decodePolyData(data):
'''Given a numpy int8 array, deserializes the data to construct a new
vtkPolyData object and returns the result.'''
if not hasattr(vtk, 'vtkCommunicator'):
r = vtk.vtkPolyDataReader()
r.ReadFromInputStringOn()
r.SetInputString(str(data.data))
r.Update()
return shallowCopy(r.GetOutput())
charArray = vnp.getVtkFromNumpy(data)
assert isinstance(charArray, vtk.vtkCharArray)
polyData = vtk.vtkPolyData()
vtk.vtkCommunicator.UnMarshalDataObject(charArray, polyData)
return polyData
def clipRange(dataObj, arrayName, thresholdRange):
if not dataObj.GetPointData().GetArray(arrayName):
raise Exception('clipRange: could not locate array: %s' % arrayName)
dataObj.GetPointData().SetScalars(dataObj.GetPointData().GetArray(arrayName))
f = vtk.vtkClipPolyData()
f.SetInput(dataObj)
f.SetValue(thresholdRange[0])
f.SetInputArrayToProcess(0, 0, 0, vtk.vtkDataObject.FIELD_ASSOCIATION_POINTS, arrayName)
f2 = vtk.vtkClipPolyData()
f2.AddInputConnection(f.GetOutputPort())
f2.SetValue(thresholdRange[1])
f2.InsideOutOn()
f2.SetInputArrayToProcess(0, 0, 0, vtk.vtkDataObject.FIELD_ASSOCIATION_POINTS, arrayName)
f2.Update()
return shallowCopy(f2.GetOutput())
def __init__(self, view, _PointCloudQueue):
self.view = view
self.PointCloudQueue = _PointCloudQueue
self.visible = True
self.p = vtk.vtkPolyData()
utime = PointCloudQueue.getPointCloudFromPointCloud(self.p)
self.polyDataObj = vis.PolyDataItem('pointcloud source', shallowCopy(self.p), view)
self.polyDataObj.actor.SetPickable(1)
self.polyDataObj.initialized = False
om.addToObjectModel(self.polyDataObj)
self.queue = PythonQt.dd.ddBotImageQueue(lcmUtils.getGlobalLCMThread())
self.queue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
self.targetFps = 30
self.timerCallback = TimerCallback(targetFps=self.targetFps)
self.timerCallback.callback = self._updateSource
def startSwarmVisualization():
global timerCallback, nav_data, nav_cloud
nav_cloud = vtknp.getVtkPolyDataFromNumpyPoints(nav_data)
nav_cloud_obj = vis.showPolyData(shallowCopy(nav_cloud), 'nav data')
nav_cloud_obj.initialized = False
def updateSwarm():
global nav_cloud
if not nav_cloud_obj.initialized:
nav_cloud_obj.mapper.SetColorModeToMapScalars()
nav_cloud_obj.initialized = True
#print nav_data.shape[0], nav_cloud.GetNumberOfPoints()
nav_cloud = vtknp.getVtkPolyDataFromNumpyPoints(nav_data)
nav_cloud_obj.setPolyData(shallowCopy(nav_cloud))
#print nav_cloud_obj.polyData.GetNumberOfPoints()
timerCallback = TimerCallback(targetFps=30)
timerCallback.callback = updateSwarm
timerCallback.start()
def __init__(self, view, _KinectQueue):
self.view = view
self.KinectQueue = _KinectQueue
self.visible = True
self.p = vtk.vtkPolyData()
utime = KinectQueue.getPointCloudFromKinect(self.p)
self.polyDataObj = vis.PolyDataItem('kinect source',
shallowCopy(self.p), view)
self.polyDataObj.actor.SetPickable(1)
self.polyDataObj.initialized = False
om.addToObjectModel(self.polyDataObj)
self.queue = PythonQt.dd.ddBotImageQueue(lcmUtils.getGlobalLCMThread())
self.queue.init(lcmUtils.getGlobalLCMThread(),
drcargs.args().config_file)
self.targetFps = 30
self.timerCallback = TimerCallback(targetFps=self.targetFps)
self.timerCallback.callback = self._updateSource
def startSwarmVisualization():
global timerCallback, nav_data, nav_cloud
nav_cloud = vtknp.getVtkPolyDataFromNumpyPoints(nav_data)
nav_cloud_obj = vis.showPolyData(shallowCopy(nav_cloud), 'nav data')
nav_cloud_obj.initialized = False
def updateSwarm():
global nav_cloud
if not nav_cloud_obj.initialized:
nav_cloud_obj.mapper.SetColorModeToMapScalars()
nav_cloud_obj.initialized = True
#print nav_data.shape[0], nav_cloud.GetNumberOfPoints()
nav_cloud = vtknp.getVtkPolyDataFromNumpyPoints(nav_data)
nav_cloud_obj.setPolyData(shallowCopy(nav_cloud))
#print nav_cloud_obj.polyData.GetNumberOfPoints()
timerCallback = TimerCallback(targetFps=30)
timerCallback.callback = updateSwarm
timerCallback.start()
def clipRange(dataObj, arrayName, thresholdRange):
if not dataObj.GetPointData().GetArray(arrayName):
raise Exception('clipRange: could not locate array: %s' % arrayName)
dataObj.GetPointData().SetScalars(
dataObj.GetPointData().GetArray(arrayName))
f = vtk.vtkClipPolyData()
f.SetInput(dataObj)
f.SetValue(thresholdRange[0])
f.SetInputArrayToProcess(0, 0, 0,
vtk.vtkDataObject.FIELD_ASSOCIATION_POINTS,
arrayName)
f2 = vtk.vtkClipPolyData()
f2.AddInputConnection(f.GetOutputPort())
f2.SetValue(thresholdRange[1])
f2.InsideOutOn()
f2.SetInputArrayToProcess(0, 0, 0,
vtk.vtkDataObject.FIELD_ASSOCIATION_POINTS,
arrayName)
f2.Update()
return shallowCopy(f2.GetOutput())
def cleanPolyData(polyData):
clean = vtk.vtkCleanPolyData()
clean.SetInput(polyData)
clean.Update()
return shallowCopy(clean.GetOutput())
def getRightFootMesh():
return shallowCopy(getFootMeshes()[1])
def getLeftFootMesh():
return shallowCopy(getFootMeshes()[0])
def computeDelaunay2D(polyData):
f = vtk.vtkDelaunay2D()
f.SetInput(polyData)
f.Update()
return shallowCopy(f.GetOutput())
def computeDelaunay2D(polyData):
f = vtk.vtkDelaunay2D()
f.SetInput(polyData)
f.Update()
return shallowCopy(f.GetOutput())
def computeNormals(polyData, featureAngle=45):
normals = vtk.vtkPolyDataNormals()
normals.SetFeatureAngle(featureAngle)
normals.SetInput(polyData)
normals.Update()
return shallowCopy(normals.GetOutput())
def getRightFootMesh():
return shallowCopy(getFootMeshes()[1])
def getLeftFootMesh():
return shallowCopy(getFootMeshes()[0])
def removeNonFinitePoints(polyData, arrayName='Points'):
polyData = shallowCopy(polyData)
labelNonFinitePoints(polyData, arrayName)
return thresholdPoints(polyData, 'is_nonfinite', [0, 0])
def cleanPolyData(polyData):
clean = vtk.vtkCleanPolyData()
clean.SetInput(polyData)
clean.Update()
return shallowCopy(clean.GetOutput())
def computeNormals(polyData, featureAngle=45):
normals = vtk.vtkPolyDataNormals()
normals.SetFeatureAngle(featureAngle)
normals.SetInput(polyData)
normals.Update()
return shallowCopy(normals.GetOutput())
def removeNonFinitePoints(polyData, arrayName='Points'):
polyData = shallowCopy(polyData)
labelNonFinitePoints(polyData, arrayName)
return thresholdPoints(polyData, 'is_nonfinite', [0, 0])
