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 placeHandModel(displayPoint, view, side='left'):
obj, _ = vis.findPickedObject(displayPoint, view)
if isinstance(obj, vis.FrameItem):
_, handFrame = handFactory.placeHandModelWithTransform(obj.transform, view, side=side, parent=obj.parent())
handFrame.frameSync = vis.FrameSync()
handFrame.frameSync.addFrame(obj)
handFrame.frameSync.addFrame(handFrame, ignoreIncoming=True)
return
pickedPoint, prop, _, normal = vis.pickPoint(displayPoint, view, pickType='cells', tolerance=0.0, returnNormal=True)
obj = vis.getObjectByProp(prop)
if not obj:
return
yaxis = -normal
zaxis = [0,0,1]
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
zaxis = np.cross(xaxis, yaxis)
zaxis /= np.linalg.norm(zaxis)
t = transformUtils.getTransformFromAxes(-zaxis, yaxis, xaxis)
t.PostMultiply()
t.Translate(pickedPoint)
handObj, handFrame = handFactory.placeHandModelWithTransform(t, view, side=side, parent=obj)
syncFrame = getChildFrame(obj)
if syncFrame:
handFrame.frameSync = vis.FrameSync()
handFrame.frameSync.addFrame(handFrame, ignoreIncoming=True)
handFrame.frameSync.addFrame(syncFrame)
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 selectLink(self, displayPoint, view):
if not self.enabled():
return False
robotModel, _ = vis.findPickedObject(displayPoint, view)
try:
robotModel.model.getLinkNameForMesh
except AttributeError:
return False
model = robotModel.model
pickedPoint, _, polyData = vis.pickProp(displayPoint, view)
linkName = model.getLinkNameForMesh(polyData)
if not linkName:
return False
fadeValue = 1.0 if linkName == self.selectedLink else 0.05
for name in model.getLinkNames():
linkColor = model.getLinkColor(name)
linkColor.setAlphaF(fadeValue)
model.setLinkColor(name, linkColor)
if linkName == self.selectedLink:
self.selectedLink = None
vis.hideCaptionWidget()
om.removeFromObjectModel(
om.findObjectByName('selected link frame'))
else:
self.selectedLink = linkName
linkColor = model.getLinkColor(self.selectedLink)
linkColor.setAlphaF(1.0)
model.setLinkColor(self.selectedLink, linkColor)
vis.showCaptionWidget(robotModel.getLinkFrame(
self.selectedLink).GetPosition(),
self.selectedLink,
view=view)
vis.updateFrame(robotModel.getLinkFrame(self.selectedLink),
'selected link frame',
scale=0.2,
parent=robotModel)
return True
def toggleFrameWidget(displayPoint, view):
obj, _ = vis.findPickedObject(displayPoint, view)
if not isinstance(obj, vis.FrameItem):
obj = getChildFrame(obj)
if not obj:
return False
edit = not obj.getProperty('Edit')
obj.setProperty('Edit', edit)
parent = obj.parent()
if getChildFrame(parent) == obj:
parent.setProperty('Alpha', 0.5 if edit else 1.0)
return True
def selectLink(self, displayPoint, view):
if not self.enabled():
return False
robotModel, _ = vis.findPickedObject(displayPoint, view)
try:
robotModel.model.getLinkNameForMesh
except AttributeError:
return False
model = robotModel.model
pickedPoint, _, polyData = vis.pickProp(displayPoint, view)
linkName = model.getLinkNameForMesh(polyData)
if not linkName:
return False
fadeValue = 1.0 if linkName == self.selectedLink else 0.05
for name in model.getLinkNames():
linkColor = model.getLinkColor(name)
linkColor.setAlphaF(fadeValue)
model.setLinkColor(name, linkColor)
if linkName == self.selectedLink:
self.selectedLink = None
vis.hideCaptionWidget()
om.removeFromObjectModel(om.findObjectByName('selected link frame'))
else:
self.selectedLink = linkName
linkColor = model.getLinkColor(self.selectedLink)
linkColor.setAlphaF(1.0)
model.setLinkColor(self.selectedLink, linkColor)
vis.showCaptionWidget(robotModel.getLinkFrame(self.selectedLink).GetPosition(), self.selectedLink, view=view)
vis.updateFrame(robotModel.getLinkFrame(self.selectedLink), 'selected link frame', scale=0.2, parent=robotModel)
return True
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 showRightClickMenu(displayPoint, view):
pickedObj, pickedPoint = vis.findPickedObject(displayPoint, view)
if not pickedObj:
return
objectName = pickedObj.getProperty('Name')
if objectName == 'grid':
return
displayPoint = displayPoint[0], view.height - displayPoint[1]
globalPos = view.mapToGlobal(QtCore.QPoint(*displayPoint))
menu = QtGui.QMenu(view)
widgetAction = QtGui.QWidgetAction(menu)
label = QtGui.QLabel('<b>%s</b>' % objectName)
label.setContentsMargins(9,9,6,6)
widgetAction.setDefaultWidget(label)
menu.addAction(widgetAction)
menu.addSeparator()
propertiesPanel = PythonQt.dd.ddPropertiesPanel()
propertiesPanel.setBrowserModeToWidget()
propertyset.PropertyPanelHelper.addPropertiesToPanel(pickedObj.properties, propertiesPanel)
def onPropertyChanged(prop):
om.PropertyPanelHelper.setPropertyFromPanel(prop, propertiesPanel, pickedObj.properties)
propertiesPanel.connect('propertyValueChanged(QtVariantProperty*)', onPropertyChanged)
propertiesMenu = menu.addMenu('Properties')
propertiesWidgetAction = QtGui.QWidgetAction(propertiesMenu)
propertiesWidgetAction.setDefaultWidget(propertiesPanel)
propertiesMenu.addAction(propertiesWidgetAction)
def onDelete():
om.removeFromObjectModel(pickedObj)
def onHide():
pickedObj.setProperty('Visible', False)
def onSelect():
om.setActiveObject(pickedObj)
reachFrame = getAsFrame(pickedObj)
collisionParent = getCollisionParent(pickedObj)
def onReachLeft():
reachToFrame(reachFrame, 'left', collisionParent)
def onReachRight():
reachToFrame(reachFrame, 'right', collisionParent)
def flipHandSide():
for obj in [pickedObj] + pickedObj.children():
if not isGraspSeed(obj):
continue
side = 'right' if obj.side == 'left' else 'left'
obj.side = side
color = [1.0, 1.0, 0.0]
if side == 'right':
color = [0.33, 1.0, 0.0]
obj.setProperty('Color', color)
def flipHandThumb():
handFrame = pickedObj.children()[0]
t = transformUtils.copyFrame(handFrame.transform)
t.PreMultiply()
t.RotateY(180)
handFrame.copyFrame(t)
pickedObj._renderAllViews()
def onSplineLeft():
splinewidget.planner.newSpline(pickedObj, 'left')
def onSplineRight():
splinewidget.planner.newSpline(pickedObj, 'right')
def getPointCloud(obj):
try:
obj = obj.model.polyDataObj
except AttributeError:
pass
try:
obj.polyData
except AttributeError:
return None
if obj and obj.polyData.GetNumberOfPoints():# and (obj.polyData.GetNumberOfCells() == obj.polyData.GetNumberOfVerts()):
return obj
pointCloudObj = getPointCloud(pickedObj)
affordanceObj = pickedObj if isinstance(pickedObj, affordanceitems.AffordanceItem) else None
def onSegmentGround():
groundPoints, scenePoints = segmentation.removeGround(pointCloudObj.polyData)
vis.showPolyData(groundPoints, 'ground points', color=[0,1,0], parent='segmentation')
vis.showPolyData(scenePoints, 'scene points', color=[1,0,1], parent='segmentation')
pickedObj.setProperty('Visible', False)
def onCopyPointCloud():
global lastRandomColor
polyData = vtk.vtkPolyData()
polyData.DeepCopy(pointCloudObj.polyData)
if pointCloudObj.getChildFrame():
polyData = segmentation.transformPolyData(polyData, pointCloudObj.getChildFrame().transform)
polyData = segmentation.addCoordArraysToPolyData(polyData)
# generate random color, and average with a common color to make them generally similar
lastRandomColor = lastRandomColor + 0.1 + 0.1*random.random()
rgb = colorsys.hls_to_rgb(lastRandomColor, 0.7, 1.0)
obj = vis.showPolyData(polyData, pointCloudObj.getProperty('Name') + ' copy', color=rgb, parent='point clouds')
t = vtk.vtkTransform()
t.PostMultiply()
t.Translate(filterUtils.computeCentroid(polyData))
segmentation.makeMovable(obj, t)
om.setActiveObject(obj)
pickedObj.setProperty('Visible', False)
def onMergeIntoPointCloud():
allPointClouds = om.findObjectByName('point clouds')
if allPointClouds:
allPointClouds = [i.getProperty('Name') for i in allPointClouds.children()]
sel = QtGui.QInputDialog.getItem(None, "Point Cloud Merging", "Pick point cloud to merge into:", allPointClouds, current=0, editable=False)
sel = om.findObjectByName(sel)
# Make a copy of each in same frame
polyDataInto = vtk.vtkPolyData()
polyDataInto.ShallowCopy(sel.polyData)
if sel.getChildFrame():
polyDataInto = segmentation.transformPolyData(polyDataInto, sel.getChildFrame().transform)
polyDataFrom = vtk.vtkPolyData()
polyDataFrom.DeepCopy(pointCloudObj.polyData)
if pointCloudObj.getChildFrame():
polyDataFrom = segmentation.transformPolyData(polyDataFrom, pointCloudObj.getChildFrame().transform)
# Actual merge
append = filterUtils.appendPolyData([polyDataFrom, polyDataInto])
if sel.getChildFrame():
polyDataInto = segmentation.transformPolyData(polyDataInto, sel.getChildFrame().transform.GetInverse())
# resample
append = segmentationroutines.applyVoxelGrid(append, 0.01)
append = segmentation.addCoordArraysToPolyData(append)
# Recenter the frame
sel.setPolyData(append)
t = vtk.vtkTransform()
t.PostMultiply()
t.Translate(filterUtils.computeCentroid(append))
segmentation.makeMovable(sel, t)
# Hide the old one
if pointCloudObj.getProperty('Name') in allPointClouds:
pointCloudObj.setProperty('Visible', False)
def onSegmentTableScene():
data = segmentation.segmentTableScene(pointCloudObj.polyData, pickedPoint)
vis.showClusterObjects(data.clusters + [data.table], parent='segmentation')
def onSegmentDrillAlignedWithTable():
segmentation.segmentDrillAlignedWithTable(pickedPoint, pointCloudObj.polyData)
def onCachePickedPoint():
''' Cache the Picked Point for general purpose use'''
global lastCachedPickedPoint
lastCachedPickedPoint = pickedPoint
#data = segmentation.segmentTableScene(pointCloudObj.polyData, pickedPoint)
#vis.showClusterObjects(data.clusters + [data.table], parent='segmentation')
def onLocalPlaneFit():
planePoints, normal = segmentation.applyLocalPlaneFit(pointCloudObj.polyData, pickedPoint, searchRadius=0.1, searchRadiusEnd=0.2)
obj = vis.showPolyData(planePoints, 'local plane fit', color=[0,1,0])
obj.setProperty('Point Size', 7)
fields = segmentation.makePolyDataFields(obj.polyData)
pose = transformUtils.poseFromTransform(fields.frame)
desc = dict(classname='BoxAffordanceItem', Name='local plane', Dimensions=list(fields.dims), pose=pose)
box = segmentation.affordanceManager.newAffordanceFromDescription(desc)
def onOrientToMajorPlane():
polyData, planeFrame = segmentation.orientToMajorPlane(pointCloudObj.polyData, pickedPoint=pickedPoint)
pointCloudObj.setPolyData(polyData)
def onDiskGlyph():
result = segmentation.applyDiskGlyphs(pointCloudObj.polyData)
obj = vis.showPolyData(result, 'disks', color=[0.8,0.8,0.8])
om.setActiveObject(obj)
pickedObj.setProperty('Visible', False)
def onArrowGlyph():
result = segmentation.applyArrowGlyphs(pointCloudObj.polyData)
obj = vis.showPolyData(result, 'disks')
def onSegmentationEditor():
segmentationpanel.activateSegmentationMode(pointCloudObj.polyData)
def addNewFrame():
t = transformUtils.copyFrame(affordanceObj.getChildFrame().transform)
t.PostMultiply()
t.Translate(np.array(pickedPoint) - np.array(t.GetPosition()))
newFrame = vis.showFrame(t, '%s frame %d' % (affordanceObj.getProperty('Name'), len(affordanceObj.children())), scale=0.2, parent=affordanceObj)
affordanceObj.getChildFrame().getFrameSync().addFrame(newFrame, ignoreIncoming=True)
def copyAffordance():
desc = dict(affordanceObj.getDescription())
del desc['uuid']
desc['Name'] = desc['Name'] + ' copy'
aff = robotSystem.affordanceManager.newAffordanceFromDescription(desc)
aff.getChildFrame().setProperty('Edit', True)
def onPromoteToAffordance():
affObj = affordanceitems.MeshAffordanceItem.promotePolyDataItem(pickedObj)
robotSystem.affordanceManager.registerAffordance(affObj)
actions = [
(None, None),
('Hide', onHide),
('Delete', onDelete),
('Select', onSelect)
]
if affordanceObj:
actions.extend([
('Copy affordance', copyAffordance),
('Add new frame', addNewFrame),
])
elif type(pickedObj) == vis.PolyDataItem:
actions.extend([
('Promote to Affordance', onPromoteToAffordance),
])
if isGraspSeed(pickedObj):
actions.extend([
(None, None),
('Flip Side', flipHandSide),
('Flip Thumb', flipHandThumb),
])
if reachFrame is not None:
actions.extend([
(None, None),
('Reach Left', onReachLeft),
('Reach Right', onReachRight),
#('Spline Left', onSplineLeft),
#('Spline Right', onSplineRight),
])
if pointCloudObj:
actions.extend([
(None, None),
('Copy Pointcloud', onCopyPointCloud),
('Merge Pointcloud Into', onMergeIntoPointCloud),
('Segment Ground', onSegmentGround),
('Segment Table', onSegmentTableScene),
('Segment Drill Aligned', onSegmentDrillAlignedWithTable),
('Local Plane Fit', onLocalPlaneFit),
('Orient with Horizontal', onOrientToMajorPlane),
('Arrow Glyph', onArrowGlyph),
('Disk Glyph', onDiskGlyph),
('Cache Pick Point', onCachePickedPoint),
(None, None),
('Open Segmentation Editor', onSegmentationEditor)
])
for actionName, func in actions:
if not actionName:
menu.addSeparator()
else:
action = menu.addAction(actionName)
action.connect('triggered()', func)
selectedAction = menu.popup(globalPos)