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 addTestData():
d = DebugData()
# d.addSphere((0,0,0), radius=0.5)
d.addArrow((0,0,0), (0,0,1), color=[1,0,0])
d.addArrow((0,0,1), (0,.5,1), color=[0,1,0])
vis.showPolyData(d.getPolyData(), 'debug data', colorByName='RGB255')
view.resetCamera()
def _openGeometry(self, filename):
polyData = ioUtils.readPolyData(filename)
if not polyData or not polyData.GetNumberOfPoints():
self._showErrorMessage('Failed to read any data from file: %s' % filename, title='Reader error')
return
vis.showPolyData(polyData, os.path.basename(filename), parent='files')
def onOpenGeometry(filename):
if filename.lower().endswith('wrl'):
onOpenVrml(filename)
return
polyData = io.readPolyData(filename)
if not polyData or not polyData.GetNumberOfPoints():
app.showErrorMessage('Failed to read any data from file: %s' % filename, title='Reader error')
return
vis.showPolyData(polyData, os.path.basename(filename), parent='files')
def _openGeometry(self, filename):
if filename.lower().endswith("wrl"):
self.onOpenVrml(filename)
return
polyData = ioUtils.readPolyData(filename)
if not polyData or not polyData.GetNumberOfPoints():
self._showErrorMessage("Failed to read any data from file: %s" % filename, title="Reader error")
return
vis.showPolyData(polyData, os.path.basename(filename), parent="files")
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 sparsifyStereoCloud(polyData):
''' Take in a typical Stereo Camera Point Cloud
Filter it down to about the density of a lidar point cloud
and remove outliers
'''
# >>> strips color out <<<
polyData = applyVoxelGrid(polyData, leafSize=0.01)
# remove outliers
polyData = labelOutliers(polyData)
vis.showPolyData(polyData, 'is_outlier', colorByName='is_outlier', visible=False, parent=getDebugFolder())
polyData = thresholdPoints(polyData, 'is_outlier', [0.0, 0.0])
return polyData
def buildBoundaries(d, scale=1.0, boundaryType="Warehouse", alpha=1.0, withData=True):
if boundaryType == "Warehouse":
worldXmin = -20
worldXmax = 100
worldYmin = -10
worldYmax = 10
if boundaryType == "Square":
worldXmin = -50*scale
worldXmax = 50*scale
worldYmin = -50*scale
worldYmax = 10*scale
if withData:
# draw boundaries for the world
NW = (worldXmax, worldYmax, 0)
NE = (worldXmax, worldYmin, 0)
SE = (worldXmin, worldYmin, 0)
SW = (worldXmin, worldYmax, 0)
NW = (worldXmax, worldYmax, 0)
listOfCorners = [NW, NE, SE, SW, NW]
for idx, value in enumerate(listOfCorners[:-1]):
firstEndpt = value
secondEndpt = listOfCorners[idx+1]
d.addLine(firstEndpt, secondEndpt, radius=1.0)
obj = vis.showPolyData(d.getPolyData(), 'boundaries', alpha=0.0)
return worldXmin, worldXmax, worldYmin, worldYmax
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 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 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 makeMarker(i):
obj = vis.showPolyData(shallowCopy(geom),
modelName + ' marker %d' % i,
color=modelColor,
parent=modelFolder)
vis.addChildFrame(obj)
return obj
def run(self):
radius = self.properties.getProperty('Radius')
thickness = 0.03
folder = om.getOrCreateContainer('affordances')
frame = self.computeValveFrame()
d = DebugData()
d.addLine(np.array([0, 0, -thickness / 2.0]),
np.array([0, 0, thickness / 2.0]),
radius=radius)
mesh = d.getPolyData()
params = dict(radius=radius,
length=thickness,
xwidth=radius,
ywidth=radius,
zwidth=thickness,
otdf_type='steering_cyl',
friendly_name='valve')
affordance = vis.showPolyData(mesh,
'valve',
color=[0.0, 1.0, 0.0],
cls=affordanceitems.FrameAffordanceItem,
parent=folder,
alpha=1.0)
frame = vis.showFrame(frame,
'valve frame',
parent=affordance,
visible=False,
scale=radius)
affordance.actor.SetUserTransform(frame.transform)
affordance.setAffordanceParams(params)
affordance.updateParamsFromActorTransform()
def run(self):
polyData = self.getPointCloud()
annotation = self.getAnnotationInput()
annotationPoint = annotation.annotationPoints[0]
planePoints, normal = segmentation.applyLocalPlaneFit(polyData, annotationPoint, searchRadius=0.1, searchRadiusEnd=0.2)
viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection()
if np.dot(normal, viewDirection) < 0:
normal = -normal
xaxis = normal
zaxis = [0, 0, 1]
yaxis = np.cross(zaxis, xaxis)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
yaxis /= np.linalg.norm(yaxis)
t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
t.PostMultiply()
t.Translate(annotationPoint)
polyData = annotation.polyData
polyData = segmentation.transformPolyData(polyData, t.GetLinearInverse())
annotation.setProperty('Visible', False)
om.removeFromObjectModel(om.findObjectByName('wall'))
obj = vis.showPolyData(polyData, 'wall')
obj.actor.SetUserTransform(t)
vis.showFrame(t, 'wall frame', scale=0.2, parent=obj)
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 pickArea(self, startPos, endPos):
self.iren.SetInteractorStyle(self.prevStyle)
picker = vtk.vtkAreaPicker()
picker.AreaPick(min(startPos[0], endPos[0]),
min(startPos[1], endPos[1]),
max(startPos[0], endPos[0]),
max(startPos[1], endPos[1]),
self.view.renderer())
frustum = picker.GetFrustum()
extractGeometry = vtk.vtkExtractPolyDataGeometry()
extractGeometry.SetImplicitFunction(frustum)
extractGeometry.SetInputData(self.polyData)
extractGeometry.ExtractBoundaryCellsOn()
extractGeometry.Update()
selected = filterUtils.cleanPolyData(extractGeometry.GetOutput())
if not selected.GetNumberOfPoints():
return
pickedIds = vnp.getNumpyFromVtk(selected, 'point_ids')
vnp.getNumpyFromVtk(self.polyData, 'is_selected')[pickedIds] = self.selectMode
selection = self.getSelectedPoints()
if not self.selectionObj:
self.selectionObj = vis.showPolyData(selection, 'selected points', color=self.selectionColor, parent='selection')
self.selectionObj.setProperty('Point Size', self.selectionPointSize)
else:
self.selectionObj.setPolyData(selection)
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 onOpenVrml(self, filename):
meshes, color = ioUtils.readVrml(filename)
folder = om.getOrCreateContainer(os.path.basename(filename), parentObj=om.getOrCreateContainer("files"))
for i, pair in enumerate(zip(meshes, color)):
mesh, color = pair
obj = vis.showPolyData(mesh, "mesh %d" % i, color=color, parent=folder)
vis.addChildFrame(obj)
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 update(self):
if not self.renderObserver:
def onEndRender(obj, event):
if self._block:
return
if not self.singleShotTimer.singleShotTimer.isActive():
self.singleShotTimer.singleShot(0)
self.renderObserver = self.view.renderWindow().AddObserver('EndEvent', onEndRender)
self._block = True
self.view.forceRender()
self.updateBufferImages()
self._block = False
depthImage, polyData, _ = computeDepthImageAndPointCloud(self.getDepthBufferImage(), self.getColorBufferImage(), self.view.camera())
self.depthScaleFilter.SetInputData(depthImage)
self.depthScaleFilter.Update()
self.depthImageLookupTable.SetRange(self.depthScaleFilter.GetOutput().GetScalarRange())
self.imageMapToColors.Update()
self.imageView.resetCamera()
#self.imageView.view.render()
if not self.pointCloudObj:
self.pointCloudObj = vis.showPolyData(polyData, 'point cloud', colorByName='rgb', view=self.pointCloudView)
else:
self.pointCloudObj.setPolyData(polyData)
self.pointCloudView.render()
def run(self):
polyData = self.getPointCloud()
annotation = self.getAnnotationInput()
annotationPoint = annotation.annotationPoints[0]
planePoints, normal = segmentation.applyLocalPlaneFit(
polyData, annotationPoint, searchRadius=0.1, searchRadiusEnd=0.2)
viewDirection = segmentation.SegmentationContext.getGlobalInstance(
).getViewDirection()
if np.dot(normal, viewDirection) < 0:
normal = -normal
xaxis = normal
zaxis = [0, 0, 1]
yaxis = np.cross(zaxis, xaxis)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
yaxis /= np.linalg.norm(yaxis)
t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
t.PostMultiply()
t.Translate(annotationPoint)
polyData = annotation.polyData
polyData = segmentation.transformPolyData(polyData,
t.GetLinearInverse())
annotation.setProperty('Visible', False)
om.removeFromObjectModel(om.findObjectByName('wall'))
obj = vis.showPolyData(polyData, 'wall')
obj.actor.SetUserTransform(t)
vis.showFrame(t, 'wall frame', scale=0.2, parent=obj)
def getSphereGeometry(self, imageName):
sphereObj = self.sphereObjects.get(imageName)
if sphereObj:
return sphereObj
if not self.imageManager.getImage(imageName).GetDimensions()[0]:
return None
sphereResolution = 50
sphereRadii = {"CAMERA_LEFT": 20, "CAMERACHEST_LEFT": 20, "CAMERACHEST_RIGHT": 20}
geometry = makeSphere(sphereRadii[imageName], sphereResolution)
self.imageManager.queue.computeTextureCoords(imageName, geometry)
tcoordsArrayName = "tcoords_%s" % imageName
vtkNumpy.addNumpyToVtk(geometry, vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:, 0].copy(), "tcoords_U")
vtkNumpy.addNumpyToVtk(geometry, vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:, 1].copy(), "tcoords_V")
geometry = clipRange(geometry, "tcoords_U", [0.0, 1.0])
geometry = clipRange(geometry, "tcoords_V", [0.0, 1.0])
geometry.GetPointData().SetTCoords(geometry.GetPointData().GetArray(tcoordsArrayName))
sphereObj = vis.showPolyData(geometry, imageName, view=self.view, parent="cameras")
sphereObj.actor.SetTexture(self.imageManager.getTexture(imageName))
sphereObj.actor.GetProperty().LightingOff()
self.view.renderer().RemoveActor(sphereObj.actor)
rendererId = 2 - self.sphereImages.index(imageName)
self.renderers[rendererId].AddActor(sphereObj.actor)
self.sphereObjects[imageName] = sphereObj
return sphereObj
def buildRobot(x=0,y=0):
d = DebugData()
d.addCone((x,y,0), (1,0,0), height=0.2, radius=0.1)
obj = vis.showPolyData(d.getPolyData(), 'robot')
frame = vis.addChildFrame(obj)
return obj
def buildWorld():
d = DebugData()
d.addLine((2,-1,0), (2,1,0), radius=0.1)
d.addLine((2,-1,0), (1,-2,0), radius=0.1)
obj = vis.showPolyData(d.getPolyData(), 'world')
return obj
def _handleRigidBodies(self, bodies):
if not bodies:
return
folder = self.getRootFolder()
for body in bodies:
name = 'Body ' + str(body.id)
x,y,z,w = body.quat
quat = (w,x,y,z)
bodyToOptitrack = transformUtils.transformFromPose(body.xyz, quat)
bodyToWorld = transformUtils.concatenateTransforms((bodyToOptitrack, self.optitrackToWorld))
obj = folder.findChild(name)
if not obj:
geometry = self._makeMarkers(body.marker_xyz)
geometry = filterUtils.transformPolyData(geometry, bodyToOptitrack.GetLinearInverse())
obj = vis.showPolyData(geometry, name, parent=folder, color=[1,0,0])
frameObj = vis.addChildFrame(obj)
frameObj.setProperty('Scale', 0.2)
frameObj.setProperty('Visible', True)
obj.getChildFrame().copyFrame(bodyToWorld)
def update(self, snapshot=False):
"""
Visualizes the pointclouds set to true. This should be called from the main thread
:return:
:rtype:
"""
for topic, data in self._subscribers.iteritems():
if not data['visualize']:
continue
msg = data['subscriber'].lastMsg
if msg is None:
continue
# get frame
# TransformStamped
# this might need to be called in thread
try:
T_W_pointcloud_stamped = self._tf_buffer.lookup_transform(
self._expressed_in_frame, msg.header.frame_id,
msg.header.stamp)
except:
continue
T_W_pointcloud = ros_numpy.numpify(
T_W_pointcloud_stamped.transform)
T_W_pointcloud_vtk = transformUtils.getTransformFromNumpy(
T_W_pointcloud)
pointcloud_numpy = DirectorROSVisualizer.numpy_from_pointcloud2_msg(
msg)
pointcloud_vtk = vnp.getVtkPolyDataFromNumpyPoints(
pointcloud_numpy)
pointcloud_vtk = filterUtils.transformPolyData(
pointcloud_vtk, T_W_pointcloud_vtk)
data['pointcloud'] = pointcloud_vtk
if snapshot:
name = data["name"] + " snapshot"
vis.showPolyData(pointcloud_vtk,
name,
parent=self._vis_container)
else:
vis.updatePolyData(pointcloud_vtk,
data['name'],
parent=self._vis_container)
def handle_message(self, msg):
# Limits the rate of message handling, since redrawing is done in the
# message handler.
self._sub.setSpeedLimit(30)
# Removes the folder completely.
om.removeFromObjectModel(om.findObjectByName(self._folder_name))
# Recreates folder.
folder = om.getOrCreateContainer(self._folder_name)
# A map from pair of body names to a list of contact forces
collision_pair_to_forces = {}
for contact in msg.contact_info:
point = np.array([contact.contact_point[0],
contact.contact_point[1],
contact.contact_point[2]])
force = np.array([contact.contact_force[0],
contact.contact_force[1],
contact.contact_force[2]])
mag = np.linalg.norm(force)
if mag > 1e-4:
mag = 0.3 / mag
key1 = (str(contact.body1_name), str(contact.body2_name))
key2 = (str(contact.body2_name), str(contact.body1_name))
if key1 in collision_pair_to_forces:
collision_pair_to_forces[key1].append(
(point, point + mag * force))
elif key2 in collision_pair_to_forces:
collision_pair_to_forces[key2].append(
(point, point + mag * force))
else:
collision_pair_to_forces[key1] = [(point, point + mag * force)]
for key, list_of_forces in iteritems(collision_pair_to_forces):
d = DebugData()
for force_pair in list_of_forces:
d.addArrow(start=force_pair[0],
end=force_pair[1],
tubeRadius=0.005,
headRadius=0.01)
vis.showPolyData(d.getPolyData(), str(key), parent=folder,
color=[0.2, 0.8, 0.2])
def handle_message(self, msg):
# Limits the rate of message handling, since redrawing is done in the
# message handler.
self._sub.setSpeedLimit(30)
# Removes the folder completely.
om.removeFromObjectModel(om.findObjectByName(self._folder_name))
# Recreates folder.
folder = om.getOrCreateContainer(self._folder_name)
# A map from pair of body names to a list of contact forces
collision_pair_to_forces = {}
for contact in msg.contact_info:
point = np.array([contact.contact_point[0],
contact.contact_point[1],
contact.contact_point[2]])
force = np.array([contact.contact_force[0],
contact.contact_force[1],
contact.contact_force[2]])
mag = np.linalg.norm(force)
if mag > 1e-4:
mag = 0.3 / mag
key1 = (str(contact.body1_name), str(contact.body2_name))
key2 = (str(contact.body2_name), str(contact.body1_name))
if key1 in collision_pair_to_forces:
collision_pair_to_forces[key1].append(
(point, point + mag * force))
elif key2 in collision_pair_to_forces:
collision_pair_to_forces[key2].append(
(point, point + mag * force))
else:
collision_pair_to_forces[key1] = [(point, point + mag * force)]
for key, list_of_forces in iteritems(collision_pair_to_forces):
d = DebugData()
for force_pair in list_of_forces:
d.addArrow(start=force_pair[0],
end=force_pair[1],
tubeRadius=0.005,
headRadius=0.01)
vis.showPolyData(
d.getPolyData(), str(key), parent=folder, color=[0, 1, 0])
def centerObject(visObj):
polyData = filterUtils.transformPolyData(visObj.polyData,
visObj.actor.GetUserTransform())
name = visObj.getProperty('Name')
om.removeFromObjectModel(visObj)
newVisObj = vis.showPolyData(polyData, name)
vis.addChildFrame(newVisObj)
def buildCircleWorld(percentObsDensity,
nonRandom=False,
circleRadius=3,
scale=None,
randomSeed=5,
obstaclesInnerFraction=1.0):
#print "building circle world"
if nonRandom:
np.random.seed(randomSeed)
d = DebugData()
worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(
d, scale=scale)
#print "boundaries done"
worldArea = (worldXmax - worldXmin) * (worldYmax - worldYmin)
#print worldArea
obsScalingFactor = 1.0 / 12.0
maxNumObstacles = obsScalingFactor * worldArea
numObstacles = int(obstaclesInnerFraction**2 * percentObsDensity /
100.0 * maxNumObstacles)
#print numObstacles
# draw random stick obstacles
obsLength = 2.0
obsXmin = worldXmin + (1 - obstaclesInnerFraction) / 2.0 * (worldXmax -
worldXmin)
obsXmax = worldXmax - (1 - obstaclesInnerFraction) / 2.0 * (worldXmax -
worldXmin)
obsYmin = worldYmin + (1 - obstaclesInnerFraction) / 2.0 * (worldYmax -
worldYmin)
obsYmax = worldYmax - (1 - obstaclesInnerFraction) / 2.0 * (worldYmax -
worldYmin)
for i in xrange(numObstacles):
firstX = obsXmin + np.random.rand() * (obsXmax - obsXmin)
firstY = obsYmin + np.random.rand() * (obsYmax - obsYmin)
firstEndpt = (firstX, firstY, +0.2)
secondEndpt = (firstX, firstY, -0.2)
#d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn())
d.addLine(firstEndpt, secondEndpt, radius=circleRadius)
obj = vis.showPolyData(d.getPolyData(), 'world')
world = World()
world.visObj = obj
world.Xmax = worldXmax
world.Xmin = worldXmin
world.Ymax = worldYmax
world.Ymin = worldYmin
world.numObstacles = numObstacles
world.percentObsDensity = percentObsDensity
return world
def onNewWalkingGoal(self, walkingGoal=None):
walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel)
frameObj = vis.updateFrame(walkingGoal,
'walking goal',
parent='planning',
scale=0.25)
frameObj.setProperty('Edit', True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
if self.placer:
self.placer.stop()
terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
if terrain:
pos = np.array(frameObj.transform.GetPosition())
polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
if polyData.GetNumberOfPoints():
polyData = segmentation.labelDistanceToLine(
polyData, pos, pos + [0, 0, 1])
polyData = segmentation.thresholdPoints(
polyData, 'distance_to_line', [0.0, 0.1])
if polyData.GetNumberOfPoints():
pos[2] = np.nanmax(
vnp.getNumpyFromVtk(polyData, 'Points')[:, 2])
frameObj.transform.Translate(
pos - np.array(frameObj.transform.GetPosition()))
d = DebugData()
d.addSphere((0, 0, 0), radius=0.03)
handle = vis.showPolyData(d.getPolyData(),
'walking goal terrain handle',
parent=frameObj,
visible=True,
color=[1, 1, 0])
handle.actor.SetUserTransform(frameObj.transform)
self.placer = PlacerWidget(app.getCurrentRenderView(), handle,
terrain)
def onFramePropertyModified(propertySet, propertyName):
if propertyName == 'Edit':
if propertySet.getProperty(propertyName):
self.placer.start()
else:
self.placer.stop()
frameObj.properties.connectPropertyChanged(onFramePropertyModified)
onFramePropertyModified(frameObj, 'Edit')
frameObj.connectFrameModified(self.onWalkingGoalModified)
self.onWalkingGoalModified(frameObj)
def sparsifyStereoCloud(polyData):
''' Take in a typical Stereo Camera Point Cloud
Filter it down to about the density of a lidar point cloud
and remove outliers
'''
# >>> strips color out <<<
polyData = applyVoxelGrid(polyData, leafSize=0.01)
# remove outliers
polyData = labelOutliers(polyData)
vis.showPolyData(polyData,
'is_outlier',
colorByName='is_outlier',
visible=False,
parent=getDebugFolder())
polyData = thresholdPoints(polyData, 'is_outlier', [0.0, 0.0])
return polyData
def onOpenVrml(self, filename):
meshes, color = ioutils.readVrml(filename)
folder = om.getOrCreateContainer(
os.path.basename(filename), parentObj=self.getRootFolder()
)
for i, pair in enumerate(zip(meshes, color)):
mesh, color = pair
obj = vis.showPolyData(mesh, "mesh %d" % i, color=color, parent=folder)
vis.addChildFrame(obj)
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 drawContactPts(self, obj, footstep, **kwargs):
leftPoints, rightPoints = FootstepsDriver.getContactPts(footstep.params.support_contact_groups)
contact_pts = rightPoints if footstep.is_right_foot else leftPoints
d = DebugData()
for pt in contact_pts:
d.addSphere(pt, radius=0.01)
d_obj = vis.showPolyData(d.getPolyData(), "contact points", parent=obj, **kwargs)
d_obj.actor.SetUserTransform(obj.actor.GetUserTransform())
def testSuperPCS4(self):
"""
Test the SuperPCS4 algorithm on some default data
:return:
"""
baseName = os.path.join(CorlUtils.getCorlDataDir(),
'registration-output/robot-scene')
pointCloudFile = os.path.join(baseName, 'robot_mesh.vtp')
robotMeshFile = os.path.join(baseName, 'robot_mesh.vtp')
robotMeshPointcloudFile = os.path.join(baseName,
'robot_mesh_pointcloud.vtp')
pointCloud = ioUtils.readPolyData(pointCloudFile)
robotMesh = ioUtils.readPolyData(robotMeshFile)
robotMeshPointcloud = ioUtils.readPolyData(robotMeshPointcloudFile)
# PCS4 algorithm performs very differently if you remove the origin point
# pointCloud = removeOriginPoints(pointCloud)
pointCloud = segmentation.applyVoxelGrid(pointCloud, leafSize=0.01)
sceneTransform = transformUtils.frameFromPositionAndRPY([0, 0, 0],
[0, 0, 90])
pointCloud = filterUtils.transformPolyData(pointCloud, sceneTransform)
# robotMeshPointcloud = shuffleAndShiftPoints(robotMeshPointcloud)
# pointCloud = shuffleAndShiftPoints(pointCloud)
print pointCloud.GetNumberOfPoints()
print robotMeshPointcloud.GetNumberOfPoints()
sceneName = 'scene pointcloud'
modelName = 'model pointcloud'
vis.showPolyData(robotMeshPointcloud, modelName)
vis.showPolyData(pointCloud, sceneName)
self.view.resetCamera()
self.view.forceRender()
sceneToModelTransform = SuperPCS4.run(pointCloud, robotMeshPointcloud)
GlobalRegistration.showAlignedPointcloud(pointCloud,
sceneToModelTransform,
sceneName + " aligned")
def buildLineSegmentTestWorld(percentObsDensity, nonRandom=False, circleRadius=3, scale=1.0, randomSeed=5,
obstaclesInnerFraction=1.0):
#print "building circle world"
if nonRandom:
np.random.seed(randomSeed)
d = DebugData()
worldXmin = 5
worldXmax = 25
worldYmin = -20
worldYmax = 20
#print "boundaries done"
worldArea = (worldXmax-worldXmin)*(worldYmax-worldYmin)
#print worldArea
obsScalingFactor = 1.0/12.0
maxNumObstacles = obsScalingFactor * worldArea
numObstacles = int(obstaclesInnerFraction**2 * percentObsDensity/100.0 * maxNumObstacles)
print numObstacles, " is num obstacles"
# draw random stick obstacles
obsLength = 2.0
obsXmin = worldXmin + (1-obstaclesInnerFraction)/2.0*(worldXmax - worldXmin)
obsXmax = worldXmax - (1-obstaclesInnerFraction)/2.0*(worldXmax - worldXmin)
obsYmin = worldYmin + (1-obstaclesInnerFraction)/2.0*(worldYmax - worldYmin)
obsYmax = worldYmax - (1-obstaclesInnerFraction)/2.0*(worldYmax - worldYmin)
for i in xrange(numObstacles):
firstX = obsXmin + np.random.rand()*(obsXmax-obsXmin)
firstY = obsYmin + np.random.rand()*(obsYmax-obsYmin)
secondX = obsXmin + np.random.rand()*(obsXmax-obsXmin)
secondY = obsYmin + np.random.rand()*(obsYmax-obsYmin)
firstEndpt = (firstX,firstY,0.2)
secondEndpt = (firstX,firstY,-0.2)
#d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn())
d.addLine(firstEndpt, secondEndpt, radius=1.0)
obj = vis.showPolyData(d.getPolyData(), 'world')
world = World()
world.visObj = obj
world.Xmax = worldXmax
world.Xmin = worldXmin
world.Ymax = worldYmax
world.Ymin = worldYmin
world.numObstacles = numObstacles
world.percentObsDensity = percentObsDensity
return world
def drawTargetPath(self):
path = DebugData()
for i in range(1,len(self.targetPath)):
p0 = self.targetPath[i-1].GetPosition()
p1 = self.targetPath[i].GetPosition()
path.addLine ( np.array( p0 ) , np.array( p1 ), radius= 0.005)
pathMesh = path.getPolyData()
self.targetPathMesh = vis.showPolyData(pathMesh, 'target frame desired path', color=[0.0, 0.3, 1.0], parent=self.targetAffordance, alpha=0.6)
self.targetPathMesh.actor.SetUserTransform(self.targetFrame)
def onShowMapButton(self):
# reloads the map each time - in case its changed on disk:
#if (self.octomap_cloud is None):
filename = director.getDRCBaseDir() + "/software/build/data/octomap.pcd"
self.octomap_cloud = io.readPolyData(filename) # c++ object called vtkPolyData
assert (self.octomap_cloud.GetNumberOfPoints() !=0 )
# clip point cloud to height - doesnt work very well yet. need to know robot's height
#self.octomap_cloud = segmentation.cropToLineSegment(self.octomap_cloud, np.array([0,0,-10]), np.array([0,0,3]) )
# access to z values
#points= vnp.getNumpyFromVtk(self.octomap_cloud, 'Points')
#zvalues = points[:,2]
# remove previous map:
folder = om.getOrCreateContainer("segmentation")
om.removeFromObjectModel(folder)
vis.showPolyData(self.octomap_cloud, 'prior map', alpha=1.0, color=[0,0,0.4])
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 onShowMapButton(self):
# reloads the map each time - in case its changed on disk:
# if (self.octomap_cloud is None):
filename = director.getDRCBaseDir() + "/software/build/data/octomap.pcd"
self.octomap_cloud = io.readPolyData(filename) # c++ object called vtkPolyData
assert self.octomap_cloud.GetNumberOfPoints() != 0
# clip point cloud to height - doesnt work very well yet. need to know robot's height
# self.octomap_cloud = segmentation.cropToLineSegment(self.octomap_cloud, np.array([0,0,-10]), np.array([0,0,3]) )
# access to z values
# points= vnp.getNumpyFromVtk(self.octomap_cloud, 'Points')
# zvalues = points[:,2]
# remove previous map:
folder = om.getOrCreateContainer("segmentation")
om.removeFromObjectModel(folder)
vis.showPolyData(self.octomap_cloud, "prior map", alpha=1.0, color=[0, 0, 0.4])
def __init__(self, uid, view, seed_pose, irisDriver, existing_region=None):
d = DebugData()
self.uid = uid
vis.PolyDataItem.__init__(self, "IRIS region {:d}".format(uid), d.getPolyData(), view)
self.transform = seed_pose
d.addSphere((0,0,0), radius=0.02)
self.seedObj = vis.showPolyData(d.getPolyData(), 'region seed', parent=om.getOrCreateContainer('IRIS region seeds'))
self.seedObj.actor.SetUserTransform(self.transform)
self.frameObj = vis.showFrame(self.transform, 'region seed frame',
scale=0.2,
visible=False,
parent=self.seedObj)
self.frameObj.setProperty('Edit', True)
self.frameObj.widget.HandleRotationEnabledOff()
terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
if terrain:
rep = self.frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
pos = np.array(self.frameObj.transform.GetPosition())
polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
if polyData.GetNumberOfPoints():
polyData = segmentation.labelDistanceToLine(polyData, pos, pos+[0,0,1])
polyData = segmentation.thresholdPoints(polyData, 'distance_to_line', [0.0, 0.1])
if polyData.GetNumberOfPoints():
pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, 'Points')[:,2])
self.frameObj.transform.Translate(pos - np.array(self.frameObj.transform.GetPosition()))
self.placer = PlacerWidget(view, self.seedObj, terrain)
self.placer.start()
else:
self.frameObj.setProperty('Edit', True)
self.frameObj.setProperty('Visible', True)
self.driver = irisDriver
self.safe_region = None
self.addProperty('Visible', True)
self.addProperty('Enabled for Walking', True)
self.addProperty('Alpha', 1.0)
self.addProperty('Color', QtGui.QColor(200,200,20))
self.frameObj.connectFrameModified(self.onFrameModified)
if existing_region is None:
self.onFrameModified(self.frameObj)
else:
self.setRegion(existing_region)
self.setProperty('Alpha', 0.5)
self.setProperty('Color', QtGui.QColor(220,220,220))
def buildCircleWorld(percentObsDensity, nonRandom=False, circleRadius=3, scale=None, randomSeed=5,
obstaclesInnerFraction=1.0, alpha=0.0):
#print "building circle world"
list_of_circles = []
if nonRandom:
np.random.seed(randomSeed)
d = DebugData()
worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d, scale=scale, boundaryType="Square", alpha=alpha)
#print "boundaries done"
worldArea = (worldXmax-worldXmin)*(worldYmax-worldYmin)
#print worldArea
obsScalingFactor = 1.0/12.0
maxNumObstacles = obsScalingFactor * worldArea
numObstacles = int(obstaclesInnerFraction**2 * percentObsDensity/100.0 * maxNumObstacles)
#print numObstacles
# draw random stick obstacles
obsLength = 2.0
obsXmin = worldXmin + (1-obstaclesInnerFraction)/2.0*(worldXmax - worldXmin)
obsXmax = worldXmax - (1-obstaclesInnerFraction)/2.0*(worldXmax - worldXmin)
obsYmin = worldYmin + (1-obstaclesInnerFraction)/2.0*(worldYmax - worldYmin)
obsYmax = worldYmax - (1-obstaclesInnerFraction)/2.0*(worldYmax - worldYmin)
for i in xrange(numObstacles):
firstX = obsXmin + np.random.rand()*(obsXmax-obsXmin)
firstY = obsYmin + np.random.rand()*(obsYmax-obsYmin)
list_of_circles.append( (firstX, firstY) )
firstEndpt = (firstX,firstY,+0.2)
secondEndpt = (firstX,firstY,-0.2)
#d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn())
d.addLine(firstEndpt, secondEndpt, radius=circleRadius)
obj = vis.showPolyData(d.getPolyData(), 'world', alpha=alpha)
world = World()
world.visObj = obj
world.Xmax = worldXmax
world.Xmin = worldXmin
world.Ymax = worldYmax
world.Ymin = worldYmin
world.numObstacles = numObstacles
world.percentObsDensity = percentObsDensity
world.list_of_circles = list_of_circles
return world
def loadReconstructedPointCloud(self, filename=None):
if filename is None:
sourceDir = spartanUtils.getSpartanSourceDir()
filename = os.path.join(sourceDir, 'logs', 'test',
'reconstructed_pointcloud.vtp')
polyData = ioUtils.readPolyData(filename)
self.pointcloud = vis.showPolyData(polyData,
'reconstructed pointcloud',
colorByName='RGB')
vis.addChildFrame(self.pointcloud)
def loadElasticFustionReconstruction(filename):
"""
Loads reconstructed pointcloud into director view
:param filename:
:return:
"""
polyData = ioUtils.readPolyData(filename)
polyData = filterUtils.transformPolyData(polyData,
getDefaultCameraToWorld())
obj = vis.showPolyData(polyData, 'reconstruction', colorByName='RGB')
return obj
def snapshotGeometry(self):
if not self.pickIsValid():
return
p = np.array([float(x) for x in self.ui.pickPt.text.split(', ')])
self.pickPoints.append(p)
polyData = self.makeSphere(p)
folder = self.getRootFolder()
i = len(folder.children())
obj = vis.showPolyData(polyData, 'point %d' % i, color=[1,0,0], parent=folder)
obj.actor.SetPickable(False)
def __init__(self):
pose = transformUtils.poseFromTransform(vtk.vtkTransform())
self.pointcloud = ioUtils.readPolyData(director.getDRCBaseDir() + '/software/models/rehearsal_pointcloud.vtp')
self.pointcloudPD = vis.showPolyData(self.pointcloud, 'coursemodel', parent=None)
segmentation.makeMovable(self.pointcloudPD, transformUtils.transformFromPose(array([0, 0, 0]), array([ 1.0, 0. , 0. , 0.0])))
self.originFrame = self.pointcloudPD.getChildFrame()
t = transformUtils.transformFromPose(array([-4.39364111, -0.51507392, -0.73125563]), array([ 0.93821625, 0. , 0. , -0.34604951]))
self.valveWalkFrame = vis.updateFrame(t, 'ValveWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-3.31840048, 0.36408685, -0.67413123]), array([ 0.93449475, 0. , 0. , -0.35597691]))
self.drillPreWalkFrame = vis.updateFrame(t, 'DrillPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.24553758, -0.52990939, -0.73255338]), array([ 0.93697004, 0. , 0. , -0.34940972]))
self.drillWalkFrame = vis.updateFrame(t, 'DrillWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.51306835, -0.92994004, -0.74173541 ]), array([-0.40456572, 0. , 0. , 0.91450893]))
self.drillWallWalkFarthestSafeFrame = vis.updateFrame(t, 'DrillWallWalkFarthestSafe', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.5314524 , -0.27401861, -0.71302976]), array([ 0.98691519, 0. , 0. , -0.16124022]))
self.drillWallWalkBackFrame = vis.updateFrame(t, 'DrillWallWalkBack', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-1.16122318, 0.04723203, -0.67493468]), array([ 0.93163145, 0. , 0. , -0.36340451]))
self.surprisePreWalkFrame = vis.updateFrame(t, 'SurprisePreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-0.5176186 , -1.00151554, -0.70650799]), array([ 0.84226497, 0. , 0. , -0.53906374]))
self.surpriseWalkFrame = vis.updateFrame(t, 'SurpriseWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-0.69100097, -0.43713269, -0.68495922]), array([ 0.98625075, 0. , 0. , -0.16525575]))
self.surpriseWalkBackFrame = vis.updateFrame(t, 'SurpriseWalkBack', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([ 0.65827322, -0.08028796, -0.77370834]), array([ 0.94399977, 0. , 0. , -0.3299461 ]))
self.terrainPreWalkFrame = vis.updateFrame(t, 'TerrainPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([ 5.47126425, -0.09790393, -0.70504679]), array([ 1., 0., 0., 0.]))
self.stairsPreWalkFrame = vis.updateFrame(t, 'StairsPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
self.frameSync = vis.FrameSync()
self.frameSync.addFrame(self.originFrame)
self.frameSync.addFrame(self.pointcloudPD.getChildFrame(), ignoreIncoming=True)
self.frameSync.addFrame(self.valveWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillPreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWallWalkFarthestSafeFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWallWalkBackFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surprisePreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surpriseWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surpriseWalkBackFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.terrainPreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.stairsPreWalkFrame, ignoreIncoming=True)
def run(self):
folder = om.getOrCreateContainer('affordances')
frame = self.computeAffordanceFrame()
mesh = segmentation.getDrillMesh()
params = segmentation.getDrillAffordanceParams(np.array(frame.GetPosition()), [1,0,0], [0,1,0], [0,0,1])
affordance = vis.showPolyData(mesh, 'drill', color=[0.0, 1.0, 0.0], cls=affordanceitems.FrameAffordanceItem, parent=folder)
frame = vis.showFrame(frame, 'drill frame', parent=affordance, visible=False, scale=0.2)
affordance.actor.SetUserTransform(frame.transform)
affordance.setAffordanceParams(params)
affordance.updateParamsFromActorTransform()
def drawContactVolumes(self, footstepTransform, color):
volFolder = getWalkingVolumesFolder()
for zs, xy in self.contact_slices.iteritems():
points0 = np.vstack((xy, zs[0] + np.zeros((1,xy.shape[1]))))
points1 = np.vstack((xy, zs[1] + np.zeros((1,xy.shape[1]))))
points = np.hstack((points0, points1))
points = points + np.array([[0.05],[0],[-0.0811]])
points = points.T
polyData = vnp.getVtkPolyDataFromNumpyPoints(points.copy())
vol_mesh = filterUtils.computeDelaunay3D(polyData)
obj = vis.showPolyData(vol_mesh, 'walking volume', parent=volFolder, alpha=0.5, visible=self.show_contact_slices, color=color)
obj.actor.SetUserTransform(footstepTransform)
def handle_message(self, msg):
print("frame channel was called")
if set(self._link_name_published) != set(msg.link_name):
# Removes the folder completely.
self.remove_folder()
self._link_name_published = msg.link_name
folder = self._get_folder()
for i in range(0, msg.num_links):
name = msg.link_name[i]
transform = transformUtils.transformFromPose(
msg.position[i], msg.quaternion[i])
# `vis.updateFrame` will either create or update the frame
# according to its name within its parent folder.
vis.updateFrame(transform, name, parent=folder, scale=0.1)
# Create map of body names to a list of contact forces
collision_pair_to_forces = {}
if msg.num_links > 1:
for i in range(1, msg.num_links):
name = msg.link_name[i]
# msg.position[i] is tuple and can be transformed into np array.
point1 = np.array(msg.position[i - 1])
point2 = np.array(msg.position[i])
collision_pair_to_forces[name] = [(point1, point2)]
for key, list_of_forces in iteritems(collision_pair_to_forces):
d = DebugData()
for force_pair in list_of_forces:
d.addArrow(start=force_pair[0],
end=force_pair[1],
tubeRadius=0.005,
headRadius=0.01)
vis.showPolyData(d.getPolyData(),
str(key),
parent=folder,
color=[0, 1, 0])
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 loadCube(subdivisions=30):
d = DebugData()
dim = np.array([0.11,0.11,0.13])
center = np.array([0,0,0])
d.addCube(dim, center, subdivisions=subdivisions)
polyData = d.getPolyData()
# set vertex colors of top face to green
points = vnp.getNumpyFromVtk(polyData, 'Points')
colors = vnp.getNumpyFromVtk(polyData, 'RGB255')
maxZ = points[:,2].max()
inds = points[:,2] > (maxZ - 0.0001)
colors[inds] = [0, 255, 0]
visObj = vis.showPolyData(polyData, 'tissue_box_subdivision', colorByName='RGB255')
print "number of points = ", polyData.GetNumberOfPoints()
sampledPolyData = segmentation.applyVoxelGrid(polyData, leafSize=0.0001)
visObj2 = vis.showPolyData(sampledPolyData, 'voxel grid', color=[0,1,0])
print "voxel number of points ", sampledPolyData.GetNumberOfPoints()
return (visObj, visObj2)
def run(self):
polyData = self.getPointCloud()
annotation = self.getAnnotationInput()
annotationPoint = annotation.annotationPoints[0]
mesh = segmentation.fitShelfItem(polyData, annotationPoint, clusterTolerance=self.properties.getProperty('Cluster tolerance'))
annotation.setProperty('Visible', False)
om.removeFromObjectModel(om.findObjectByName('shelf item'))
obj = vis.showPolyData(mesh, 'shelf item', color=[0,1,0])
t = transformUtils.frameFromPositionAndRPY(segmentation.computeCentroid(mesh), [0,0,0])
segmentation.makeMovable(obj, t)
def __init__(self, robotSystem, cameraView):
self.meshPoints = None
self.imagePoints = None
self.cameraView = cameraView
self.robotMesh = vtk.vtkPolyData()
robotSystem.robotStateModel.model.getModelMesh(self.robotMesh)
self.robotBaseFrame = robotSystem.robotStateModel.getLinkFrame('base')
self.view = PythonQt.dd.ddQVTKWidgetView()
vis.showPolyData(self.robotMesh, 'robot mesh', view=self.view)
self.imageFitter = ImageFitter(self)
vis.showPolyData(self.imageFitter.getPointCloud(),
'pointcloud',
view=self.view,
colorByName='rgb_colors',
visible=False)
self.picker = pointpicker.PointPicker(self.view)
self.picker.pickType = 'cells'
self.picker.numberOfPoints = 3
self.picker.annotationName = 'mesh annotation'
self.picker.annotationFunc = self.onPickPoints
self.picker.start()
self.widget = QtGui.QWidget()
layout = QtGui.QHBoxLayout(self.widget)
layout.addWidget(self.cameraView.view)
layout.addWidget(self.view)
self.widget.resize(800, 400)
self.widget.setWindowTitle('Camera Alignment Tool')
self.widget.show()
self.viewBehaviors = viewbehaviors.ViewBehaviors(self.view)
applogic.resetCamera(viewDirection=[0, 1, 0], view=self.view)
applogic.setCameraTerrainModeEnabled(self.view, True)
def loadObjectMeshes(self):
stream = file(self.pathDict['registrationResult'])
registrationResult = yaml.load(stream)
folder = om.getOrCreateContainer('data files')
for objName, data in registrationResult.iteritems():
filename = data['filename']
if len(filename) == 0:
filename = utils.getObjectMeshFilename(self.objectData,
objName)
else:
filename = os.path.join(utils.getLabelFusionDataDir(),
filename)
polyData = ioUtils.readPolyData(filename)
color = self.getColorFromIndex(objName)
obj = vis.showPolyData(polyData,
name=objName,
parent=folder,
color=color)
self.storedColors[objName] = color
objToWorld = transformUtils.transformFromPose(*data['pose'])
self.objectToWorld[objName] = objToWorld
obj.actor.SetUserTransform(objToWorld)
outlineFilter = vtk.vtkOutlineFilter()
outlineFilter.SetInput(polyData)
outlineFilter.Update()
bbox = vis.showPolyData(outlineFilter.GetOutput(),
objName + ' bbox',
parent=obj)
bbox.actor.GetProperty().SetLineWidth(1)
bbox.actor.SetUserTransform(objToWorld)
bbox.setProperty('Color', [1, 1, 1])
bbox.setProperty('Alpha', 0.8)
bbox.setProperty('Visible', False)
def drawTargetPath(self):
path = DebugData()
for i in range(1, len(self.targetPath)):
p0 = self.targetPath[i - 1].GetPosition()
p1 = self.targetPath[i].GetPosition()
path.addLine(np.array(p0), np.array(p1), radius=0.005)
pathMesh = path.getPolyData()
self.targetPathMesh = vis.showPolyData(pathMesh,
'target frame desired path',
color=[0.0, 0.3, 1.0],
parent=self.targetAffordance,
alpha=0.6)
self.targetPathMesh.actor.SetUserTransform(self.targetFrame)
def openGeometry(self, filename):
if filename.lower().endswith('wrl'):
self.onOpenVrml(filename)
return
polyData = ioUtils.readPolyData(filename)
if not polyData or not polyData.GetNumberOfPoints():
self.app.showErrorMessage('Failed to read any data from file: %s' % filename, title='Reader error')
return
obj = vis.showPolyData(polyData, os.path.basename(filename), parent=self.getRootFolder())
vis.addChildFrame(obj)
def showFusedMaps(self):
om.removeFromObjectModel(om.findObjectByName('stereo'))
om.getOrCreateContainer('stereo')
q = imageManager.queue
cameraToLocalNow = vtk.vtkTransform()
utime = q.getCurrentImageTime('CAMERA_TSDF')
q.getTransform('MULTISENSE_CAMERA_LEFT', 'local', utime,
cameraToLocalNow)
cameraToLocalFusedNow = vtk.vtkTransform()
q.getTransform('MULTISENSE_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')
