def onViewDoubleClicked(self, displayPoint):
obj, pickedPoint = vis.findPickedObject(displayPoint, self.view)
if pickedPoint is None or not obj:
return
imageName = obj.getProperty('Name')
imageUtime = self.imageManager.getUtime(imageName)
cameraToLocal = vtk.vtkTransform()
self.imageManager.queue.getTransform(imageName, 'local', imageUtime,
cameraToLocal)
utorsoToLocal = vtk.vtkTransform()
self.imageManager.queue.getTransform('utorso', 'local', imageUtime,
utorsoToLocal)
p = range(3)
utorsoToLocal.TransformPoint(pickedPoint, p)
ray = np.array(p) - np.array(cameraToLocal.GetPosition())
ray /= np.linalg.norm(ray)
if self.rayCallback:
self.rayCallback(np.array(cameraToLocal.GetPosition()), ray)
def addGeometry(self, path, geomItems):
folder = self.getPathFolder(path)
ancestors = findPathToAncestor(folder, self.getRootFolder())
geomTransform = vtk.vtkTransform()
for item in reversed(ancestors):
if not hasattr(item, "transform"):
item.transform = vtk.vtkTransform()
item.transform.PostMultiply()
geomTransform.Concatenate(item.transform)
for geom in geomItems:
existing_item = self.getItemByPath(path + ["geometry"])
item = geom.polyDataItem
if existing_item is not None:
for prop in existing_item.propertyNames():
item.setProperty(prop, existing_item.getProperty(prop))
om.removeFromObjectModel(existing_item)
else:
item.setProperty("Point Size", 2)
for colorBy in ["rgb", "intensity"]:
try:
item.setProperty("Color By", colorBy)
except ValueError:
pass
else:
break
item.addToView(self.view)
om.addToObjectModel(item, parentObj=folder)
item.actor.SetUserTransform(geomTransform)
return path
def constructTestFrames(self):
T = vtk.vtkTransform()
S = vtk.vtkTransform()
S.Translate([1, 2, 0])
FM = transformUtils.forceMomentTransformation(S, T)
print FM
return T, S, FM
def showFusedMaps(self):
om.removeFromObjectModel(om.findObjectByName("stereo"))
om.getOrCreateContainer("stereo")
q = imageManager.queue
cameraToLocalNow = vtk.vtkTransform()
utime = q.getCurrentImageTime("CAMERA_TSDF")
q.getTransform("CAMERA_LEFT", "local", utime, cameraToLocalNow)
cameraToLocalFusedNow = vtk.vtkTransform()
q.getTransform("CAMERA_LEFT_ALT", "local", utime, cameraToLocalFusedNow)
for i in range(len(self.pointClouds)):
fusedNowToLocalNow = vtk.vtkTransform()
fusedNowToLocalNow.PreMultiply()
fusedNowToLocalNow.Concatenate(cameraToLocalNow)
fusedNowToLocalNow.Concatenate(cameraToLocalFusedNow.GetLinearInverse())
fusedTransform = vtk.vtkTransform()
fusedTransform.PreMultiply()
fusedTransform.Concatenate(fusedNowToLocalNow)
fusedTransform.Concatenate(self.cameraToLocalFusedTransforms[i])
pd = filterUtils.transformPolyData(self.pointClouds[i], fusedTransform)
vis.showFrame(fusedTransform, ("cloud frame " + str(i)), visible=True, scale=0.2, parent="stereo")
vis.showPolyData(pd, ("stereo " + str(i)), parent="stereo", colorByName="rgb_colors")
def showFusedMaps(self):
om.removeFromObjectModel(om.findObjectByName('stereo'))
om.getOrCreateContainer('stereo')
q = imageManager.queue
cameraToLocalNow = vtk.vtkTransform()
utime = q.getCurrentImageTime('CAMERA_TSDF')
q.getTransform('CAMERA_LEFT', 'local', utime, cameraToLocalNow)
cameraToLocalFusedNow = vtk.vtkTransform()
q.getTransform('CAMERA_LEFT_ALT', 'local', utime,
cameraToLocalFusedNow)
for i in range(len(self.pointClouds)):
fusedNowToLocalNow = vtk.vtkTransform()
fusedNowToLocalNow.PreMultiply()
fusedNowToLocalNow.Concatenate(cameraToLocalNow)
fusedNowToLocalNow.Concatenate(
cameraToLocalFusedNow.GetLinearInverse())
fusedTransform = vtk.vtkTransform()
fusedTransform.PreMultiply()
fusedTransform.Concatenate(fusedNowToLocalNow)
fusedTransform.Concatenate(self.cameraToLocalFusedTransforms[i])
pd = filterUtils.transformPolyData(self.pointClouds[i],
fusedTransform)
vis.showFrame(fusedTransform, ('cloud frame ' + str(i)),
visible=True,
scale=0.2,
parent='stereo')
vis.showPolyData(pd, ('stereo ' + str(i)),
parent='stereo',
colorByName='rgb_colors')
def transformPlanToBDIFrame(self, plan):
if (self.pose_bdi is None):
# print "haven't received POSE_BDI"
return
# TODO: This transformation should be rewritten using the LOCAL_TO_LOCAL_BDI frame
# instead of using FK here
t_bodybdi = transformUtils.transformFromPose(self.pose_bdi.pos, self.pose_bdi.orientation)
t_bodybdi.PostMultiply()
current_pose = self.jointController.q
t_bodymain = transformUtils.transformFromPose( current_pose[0:3] , transformUtils.rollPitchYawToQuaternion(current_pose[3:6]) )
t_bodymain.PostMultiply()
# iterate and transform
self.bdi_plan = plan.decode( plan.encode() ) # decode and encode ensures deepcopy
for i, footstep in enumerate(self.bdi_plan.footsteps):
step = footstep.pos
t_step = frameFromPositionMessage(step)
t_body_to_step = vtk.vtkTransform()
t_body_to_step.DeepCopy(t_step)
t_body_to_step.PostMultiply()
t_body_to_step.Concatenate(t_bodymain.GetLinearInverse())
t_stepbdi = vtk.vtkTransform()
t_stepbdi.DeepCopy(t_body_to_step)
t_stepbdi.PostMultiply()
t_stepbdi.Concatenate(t_bodybdi)
footstep.pos = positionMessageFromFrame(t_stepbdi)
if (self.showBDIPlan is True):
self.drawBDIFootstepPlan()
def getStereoPointCloudElapsed(self,decimation=4, imagesChannel='CAMERA', cameraName='CAMERA_LEFT', removeSize=0):
q = imageManager.queue
utime = q.getCurrentImageTime(cameraName)
if utime == 0:
return None, None, None
if (utime - self.lastUtime < 1E6):
return None, None, None
p = vtk.vtkPolyData()
cameraToLocalFused = vtk.vtkTransform()
q.getTransform('CAMERA_LEFT_ALT', 'local', utime, cameraToLocalFused)
cameraToLocal = vtk.vtkTransform()
q.getTransform('CAMERA_LEFT', 'local', utime, cameraToLocal)
prevToCurrentCameraTransform = vtk.vtkTransform()
prevToCurrentCameraTransform.PostMultiply()
prevToCurrentCameraTransform.Concatenate( cameraToLocal )
prevToCurrentCameraTransform.Concatenate( self.lastCameraToLocal.GetLinearInverse() )
distTravelled = np.linalg.norm( prevToCurrentCameraTransform.GetPosition() )
# 0.2 heavy overlap
# 0.5 quite a bit of overlap
# 1.0 is good
if (distTravelled < 0.2 ):
return None, None, None
q.getPointCloudFromImages(imagesChannel, p, decimation, removeSize, removeThreshold = -1)
self.lastCameraToLocal = cameraToLocal
self.lastUtime = utime
return p, cameraToLocalFused, cameraToLocal
def updateObjectInCamera(self, obj, cameraObj):
imageView = self.imageView
objToLocalT = transformUtils.copyFrame(obj.actor.GetUserTransform() or vtk.vtkTransform())
localToCameraT = vtk.vtkTransform()
self.imageQueue.getTransform('local', imageView.imageName, localToCameraT)
t = vtk.vtkTransform()
t.PostMultiply()
t.Concatenate(objToLocalT)
t.Concatenate(localToCameraT)
pd = filterUtils.transformPolyData(obj.polyData, t)
'''
normals = pd.GetPointData().GetNormals()
cameraToImageT = vtk.vtkTransform()
imageQueue.getCameraProjectionTransform(imageView.imageName, cameraToImageT)
pd = filterUtils.transformPolyData(pd, cameraToImageT)
pts = vnp.getNumpyFromVtk(pd, 'Points')
pts[:,0] /= pts[:,2]
pts[:,1] /= pts[:,2]
pd.GetPointData().SetNormals(normals)
'''
self.imageQueue.projectPoints(imageView.imageName, pd)
cameraObj.setPolyData(pd)
self.addActorToImageOverlay(cameraObj, imageView)
def updateObjectInCamera(self, obj, cameraObj):
imageView = self.imageView
objToLocalT = transformUtils.copyFrame(obj.actor.GetUserTransform()
or vtk.vtkTransform())
localToCameraT = vtk.vtkTransform()
self.imageQueue.getTransform("local", imageView.imageName,
localToCameraT)
t = vtk.vtkTransform()
t.PostMultiply()
t.Concatenate(objToLocalT)
t.Concatenate(localToCameraT)
pd = filterUtils.transformPolyData(obj.polyData, t)
"""
normals = pd.GetPointData().GetNormals()
cameraToImageT = vtk.vtkTransform()
imageQueue.getCameraProjectionTransform(imageView.imageName, cameraToImageT)
pd = filterUtils.transformPolyData(pd, cameraToImageT)
pts = vnp.getNumpyFromVtk(pd, 'Points')
pts[:,0] /= pts[:,2]
pts[:,1] /= pts[:,2]
pd.GetPointData().SetNormals(normals)
"""
self.imageQueue.projectPoints(imageView.imageName, pd)
cameraObj.setPolyData(pd)
self.addActorToImageOverlay(cameraObj, imageView)
def updateGeometryFromProperties(self):
filename = self.getProperty('Filename')
scale = self.getProperty('Scale')
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)
if not scale == [1, 1, 1]:
transform = vtk.vtkTransform()
transform.Scale(scale)
polyData = filterUtils.transformPolyData(
polyData, transform)
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 updateGeometryFromProperties(self):
filename = self.getProperty('Filename')
scale = self.getProperty('Scale')
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)
if not scale == [1, 1, 1]:
transform = vtk.vtkTransform()
transform.Scale(scale)
transformFilter = vtk.vtkTransformPolyDataFilter()
transformFilter.SetInput(polyData)
transformFilter.SetTransform(transform)
transformFilter.Update()
polyData = transformFilter.GetOutput()
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 getStereoPointCloudElapsed(self, decimation=4, imagesChannel="CAMERA", cameraName="CAMERA_LEFT", removeSize=0):
q = imageManager.queue
utime = q.getCurrentImageTime(cameraName)
if utime == 0:
return None, None, None
if utime - self.lastUtime < 1e6:
return None, None, None
p = vtk.vtkPolyData()
cameraToLocalFused = vtk.vtkTransform()
q.getTransform("CAMERA_LEFT_ALT", "local", utime, cameraToLocalFused)
cameraToLocal = vtk.vtkTransform()
q.getTransform("CAMERA_LEFT", "local", utime, cameraToLocal)
prevToCurrentCameraTransform = vtk.vtkTransform()
prevToCurrentCameraTransform.PostMultiply()
prevToCurrentCameraTransform.Concatenate(cameraToLocal)
prevToCurrentCameraTransform.Concatenate(self.lastCameraToLocal.GetLinearInverse())
distTravelled = np.linalg.norm(prevToCurrentCameraTransform.GetPosition())
# 0.2 heavy overlap
# 0.5 quite a bit of overlap
# 1.0 is good
if distTravelled < 0.2:
return None, None, None
q.getPointCloudFromImages(imagesChannel, p, decimation, removeSize, removeThreshold=-1)
self.lastCameraToLocal = cameraToLocal
self.lastUtime = utime
return p, cameraToLocalFused, cameraToLocal
def computeWrench(self, linkName, forceDirection, forceMagnitude,
forceLocation):
outputFrame = vtk.vtkTransform()
wrenchFrame = vtk.vtkTransform()
wrenchFrame.Translate(forceLocation)
forceMomentTransform = transformUtils.forceMomentTransformation(
wrenchFrame, outputFrame)
wrench = np.zeros(6)
wrench[3:] = forceMagnitude * forceDirection
wrenchTransformed = np.dot(forceMomentTransform, wrench)
return wrenchTransformed
def drawFrameInCamera(t, frameName='new frame',visible=True):
v = imageView.view
q = cameraview.imageManager.queue
localToCameraT = vtk.vtkTransform()
q.getTransform('local', 'CAMERA_LEFT', localToCameraT)
res = vis.showFrame( vtk.vtkTransform() , 'temp',view=v, visible=True, scale = 0.2)
om.removeFromObjectModel(res)
pd = res.polyData
pd = filterUtils.transformPolyData(pd, t)
pd = filterUtils.transformPolyData(pd, localToCameraT)
q.projectPoints('CAMERA_LEFT', pd )
vis.showPolyData(pd, ('overlay ' + frameName), view=v, colorByName='Axes',parent='camera overlay',visible=visible)
def drawFrameInCamera(t, frameName='new frame',visible=True):
v = imageView.view
q = cameraview.imageManager.queue
localToCameraT = vtk.vtkTransform()
q.getTransform('local', 'CAMERA_LEFT', localToCameraT)
res = vis.showFrame( vtk.vtkTransform() , 'temp',view=v, visible=True, scale = 0.2)
om.removeFromObjectModel(res)
pd = res.polyData
pd = filterUtils.transformPolyData(pd, t)
pd = filterUtils.transformPolyData(pd, localToCameraT)
q.projectPoints('CAMERA_LEFT', pd )
vis.showPolyData(pd, ('overlay ' + frameName), view=v, colorByName='Axes',parent='camera overlay',visible=visible)
def gripperPositionTarget(directionToTarget,
yaw=None,
pitch=None,
roll=0,
radius=None):
# project into x,y plane
d = np.array(directionToTarget)
d = d / np.linalg.norm(d)
d[2] = 0
theta = np.deg2rad(pitch)
phi = np.deg2rad(yaw)
v = np.zeros(3)
v[0] = np.sin(theta) * np.cos(phi)
v[1] = np.sin(theta) * np.sin(phi)
v[2] = np.cos(theta)
v = v / np.linalg.norm(v)
# transform that takes [1,0,0] to v
# t = transformUtils.getTransformFromOriginAndNormal([0,0,0], v)
# d = np.array(directionToTarget)
# d = d/np.linalg.norm(d)
# t = transformUtils.getTransformFromOriginAndNormal([0,0,0], d, normalAxis=0)
# s = radius * np.array(t.TransformVector(v))
s = radius * v
# rotate s if needed
# alpha = angle between [1,0,0] and d
xAxis = np.array([1, 0, 0])
alpha = np.arccos(np.dot(xAxis, d))
alphaDegrees = np.rad2deg(alpha)
t = vtk.vtkTransform()
t.RotateZ(alphaDegrees)
xAxisPlus = np.array(t.TransformVector(xAxis))
t2 = vtk.vtkTransform()
t2.RotateZ(-alphaDegrees)
xAxisMinus = np.array(t2.TransformVector(xAxis))
if np.dot(xAxisPlus, d) > np.dot(xAxisMinus, d):
s = t.TransformVector(s)
else:
s = t2.TransformVector(s)
return s
def buildAccelSphere(center, a_max, color=[1,0.3,0.0], alpha=0.5):
d = DebugData()
d.addSphere([0,0,0], radius=1)
polyData = d.getPolyData()
t = vtk.vtkTransform()
t.Scale(a_max, a_max, a_max)
polyData = filterUtils.transformPolyData(polyData, t)
t = vtk.vtkTransform()
t.Translate(center)
polyData = filterUtils.transformPolyData(polyData, t)
obj = vis.updatePolyData(polyData, 'accel_sphere', color=color, alpha=alpha)
return polyData
def buildEllipse(i, center, x_scale, y_scale, z_scale, color=[1,1,1], alpha=1.0):
d = DebugData()
d.addSphere([0,0,0], radius=1)
polyData = d.getPolyData()
t = vtk.vtkTransform()
t.Scale(x_scale, y_scale, z_scale)
polyData = filterUtils.transformPolyData(polyData, t)
t = vtk.vtkTransform()
t.Translate(center)
polyData = filterUtils.transformPolyData(polyData, t)
obj = vis.updatePolyData(polyData, str(i), color=color, alpha=alpha)
def setRobotFrameState(self, x, y, theta, roll=0.0, pitch=0.0):
t = vtk.vtkTransform()
t.Translate(x,y,0.0)
t.RotateZ(np.degrees(theta))
t.RotateY(np.degrees(pitch))
t.RotateX(np.degrees(roll))
self.robot.getChildFrame().copyFrame(t)
def updateFrames(self):
if not self.frames:
for name in self.frameNames:
self.frames[name] = vtk.vtkTransform()
for name, frame in self.frames.items():
frame.SetMatrix(
self.robotStateModel.getLinkFrame(name).GetMatrix())
def promotePolyDataItem(cls, obj):
parent = obj.parent()
view = obj.views[0]
name = obj.getProperty('Name')
polyData = obj.polyData
props = obj.properties._properties
childFrame = obj.getChildFrame()
if childFrame:
t = transformUtils.copyFrame(childFrame.transform)
else:
t = vtk.vtkTransform()
t.PostMultiply()
t.Translate(filterUtils.computeCentroid(polyData))
polyData = filterUtils.transformPolyData(polyData, t.GetLinearInverse())
children = [c for c in obj.children() if c is not childFrame]
meshId = cls.getMeshManager().add(polyData)
om.removeFromObjectModel(obj)
obj = MeshAffordanceItem(name, view)
obj.setProperty('Filename', meshId)
om.addToObjectModel(obj, parentObj=parent)
frame = vis.addChildFrame(obj)
frame.copyFrame(t)
for child in children:
om.addToObjectModel(child, parentObj=obj)
obj.syncProperties(props)
return obj
def __init__(self):
#Loading
filename = os.path.join(os.environ['SPARTAN_SOURCE_DIR'],
'apps/chris/ddGroundTruthAnnotationPanel.ui')
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(filename)
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
uifile.close()
#UI elements
self.ui = WidgetDict(self.widget.children())
self.view = app.getCurrentRenderView()
self.ui.enabledCheck.connect('toggled(bool)', self.onEnabledCheckBox)
self.ui.clearButton.connect('clicked()', self.onClear)
self.ui.saveScene.connect('clicked()', self.saveCurrentScene)
self.ui.align.connect('clicked()', self.vtkICP)
self.eventFilter = MyEventFilter(view, self)
self.annotation = vis.PolyDataItem('annotation',
self.makeSphere((0, 0, 0)), view)
self.annotation.setProperty('Color', [0, 1, 0])
self.annotation.actor.SetPickable(False)
self.annotation.actor.SetUserTransform(vtk.vtkTransform())
self.setEnabled(False)
#State
self.pickPoints = []
self.objects = []
self.isModifyingBoundingBox = False
#init folders
self.getRootFolder()
self.getModelObjectsFolder()
self.getTransformedObjectsFolder()
def promotePolyDataItem(cls, obj):
parent = obj.parent()
view = obj.views[0]
name = obj.getProperty('Name')
polyData = obj.polyData
props = obj.properties._properties
childFrame = obj.getChildFrame()
if childFrame:
t = transformUtils.copyFrame(childFrame.transform)
else:
t = vtk.vtkTransform()
t.PostMultiply()
t.Translate(filterUtils.computeCentroid(polyData))
polyData = filterUtils.transformPolyData(polyData,
t.GetLinearInverse())
children = [c for c in obj.children() if c is not childFrame]
meshId = cls.getMeshManager().add(polyData)
om.removeFromObjectModel(obj)
obj = MeshAffordanceItem(name, view)
obj.setProperty('Filename', meshId)
om.addToObjectModel(obj, parentObj=parent)
frame = vis.addChildFrame(obj)
frame.copyFrame(t)
for child in children:
om.addToObjectModel(child, parentObj=obj)
obj.syncProperties(props)
return obj
def showGrid(view, cellSize=0.5, numberOfCells=25, name='grid', parent='sensors', color=[1,1,1], alpha=0.05, gridTransform=None, viewBoundsFunction=None):
grid = vtk.vtkGridSource()
grid.SetScale(cellSize)
grid.SetGridSize(numberOfCells)
grid.SetSurfaceEnabled(True)
grid.Update()
gridObj = showPolyData(grid.GetOutput(), name, view=view, alpha=alpha, color=color, visible=True, parent=parent)
gridObj.gridSource = grid
gridObj.actor.GetProperty().LightingOff()
gridObj.actor.SetPickable(False)
gridTransform = gridTransform or vtk.vtkTransform()
gridObj.actor.SetUserTransform(gridTransform)
showFrame(gridTransform, name + ' frame', scale=0.2, visible=False, parent=gridObj, view=view)
gridObj.setProperty('Surface Mode', 'Wireframe')
viewBoundsFunction = viewBoundsFunction or computeViewBoundsNoGrid
def onViewBoundsRequest():
if view not in gridObj.views or not gridObj.getProperty('Visible'):
return
bounds = viewBoundsFunction(view, gridObj)
if vtk.vtkMath.AreBoundsInitialized(bounds):
view.addCustomBounds(bounds)
else:
view.addCustomBounds([-1, 1, -1, 1, -1, 1])
view.connect('computeBoundsRequest(ddQVTKWidgetView*)', onViewBoundsRequest)
return gridObj
def drawObjectInCamera(objectName,visible=True):
v = imageView.view
q = cameraview.imageManager.queue
localToCameraT = vtk.vtkTransform()
q.getTransform('local', 'CAMERA_LEFT', localToCameraT)
obj = om.findObjectByName(objectName)
if obj is None:
return
objToLocalT = transformUtils.copyFrame(obj.actor.GetUserTransform() or vtk.vtkTransform())
objPolyDataOriginal = obj.polyData
pd = objPolyDataOriginal
pd = filterUtils.transformPolyData(pd, objToLocalT)
pd = filterUtils.transformPolyData(pd, localToCameraT)
q.projectPoints('CAMERA_LEFT', pd)
vis.showPolyData(pd, ('overlay ' + objectName), view=v, color=[0,1,0],parent='camera overlay',visible=visible)
def applyFrameTransform(x, y, z, yaw):
if lastEditedFrame is not None and lastEditedFrame.getProperty('Edit'):
t = vtk.vtkTransform()
t.Concatenate(lastEditedFrame.transform)
t.RotateZ(yaw)
t.Translate(x, y, z)
lastEditedFrame.copyFrame(t)
def getLinkFrame(self, linkName):
t = vtk.vtkTransform()
t.PostMultiply()
if self.model.getLinkToWorld(linkName, t):
return t
else:
return None
def applyCameraTexture(obj, imageManager, imageName='CAMERA_LEFT'):
imageUtime = imageManager.getUtime(imageName)
if not imageUtime:
return
cameraToLocal = vtk.vtkTransform()
imageManager.queue.getTransform(imageName, 'local', imageUtime,
cameraToLocal)
pd = filterUtils.transformPolyData(obj.polyData,
obj.actor.GetUserTransform())
pd = filterUtils.transformPolyData(pd, cameraToLocal.GetLinearInverse())
imageManager.queue.computeTextureCoords(imageName, pd)
tcoordsArrayName = 'tcoords_%s' % imageName
tcoords = pd.GetPointData().GetArray(tcoordsArrayName)
assert tcoords
obj.polyData.GetPointData().SetTCoords(None)
obj.polyData.GetPointData().SetTCoords(tcoords)
obj._updateColorByProperty()
obj.actor.SetTexture(imageManager.getTexture(imageName))
obj.actor.GetProperty().LightingOff()
obj.actor.GetProperty().SetColor([1, 1, 1])
def interpolateReach(self):
t = vtk.vtkTransform()
handObj, handFrame = self.handFactory.placeHandModelWithTransform(t, self.view, side=self.side, name='grasp interpolation', parent='debug')
handObj.setProperty('Alpha', 0.2)
handFrame.setProperty('Visible', True)
sliderMax = 100.0
def sliderChanged(sliderValue):
sliderValue = sliderValue/float(sliderMax)
t = self.splineInterp(sliderValue)
handFrame.copyFrame(t)
reachGoal = self.getReachGoalFrame(self.side)
if reachGoal:
reachGoal.copyFrame(t)
self.slider = QtGui.QSlider(QtCore.Qt.Horizontal)
self.slider.connect('valueChanged(int)', sliderChanged)
self.slider.setMaximum(sliderMax)
self.slider.show()
self.slider.resize(500, 30)
sliderChanged(sliderMax)
def getWorldPositionAndRay(self, imagePixel, imageUtime=None):
'''
Given an XY image pixel, computes an equivalent ray in the world
coordinate system using the camera to local transform at the given
imageUtime. If imageUtime is None, then the utime of the most recent
image is used.
Returns the camera xyz position in world, and a ray unit vector.
'''
if imageUtime is None:
imageUtime = self.imageManager.getUtime(self.imageName)
# input is pixel u,v, output is unit x,y,z in camera coordinates
cameraPoint = self.imageManager.queue.unprojectPixel(
self.imageName, imagePixel[0], imagePixel[1])
cameraToLocal = vtk.vtkTransform()
self.imageManager.queue.getTransform(self.imageName, 'local',
imageUtime, cameraToLocal)
p = np.array(cameraToLocal.TransformPoint(cameraPoint))
cameraPosition = np.array(cameraToLocal.GetPosition())
ray = p - cameraPosition
ray /= np.linalg.norm(ray)
return cameraPosition, ray
def getFrameToWorld(self, frameId):
t = vtk.vtkTransform()
t.PostMultiply()
if self.model.getFrameToWorld(frameId, t):
return t
else:
return None
def interpolateReach(self):
t = vtk.vtkTransform()
handObj, handFrame = self.handFactory.placeHandModelWithTransform(
t,
self.view,
side=self.side,
name='grasp interpolation',
parent='debug')
handObj.setProperty('Alpha', 0.2)
handFrame.setProperty('Visible', True)
sliderMax = 100.0
def sliderChanged(sliderValue):
sliderValue = sliderValue / float(sliderMax)
t = self.splineInterp(sliderValue)
handFrame.copyFrame(t)
reachGoal = self.getReachGoalFrame(self.side)
if reachGoal:
reachGoal.copyFrame(t)
self.slider = QtGui.QSlider(QtCore.Qt.Horizontal)
self.slider.connect('valueChanged(int)', sliderChanged)
self.slider.setMaximum(sliderMax)
self.slider.show()
self.slider.resize(500, 30)
sliderChanged(sliderMax)
def showGrid(view, cellSize=0.5, numberOfCells=25, name='grid', parent='sensors', color=[1,1,1], alpha=0.05, gridTransform=None, viewBoundsFunction=None):
grid = vtk.vtkGridSource()
grid.SetScale(cellSize)
grid.SetGridSize(numberOfCells)
grid.SetSurfaceEnabled(True)
grid.Update()
gridObj = showPolyData(grid.GetOutput(), name, view=view, alpha=alpha, color=color, visible=True, parent=parent)
gridObj.gridSource = grid
gridObj.actor.GetProperty().LightingOff()
gridObj.actor.SetPickable(False)
gridTransform = gridTransform or vtk.vtkTransform()
gridObj.actor.SetUserTransform(gridTransform)
showFrame(gridTransform, name + ' frame', scale=0.2, visible=False, parent=gridObj, view=view)
gridObj.setProperty('Surface Mode', 'Wireframe')
viewBoundsFunction = viewBoundsFunction or computeViewBoundsNoGrid
def onViewBoundsRequest():
if view not in gridObj.views or not gridObj.getProperty('Visible'):
return
bounds = viewBoundsFunction(view, gridObj)
if vtk.vtkMath.AreBoundsInitialized(bounds):
view.addCustomBounds(bounds)
else:
view.addCustomBounds([-1, 1, -1, 1, -1, 1])
view.connect('computeBoundsRequest(ddQVTKWidgetView*)', onViewBoundsRequest)
return gridObj
def drawObjectInCamera(objectName,visible=True):
v = imageView.view
q = cameraview.imageManager.queue
localToCameraT = vtk.vtkTransform()
q.getTransform('local', 'CAMERA_LEFT', localToCameraT)
obj = om.findObjectByName(objectName)
if obj is None:
return
objToLocalT = transformUtils.copyFrame(obj.actor.GetUserTransform() or vtk.vtkTransform())
objPolyDataOriginal = obj.polyData
pd = objPolyDataOriginal
pd = filterUtils.transformPolyData(pd, objToLocalT)
pd = filterUtils.transformPolyData(pd, localToCameraT)
q.projectPoints('CAMERA_LEFT', pd)
vis.showPolyData(pd, ('overlay ' + objectName), view=v, color=[0,1,0],parent='camera overlay',visible=visible)
def show(data, offset=[0,0,0]):
polyData = data.getPolyData()
obj = vis.showPolyData(polyData, 'data', colorByName='RGB255')
t = vtk.vtkTransform()
t.Translate(offset)
vis.addChildFrame(obj).copyFrame(t)
return obj
def fitSupport(pickPoint=[0.92858565, 0.00213802, 0.30315629]):
om.removeFromObjectModel(om.findObjectByName('cylinder'))
polyData = getPointCloud()
t = vtk.vtkTransform()
t.Translate(pickPoint)
polyData = segmentation.cropToBox(polyData, t, [0.3, 0.3, 0.5])
addHSVArrays(polyData)
vis.updatePolyData(polyData,
'crop region',
colorByName='rgb_colors',
visible=False)
zMax = getMaxZCoordinate(polyData)
cyl = makeCylinder()
cyl.setProperty('Radius', 0.03)
cyl.setProperty('Length', zMax)
origin = segmentation.computeCentroid(polyData)
origin[2] = zMax / 2.0
t = transformUtils.frameFromPositionAndRPY(origin, [0, 0, 0])
cyl.getChildFrame().copyFrame(t)
def addChildFrame(obj, initialTransform=None):
'''
Adds a child frame to the given PolyDataItem. If initialTransform is given,
the object's polydata is transformed using the inverse of initialTransform
and then a child frame is assigned to the object to maintain its original
position.
'''
if obj.getChildFrame():
return frame
if initialTransform:
pd = filterUtils.transformPolyData(obj.polyData, initialTransform.GetLinearInverse())
obj.setPolyData(pd)
t = initialTransform
else:
t = obj.actor.GetUserTransform()
if t is None:
t = vtk.vtkTransform()
t.PostMultiply()
frame = showFrame(t, obj.getProperty('Name') + ' frame', parent=obj, scale=0.2, visible=False)
obj.actor.SetUserTransform(t)
return frame
def initHandTransforms(self, robotModel, thisCombination):
handRootLink = thisCombination['handRootLink']
robotMountLink = thisCombination['robotMountLink']
palmLink = thisCombination['palmLink']
baseLinkName = 'plane::xy::base'
if baseLinkName in self.handModel.model.getLinkNames():
baseToHandRoot = self.getLinkToLinkTransform(
self.handModel, baseLinkName, handRootLink)
else:
baseToHandRoot = self.handModel.getLinkFrame(handRootLink)
robotMountToHandRoot = self.getLinkToLinkTransform(
robotModel, robotMountLink, handRootLink)
robotMountToHandLink = self.getLinkToLinkTransform(
robotModel, robotMountLink, self.handLinkName)
robotMountToPalm = self.getLinkToLinkTransform(robotModel,
robotMountLink,
palmLink)
t = vtk.vtkTransform()
t.PostMultiply()
t.Concatenate(baseToHandRoot)
t.Concatenate(robotMountToHandRoot.GetLinearInverse())
t.Concatenate(robotMountToPalm)
self.modelToPalm = t
self.handLinkToPalm = self.getLinkToLinkTransform(
robotModel, self.handLinkName, palmLink)
self.palmToHandLink = self.handLinkToPalm.GetLinearInverse()
def newPolyData(self, name, view, parent=None):
polyData = self.getNewHandPolyData()
if isinstance(parent, str):
parent = om.getOrCreateContainer(parent)
color = [1.0, 1.0, 0.0]
if self.side == 'right':
color = [0.33, 1.0, 0.0]
obj = vis.showPolyData(polyData,
name,
view=view,
color=color,
visible=False,
parent=parent)
obj.side = self.side
frame = vtk.vtkTransform()
frame.PostMultiply()
obj.actor.SetUserTransform(frame)
frameObj = vis.showFrame(frame,
'%s frame' % name,
view=view,
scale=0.2,
visible=False,
parent=obj)
return obj
def getFrameToWorld(self, frameId):
t = vtk.vtkTransform()
t.PostMultiply()
if self.model.getFrameToWorld(frameId, t):
return t
else:
return None
def getLinkFrame(self, linkName):
t = vtk.vtkTransform()
t.PostMultiply()
if self.model.getLinkToWorld(linkName, t):
return t
else:
return None
def addChildFrame(obj, initialTransform=None):
'''
Adds a child frame to the given PolyDataItem. If initialTransform is given,
the object's polydata is transformed using the inverse of initialTransform
and then a child frame is assigned to the object to maintain its original
position.
'''
if obj.getChildFrame():
return
if initialTransform:
pd = transformPolyData(obj.polyData,
initialTransform.GetLinearInverse())
obj.setPolyData(pd)
t = obj.actor.GetUserTransform()
if t is None:
t = vtk.vtkTransform()
t.PostMultiply()
frame = showFrame(t,
obj.getProperty('Name') + ' frame',
parent=obj,
scale=0.2,
visible=False)
obj.actor.SetUserTransform(t)
return frame
def __init__(self,
channel="OPTITRACK_FRAMES",
desc_channel="OPTITRACK_DATA_DESCRIPTIONS"):
self.channel = channel
self.desc_channel = desc_channel
self.subscriber = None
self.desc_subscriber = None
self.unitConversion = 0.001
self.data_descriptions = None
self.marker_sets = om.getOrCreateContainer(
"Marker Sets", parentObj=self.getRootFolder())
self.rigid_bodies = om.getOrCreateContainer(
"Rigid Bodies", parentObj=self.getRootFolder())
self.labeled_markers = om.getOrCreateContainer(
"Labeled Markers", parentObj=self.getRootFolder())
self.unlabeled_markers = om.getOrCreateContainer(
"Unlabeled Markers", parentObj=self.getRootFolder())
self.drawEdges = False
self.markerGeometry = None
self.optitrackToWorld = vtk.vtkTransform()
self.optitrackToWorld.SetMatrix(
self.defaultOptitrackToWorld.GetMatrix())
self.initSubscriber()
self.callbacks = callbacks.CallbackRegistry([
'RIGID_BODY_LIST_CHANGED',
])
def applyCameraTexture(obj, imageManager, imageName="CAMERA_LEFT"):
imageUtime = imageManager.getUtime(imageName)
if not imageUtime:
return
cameraToLocal = vtk.vtkTransform()
imageManager.queue.getTransform(imageName, "local", imageUtime, cameraToLocal)
pd = filterUtils.transformPolyData(obj.polyData, obj.actor.GetUserTransform())
pd = filterUtils.transformPolyData(pd, cameraToLocal.GetLinearInverse())
imageManager.queue.computeTextureCoords(imageName, pd)
tcoordsArrayName = "tcoords_%s" % imageName
tcoords = pd.GetPointData().GetArray(tcoordsArrayName)
assert tcoords
obj.polyData.GetPointData().SetTCoords(None)
obj.polyData.GetPointData().SetTCoords(tcoords)
obj._updateColorByProperty()
obj.actor.SetTexture(imageManager.getTexture(imageName))
obj.actor.GetProperty().LightingOff()
obj.actor.GetProperty().SetColor([1, 1, 1])
def runIK(self,
targetFrame,
startPose=None,
graspToHandLinkFrame=None,
positionTolerance=0.0,
angleToleranceInDegrees=0.0,
seedPoseName='q_nom'):
if graspToHandLinkFrame is None:
graspToHandLinkFrame = vtk.vtkTransform()
if startPose is None:
startPose = self.getPlanningStartPose()
ikPlanner = self.robotSystem.ikPlanner
startPoseName = 'reach_start'
endPoseName = 'reach_end'
ikPlanner.addPose(startPose, startPoseName)
side = ikPlanner.reachingSide
constraints = []
constraints.append(
KukaPlanningUtils.createLockedBasePostureConstraint(
ikPlanner, startPoseName))
# positionConstraint, orientationConstraint = ikPlanner.createPositionOrientationGraspConstraints(side, targetFrame,
# graspToHandLinkFrame,
# positionTolerance=positionTolerance,
# angleToleranceInDegrees=angleToleranceInDegrees)
positionConstraint, orientationConstraint = ikPlanner.createPositionOrientationConstraint(
self.endEffectorLinkName,
targetFrame,
graspToHandLinkFrame,
positionTolerance=positionTolerance,
angleToleranceInDegrees=angleToleranceInDegrees)
positionConstraint.tspan = [1.0, 1.0]
orientationConstraint.tspan = [1.0, 1.0]
constraints.append(positionConstraint)
constraints.append(orientationConstraint)
constraintSet = ConstraintSet(ikPlanner, constraints, 'reach_end',
startPoseName)
constraintSet.ikParameters = IkParameters()
constraintSet.seedPoseName = seedPoseName
print "consraintSet ", constraintSet
print "constraintSet.seedPoseName ", constraintSet.seedPoseName
print "constraintSet.nominalPoseName ", constraintSet.nominalPoseName
endPose, info = constraintSet.runIk()
returnData = dict()
returnData['info'] = info
returnData['endPose'] = endPose
return returnData
def __init__(self, link):
self.transform = vtk.vtkTransform()
self.geometry = []
for g in link.geom:
self.geometry.extend(
Geometry.createGeometry(link.name + ' geometry data', g,
self.transform))
def makeBox():
''' has property Dimensions '''
desc = dict(classname='BoxAffordanceItem',
Name='box',
pose=transformUtils.poseFromTransform(vtk.vtkTransform()))
box = newAffordanceFromDescription(desc)
box.getChildFrame().setProperty('Scale', 0.1)
return box
def moveToGraspFrame(self, frame):
t = vtk.vtkTransform()
t.PostMultiply()
t.Concatenate(self.modelToPalm)
t.Concatenate(frame)
self.moveHandModelToFrame(self.handModel, t)
def getGroundFrame(self):
return vtk.vtkTransform()
robotModel = robotSystem.robotStateModel
baseLinkFrame = robotModel.model.getLinkFrame(robotModel.model.getLinkNames()[0])
#baseLinkFrame.PostMultiply()
#baseLinkFrame.Translate(0,0,-baseLinkFrame.GetPosition()[2])
return baseLinkFrame
def __init__(self):
self.lastUtime = 0
self.lastCameraToLocal = vtk.vtkTransform()
self.cameraToLocalFusedTransforms = []
self.cameraToLocalTransforms = []
self.pointClouds = []
def getGroundFrame(self):
return vtk.vtkTransform()
robotModel = robotSystem.robotStateModel
baseLinkFrame = robotModel.model.getLinkFrame(robotModel.model.getLinkNames()[0])
#baseLinkFrame.PostMultiply()
#baseLinkFrame.Translate(0,0,-baseLinkFrame.GetPosition()[2])
return baseLinkFrame
def getCameraToLocal(self):
"""
Returns cameraToLocal. cameraToHead is pulled from bot frames while
headToLocal is pulled from the robot model forward kinematics.
"""
headToLocal = self.robotModel.getLinkFrame(self.robotModel.getHeadLink())
cameraToHead = vtk.vtkTransform()
self.imageManager.queue.getTransform(self.cameraName, self.robotModel.getHeadLink(), 0, cameraToHead)
return transformUtils.concatenateTransforms([cameraToHead, headToLocal])
def updateGrid(self, model):
pos = self.jointController.q[:3]
x = int(np.round(pos[0])) / 10
y = int(np.round(pos[1])) / 10
z = int(np.round(pos[2] - 0.85)) / 1
t = vtk.vtkTransform()
t.Translate((x*10,y*10,z))
self.gridFrame.copyFrame(t)
def getLinkToLinkTransform(model, linkA, linkB):
linkAToWorld = model.getLinkFrame(linkA)
linkBToWorld = model.getLinkFrame(linkB)
assert linkAToWorld
assert linkBToWorld
t = vtk.vtkTransform()
t.PostMultiply()
t.Concatenate(linkAToWorld)
t.Concatenate(linkBToWorld.GetLinearInverse())
return t
def getPalmToWorldTransform(self, robotModel):
handLinkToWorld = robotModel.getLinkFrame(self.handLinkName)
t = vtk.vtkTransform()
t.PostMultiply()
t.Concatenate(self.palmToHandLink)
t.Concatenate(handLinkToWorld)
return t
def gravitizeAccelSphere(accel_sphere_poly_data, gravity_max, color=[1,0.3,0.0], alpha=0.5):
d = DebugData()
center = [0, 0, -gravity_max]
t = vtk.vtkTransform()
t.Translate(center)
polyData = filterUtils.transformPolyData(accel_sphere_poly_data, t)
obj = vis.updatePolyData(polyData, 'accel_sphere', color=color, alpha=alpha)
return polyData
def getCameraToLocal(self):
'''
Returns cameraToLocal. cameraToHead is pulled from bot frames while
headToLocal is pulled from the robot model forward kinematics.
'''
headToLocal = self.robotModel.getLinkFrame( self.headLink )
cameraToHead = vtk.vtkTransform()
# TODO: 'head' should match self.headLink
self.imageManager.queue.getTransform(self.cameraName, 'head', 0, cameraToHead)
return transformUtils.concatenateTransforms([cameraToHead, headToLocal])
def updateSphereGeometry(self):
for imageName in self.sphereImages:
sphereObj = self.getSphereGeometry(imageName)
if not sphereObj:
continue
transform = vtk.vtkTransform()
self.imageManager.queue.getBodyToCameraTransform(imageName, transform)
sphereObj.actor.SetUserTransform(transform.GetLinearInverse())
def resetCameraToRobot(view):
t = robotModel.getLinkFrame('pelvis')
if t is None:
t = vtk.vtkTransform()
focalPoint = [0.0, 0.0, 0.25]
position = [-4.0, -2.0, 2.25]
t.TransformPoint(focalPoint, focalPoint)
t.TransformPoint(position, position)
flyer = cameracontrol.Flyer(view)
flyer.zoomTo(focalPoint, position)