def transformPolyData(polyData, transform):
t = vtk.vtkTransformPolyDataFilter()
t.SetTransform(transform)
t.SetInputData(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 readPolyData(filename, computeNormals=False):
ext = os.path.splitext(filename)[1].lower()
readers = {
'.vtp' : vtk.vtkXMLPolyDataReader,
'.vtk' : vtk.vtkPolyDataReader,
'.ply' : vtk.vtkPLYReader,
'.obj' : vtk.vtkOBJReader,
'.stl' : vtk.vtkSTLReader,
}
try:
readers['.pcd'] = vtk.vtkPCDReader
except AttributeError:
pass
if ext not in readers:
raise Exception('Unknown file extension in readPolyData: %s' % filename)
reader = readers[ext]()
reader.SetFileName(filename)
reader.Update()
polyData = shallowCopy(reader.GetOutput())
if polyData.GetNumberOfPoints() and not polyData.GetNumberOfCells():
f = vtk.vtkVertexGlyphFilter()
f.SetInputData(polyData)
f.Update()
polyData = shallowCopy(f.GetOutput())
if computeNormals:
return _computeNormals(polyData)
else:
return polyData
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 createTexturedPlane():
source = vtk.vtkPlaneSource()
textureMap = vtk.vtkTextureMapToPlane()
textureMap.SetInput(source.GetOutput())
textureMap.Update()
return shallowCopy(textureMap.GetOutput())
def appendPolyData(polyDataList):
append = vtk.vtkAppendPolyData()
if polyDataList:
for polyData in polyDataList:
append.AddInputData(polyData)
append.Update()
return shallowCopy(append.GetOutput())
def __init__(self, view, polyData):
self.view = view
self.polyData = shallowCopy(polyData)
self.selectionObj = None
self.selectionColor = [1, 0, 0]
self.selectionPointSize = 3
self.selectMode = 1
self.iren = view.renderWindow().GetInteractor()
self.prevStyle = self.iren.GetInteractorStyle()
self.rubberBandStyle = vtk.vtkInteractorStyleRubberBand3D()
self.rubberBandStyle.AddObserver("SelectionChangedEvent",
self.onRubberBandPickEvent)
self.eventFilter = PointSelector.EventFilter(view)
self.eventFilter.selector = self
vnp.addNumpyToVtk(
self.polyData,
np.arange(self.polyData.GetNumberOfPoints(), dtype=int),
"point_ids",
)
vnp.addNumpyToVtk(
self.polyData,
np.zeros(self.polyData.GetNumberOfPoints(), dtype=int),
"is_selected",
)
def appendPolyData(polyDataList):
assert len(polyDataList)
append = vtk.vtkAppendPolyData()
for polyData in polyDataList:
append.AddInput(polyData)
append.Update()
return shallowCopy(append.GetOutput())
def createTexturedPlane():
source = vtk.vtkPlaneSource()
textureMap = vtk.vtkTextureMapToPlane()
textureMap.SetInput(source.GetOutput())
textureMap.Update()
return shallowCopy(textureMap.GetOutput())
def makeMarker(i):
obj = vis.showPolyData(shallowCopy(geom),
modelName + ' marker %d' % i,
color=modelColor,
parent=modelFolder)
vis.addChildFrame(obj)
return obj
def getMarkerGeometry(self):
if self.markerGeometry is None:
d = DebugData()
d.addSphere(np.zeros(3), radius=0.007, resolution=8)
self.markerGeometry = shallowCopy(d.getPolyData())
return self.markerGeometry
def applyQuadricClustering(polyData, spacing=0.01):
f = vtk.vtkQuadricClustering()
f.AutoAdjustNumberOfDivisionsOn()
f.SetDivisionOrigin(0, 0, 0)
f.SetDivisionSpacing(spacing, spacing, spacing)
f.SetInput(polyData)
f.Update()
return shallowCopy(f.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 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 getMarkerGeometry(self):
if self.markerGeometry is None:
d = DebugData()
d.addSphere(np.zeros(3), radius=0.007, resolution=12)
self.markerGeometry = shallowCopy(d.getPolyData())
return self.markerGeometry
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 clipByPlane(polyData, planeOrigin, planeNormal):
f = vtk.vtkClipPolyData()
f.SetInput(polyData)
p = vtk.vtkPlane()
p.SetOrigin(planeOrigin)
p.SetNormal(planeNormal)
f.SetClipFunction(p)
f.Update()
return shallowCopy(f.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
om.removeFromObjectModel(existingWidget)
if previousStep == stepIndex:
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)
return True
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 decimateMesh(polyData, targetReduction=0.1):
"""
Reduce the number of triangles in the input mesh by targetReduction.
0.1 = 10% reduction (if there was 100 triangles, now there will be 90)
"""
f = vtk.vtkDecimatePro()
f.SetInputData(polyData)
f.SetTargetReduction(targetReduction)
f.Update()
return shallowCopy(f.GetOutput())
def decimateMesh(polyData, targetReduction=0.1):
'''
Reduce the number of triangles in the input mesh by targetReduction.
0.1 = 10% reduction (if there was 100 triangles, now there will be 90)
'''
f = vtk.vtkDecimatePro()
f.SetInput(polyData)
f.SetTargetReduction(targetReduction)
f.Update()
return shallowCopy(f.GetOutput())
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 flipImage(image, flipAxis=1):
"""
Flip a vtkImageData using the vtkImageFlip filter.
The flipAxis can be 0 or 1 to flip horizontally or vertically.
"""
assert flipAxis in (0, 1)
f = vtk.vtkImageFlip()
f.SetFilteredAxis(flipAxis)
f.SetInputData(image)
f.Update()
return shallowCopy(f.GetOutput())
def rotateImage180(image):
'''
rotates an image by 180 degrees
'''
r1 = vtk.vtkImageFlip()
r1.SetInput(image)
r1.SetFilteredAxis(0)
r1.Update()
r2 = vtk.vtkImageFlip()
r2.SetInput(r1.GetOutput())
r2.SetFilteredAxis(1)
r2.Update()
return shallowCopy(r2.GetOutput())
def rotateImage180(image):
"""
rotates an image by 180 degrees
"""
r1 = vtk.vtkImageFlip()
r1.SetInputData(image)
r1.SetFilteredAxis(0)
r1.Update()
r2 = vtk.vtkImageFlip()
r2.SetInputData(r1.GetOutput())
r2.SetFilteredAxis(1)
r2.Update()
return shallowCopy(r2.GetOutput())
def thresholdCells(polyData, arrayName, thresholdRange):
assert(polyData.GetCellData().GetArray(arrayName))
f = vtk.vtkThreshold()
# f.SetAttributeModeToUseCellData()
f.SetInput(polyData)
f.ThresholdBetween(thresholdRange[0], thresholdRange[1])
f.SetInputArrayToProcess(0,0,0, vtk.vtkDataObject.FIELD_ASSOCIATION_CELLS, arrayName)
f.Update()
g = vtk.vtkGeometryFilter()
g.SetInput(f.GetOutput())
g.Update()
return shallowCopy(g.GetOutput())
def computeDelaunay3D(polyData):
f = vtk.vtkDelaunay3D()
f.SetInputData(polyData)
f.SetOffset(100.0)
f.Update()
surface = vtk.vtkGeometryFilter()
surface.SetInputData(f.GetOutput())
surface.Update()
clean = vtk.vtkCleanPolyData()
clean.SetInputData(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 readMultiBlock(filename):
'''Reads a .vtm file and returns a list of vtkPolyData objects'''
reader = vtk.vtkXMLMultiBlockDataReader()
reader.SetFileName(filename)
reader.Update()
polyDataList = []
mb = reader.GetOutput()
for i in range(mb.GetNumberOfBlocks()):
polyData = vtk.vtkPolyData.SafeDownCast(mb.GetBlock(i))
if polyData and polyData.GetNumberOfPoints():
polyDataList.append(shallowCopy(polyData))
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 numpyToPolyData(pts, pointData=None, createVertexCells=True):
pd = vtk.vtkPolyData()
pd.SetPoints(getVtkPointsFromNumpy(pts.copy()))
if pointData is not None:
for key, value in pointData.iteritems():
addNumpyToVtk(pd, value.copy(), key)
if createVertexCells:
f = vtk.vtkVertexGlyphFilter()
f.SetInputData(pd)
f.Update()
pd = shallowCopy(f.GetOutput())
return pd
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 __init__(self, view, polyData):
self.view = view
self.polyData = shallowCopy(polyData)
self.selectionObj = None
self.selectionColor = [1, 0, 0]
self.selectionPointSize = 3
self.selectMode = 1
self.iren = view.renderWindow().GetInteractor()
self.prevStyle = self.iren.GetInteractorStyle()
self.rubberBandStyle = vtk.vtkInteractorStyleRubberBand3D()
self.rubberBandStyle.AddObserver('SelectionChangedEvent', self.onRubberBandPickEvent)
self.eventFilter = PointSelector.EventFilter(view)
self.eventFilter.selector = self
vnp.addNumpyToVtk(self.polyData, np.arange(self.polyData.GetNumberOfPoints(), dtype=int), 'point_ids')
vnp.addNumpyToVtk(self.polyData, np.zeros(self.polyData.GetNumberOfPoints(), dtype=int), 'is_selected')
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 numpyToPolyData(pts, pointData=None, createVertexCells=True):
pd = vtk.vtkPolyData()
pd.SetPoints(getVtkPointsFromNumpy(pts.copy()))
if pointData is not None:
for key, value in pointData.iteritems():
addNumpyToVtk(pd, value.copy(), key)
if createVertexCells:
f = vtk.vtkVertexGlyphFilter()
f.SetInputData(pd)
f.Update()
pd = shallowCopy(f.GetOutput())
return pd
def readImage(filename):
ext = os.path.splitext(filename)[1].lower()
readers = {
'.png' : vtk.vtkPNGReader,
'.jpg' : vtk.vtkJPEGReader,
'.vti' : vtk.vtkXMLImageDataReader,
}
if ext not in readers:
raise Exception('Unknown file extension in readImage: %s' % filename)
reader = readers[ext]()
reader.SetFileName(filename)
reader.Update()
image = shallowCopy(reader.GetOutput())
return image
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 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 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, _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 __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 addPolyData(self, polyData, color=[1,1,1], extraLabels=None):
'''
Add a vtkPolyData to the debug data. A color can be provided.
If the extraLabels argument is used, it should be a list of tuples,
each tuple is (labelName, labelValue) where labelName is a string and
labelValue is an int or float. An array with labelName will be filled
with labelValue and added to the poly data.
'''
polyData = shallowCopy(polyData)
if color is not None:
colorArray = np.empty((polyData.GetNumberOfPoints(), 3), dtype=np.uint8)
colorArray[:,:] = np.array(color)*255
vnp.addNumpyToVtk(polyData, colorArray, 'RGB255')
if extraLabels is not None:
for labelName, labelValue in extraLabels:
extraArray = np.empty((polyData.GetNumberOfPoints(), 1), dtype=type(labelValue))
extraArray[:] = labelValue
vnp.addNumpyToVtk(polyData, extraArray, labelName)
self.append.AddInputData(polyData)
def thresholdCells(polyData, arrayName, thresholdRange, arrayType="cells"):
assert arrayType in ("points", "cells")
f = vtk.vtkThreshold()
f.SetInputData(polyData)
f.ThresholdBetween(thresholdRange[0], thresholdRange[1])
if arrayType == "cells":
assert polyData.GetCellData().GetArray(arrayName)
f.SetInputArrayToProcess(0, 0, 0,
vtk.vtkDataObject.FIELD_ASSOCIATION_CELLS,
arrayName)
else:
assert polyData.GetPointData().GetArray(arrayName)
f.SetInputArrayToProcess(0, 0, 0,
vtk.vtkDataObject.FIELD_ASSOCIATION_POINTS,
arrayName)
f.Update()
g = vtk.vtkGeometryFilter()
g.SetInputConnection(f.GetOutputPort())
g.Update()
return shallowCopy(g.GetOutput())
def addPolyData(self, polyData, color=[1, 1, 1], extraLabels=None):
'''
Add a vtkPolyData to the debug data. A color can be provided.
If the extraLabels argument is used, it should be a list of tuples,
each tuple is (labelName, labelValue) where labelName is a string and
labelValue is an int or float. An array with labelName will be filled
with labelValue and added to the poly data.
'''
polyData = shallowCopy(polyData)
if color is not None:
colorArray = np.empty((polyData.GetNumberOfPoints(), 3),
dtype=np.uint8)
colorArray[:, :] = np.array(color) * 255
vnp.addNumpyToVtk(polyData, colorArray, 'RGB255')
if extraLabels is not None:
for labelName, labelValue in extraLabels:
extraArray = np.empty((polyData.GetNumberOfPoints(), 1),
dtype=type(labelValue))
extraArray[:] = labelValue
vnp.addNumpyToVtk(polyData, extraArray, labelName)
self.append.AddInputData(polyData)
def computeDelaunay2D(polyData):
f = vtk.vtkDelaunay2D()
f.SetInput(polyData)
f.Update()
return shallowCopy(f.GetOutput())
def computePointFeatureHistograms(polyData, searchRadius=0.10):
f = vtk.vtkPCLFPFHEstimation()
f.SetInput(polyData)
f.SetSearchRadius(searchRadius)
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 makeMarker(i):
obj = vis.showPolyData(
shallowCopy(geom), modelName + " marker %d" % i, color=modelColor, parent=modelFolder
)
vis.addChildFrame(obj)
return obj
def getPolyData(self):
if self.append.GetNumberOfInputConnections(0):
self.append.Update()
return shallowCopy(self.append.GetOutput())
def cleanPolyData(polyData):
clean = vtk.vtkCleanPolyData()
clean.SetInput(polyData)
clean.Update()
return shallowCopy(clean.GetOutput())
def applySubdivision(polyData, subdivisions=3):
f = vtk.vtkLoopSubdivisionFilter()
f.SetInput(polyData)
f.SetNumberOfSubdivisions(subdivisions)
f.Update()
return shallowCopy(f.GetOutput())
def triangulatePolyData(polyData):
f = vtk.vtkTriangleFilter()
f.SetInput(polyData)
f.Update()
return shallowCopy(f.GetOutput())
def getLeftFootMesh():
return shallowCopy(getFootMeshes()[0])
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
om.removeFromObjectModel(existingWidget)
if previousStep == stepIndex:
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 makeDebugPoints(points, radius=0.01):
d = DebugData()
for p in points:
d.addSphere(p, radius=radius)
return shallowCopy(d.getPolyData())
def getRightFootMesh():
return shallowCopy(getFootMeshes()[1])
if useVoxelGrid:
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.02)
if testNormals:
print 'computing normals...'
f = vtk.vtkRobustNormalEstimator()
f.SetInput(polyData)
f.SetMaxIterations(100)
f.SetMaxEstimationError(0.01)
f.SetMaxCenterError(0.02)
f.SetComputeCurvature(True)
f.SetRadius(0.1)
f.Update()
polyData = shallowCopy(f.GetOutput())
print 'done.'
# filter points without normals
normals = vnp.getNumpyFromVtk(polyData, 'normals')
segmentation.flipNormalsWithViewDirection(polyData, [1, -1, -1])
normalsValid = np.any(normals, axis=1)
vnp.addNumpyToVtk(polyData, np.array(normalsValid, dtype=np.int32),
'normals_valid')
vis.showPolyData(polyData,
'scene points',
colorByName='normals_valid',
visible=False)
def removeNonFinitePoints(polyData, arrayName='Points'):
polyData = shallowCopy(polyData)
labelNonFinitePoints(polyData, arrayName)
return thresholdPoints(polyData, 'is_nonfinite', [0, 0])
if useVoxelGrid:
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.02)
if testNormals:
print 'computing normals...'
f = vtk.vtkRobustNormalEstimator()
f.SetInput(polyData)
f.SetMaxIterations(100)
f.SetMaxEstimationError(0.01)
f.SetMaxCenterError(0.02)
f.SetComputeCurvature(True)
f.SetRadius(0.1)
f.Update()
polyData = shallowCopy(f.GetOutput())
print 'done.'
# filter points without normals
normals = vnp.getNumpyFromVtk(polyData, 'normals')
segmentation.flipNormalsWithViewDirection(polyData, [1, -1, -1])
normalsValid = np.any(normals, axis=1)
vnp.addNumpyToVtk(polyData, np.array(normalsValid, dtype=np.int32), 'normals_valid')
vis.showPolyData(polyData, 'scene points', colorByName='normals_valid', visible=False)
numPoints = polyData.GetNumberOfPoints()
