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 onOpenGeometry(filename):
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 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 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 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 buildRobot():
d = DebugData()
d.addCone((0,0,0), (1,0,0), height=0.2, radius=0.1)
obj = vis.showPolyData(d.getPolyData(), 'robot')
frame = vis.addChildFrame(obj)
return 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 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 onSegmentTable(self, p1, p2):
print p1
print p2
self.picker.stop()
om.removeFromObjectModel(self.picker.annotationObj)
self.picker = None
om.removeFromObjectModel(om.findObjectByName('table demo'))
self.tableData = segmentation.segmentTableEdge(self.getInputPointCloud(), p1, p2)
self.tableObj = vis.showPolyData(self.tableData.mesh, 'table', parent='table demo', color=[0,1,0])
self.tableFrame = vis.showFrame(self.tableData.frame, 'table frame', parent=self.tableObj, scale=0.2)
self.tableBox = vis.showPolyData(self.tableData.box, 'table box', parent=self.tableObj, color=[0,1,0], visible=False)
self.tableObj.actor.SetUserTransform(self.tableFrame.transform)
self.tableBox.actor.SetUserTransform(self.tableFrame.transform)
if self.useCollisionEnvironment:
self.addCollisionObject(self.tableObj)
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 onShowMapButton(self):
# reloads the map each time - in case its changed on disk:
#if (self.octomap_cloud is None):
filename = ddapp.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 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 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 drawPolytope(msg):
_id = msg.id
if msg.remove:
om.removeFromObjectModel(om.findObjectByName("IRIS polytopes"))
return
if msg.highlighted:
color = [0.8, 0.2, 0.2]
else:
color = [0.8, 0.8, 0.8]
name = "polytope {:d}".format(_id)
obj = om.findObjectByName(name)
if obj:
om.removeFromObjectModel(obj)
V = np.array(msg.V)
polyData = vnp.numpyToPolyData(V.T.copy())
vol_mesh = filterUtils.computeDelaunay3D(polyData)
debug = DebugData()
debug.addPolyData(vol_mesh)
vis.showPolyData(debug.getPolyData(), name, color=color, alpha=0.4, parent="IRIS polytopes")
def drawContactPts(self, obj, footstep, **kwargs):
if footstep.is_right_foot:
_, contact_pts = FootstepsDriver.getContactPts()
else:
contact_pts, _ = FootstepsDriver.getContactPts()
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 __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 __init__(self):
pose = transformUtils.poseFromTransform(vtk.vtkTransform())
self.pointcloud = ioUtils.readPolyData(ddapp.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 prepTestDemoSequence(self):
'''
Running this function should launch a full planning sequence
to pick to objects, walk and drop.
Requires footstep footstepPlanner
'''
filename = os.path.expanduser('~/drc-testing-data/tabletop/table-and-bin-scene.vtp')
scene = ioUtils.readPolyData(filename)
vis.showPolyData(scene,"scene")
#stanceFrame = transformUtils.frameFromPositionAndRPY([0, 0, 0], [0, 0, 123.0])
#self.teleportRobotToStanceFrame(stanceFrame)
self.userFitTable()
self.onSegmentTable( np.array([-1.72105646, 2.73210716, 0.79449952]), np.array([-1.67336452, 2.63351011, 0.78698605]) )
self.userFitBin()
self.onSegmentBin( np.array([-0.02, 2.43, 0.61 ]), np.array([-0.40, 2.79, 0.61964661]) )
self.computeTableStanceFrame()
self.computeBinStanceFrame()
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 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 buildWarehouseWorld(percentObsDensity, nonRandom=False, circleRadius=0.1, scale=None, randomSeed=5,
obstaclesInnerFraction=1.0):
if nonRandom:
np.random.seed(randomSeed)
d = DebugData()
worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d, scale=scale, boundaryType="Warehouse")
numObstacles = 8
obsLength = 2.0
worldLength = worldXmax - worldXmin
print worldXmin
print worldXmax
obstacleZone = [worldXmin + 0.2 * worldLength, worldXmax - 0.2 * worldLength ]
print obstacleZone
obstacleLength = obstacleZone[1] - obstacleZone[0]
incrementSize = obstacleLength * 1.0 / numObstacles
print incrementSize
leftOrRight = -1.0
for i in xrange(numObstacles):
firstX = obstacleZone[0] + incrementSize * i
leftOrRight = leftOrRight * -1.0
firstEndpt = (firstX, leftOrRight * worldYmax,0.0)
secondEndpt = (firstX, 0.0, 0.0)
#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 debugDrawFootPoints(model):
pts_left, pts_right = FootstepsDriver.getContactPts()
d = DebugData()
for linkName in [_leftFootLink, _rightFootLink]:
t = model.getLinkFrame(linkName)
d.addFrame(t, scale=0.2)
if (linkName is _leftFootLink):
pts = pts_left
else:
pts = pts_right
footMidPoint = np.mean(pts, axis=0)
for p in pts.tolist() + [footMidPoint.tolist()]:
t.TransformPoint(p, p)
d.addSphere(p, radius=0.015)
midpt = FootstepsDriver.getFeetMidPoint(model)
d.addFrame(midpt, scale=0.2)
vis.showPolyData(d.getPolyData(), 'foot points debug', parent='debug', colorByName='RGB255')
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 drawFittedSteps(self, footsteps):
''' Draw the footsteps fitted to the blocks
These are NOT the steps placed by the planner
'''
left_color=None
right_color=None
for i, footstep in enumerate(footsteps):
mesh,color = self.getMeshAndColor(footstep.is_right_foot)
vis.updateFrame(footstep.transform, 'foot placement %d' % i , parent='foot placements', scale=0.2, visible=False)
obj = vis.showPolyData(mesh, 'step %d' % i, color=color, alpha=1.0, parent='steps')
#frameObj = vis.showFrame(footstepTransform, stepName + ' frame', parent=obj, scale=0.3, visible=False)
obj.actor.SetUserTransform(footstep.transform)
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 drawFittedSteps(self, footsteps):
''' Draw the footsteps fitted to the blocks
These are NOT the steps placed by the planner
'''
left_color=None
right_color=None
for i, footstep in enumerate(footsteps):
mesh,color = self.getMeshAndColor(footstep.is_right_foot)
vis.updateFrame(footstep.transform, 'foot placement %d' % i , parent='foot placements', scale=0.2, visible=False)
obj = vis.showPolyData(mesh, 'step %d' % i, color=color, alpha=1.0, parent='steps')
#frameObj = vis.showFrame(footstepTransform, stepName + ' frame', parent=obj, scale=0.3, visible=False)
obj.actor.SetUserTransform(footstep.transform)
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 drawObjectInCamera(self, objectName, visible=True):
imageView = self.cameraView.views[self.imageViewName]
v = imageView.view
q = self.cameraView.imageManager.queue
localToCameraT = vtk.vtkTransform()
q.getTransform('local', self.imageViewName, 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(self.imageViewName, pd)
vis.showPolyData(pd, ('overlay ' + objectName),
view=v,
color=[0, 1, 0],
parent='camera overlay',
visible=visible)
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 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 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 buildRobot(x=0, y=0):
#print "building robot"
polyData = ioUtils.readPolyData('crazyflie.obj')
scale = 0.01
t = vtk.vtkTransform()
t.RotateX(90)
t.Scale(scale, scale, scale)
polyData = filterUtils.transformPolyData(polyData, t)
#d = DebugData()
#d.addCone((x,y,0), (1,0,0), height=0.2, radius=0.1)
#polyData = d.getPolyData()
obj = vis.showPolyData(polyData, 'robot')
robotFrame = vis.addChildFrame(obj)
return obj, robotFrame
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):
#print "building robot"
polyData = ioUtils.readPolyData('celica.obj')
scale = 0.04
t = vtk.vtkTransform()
t.RotateZ(90)
t.Scale(scale, scale, scale)
polyData = filterUtils.transformPolyData(polyData, t)
#d = DebugData()
#d.addCone((x,y,0), (1,0,0), height=0.2, radius=0.1)
#polyData = d.getPolyData()
obj = vis.showPolyData(polyData, 'robot')
robotFrame = vis.addChildFrame(obj)
return obj, robotFrame
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 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 spawnTargetAffordance(self):
for obj in om.getObjects():
if obj.getProperty('Name') == 'target':
om.removeFromObjectModel(obj)
targetFrame = transformUtils.frameFromPositionAndRPY([0.6, 0.2, 0.6],
[180, 0, 90])
folder = om.getOrCreateContainer('affordances')
z = DebugData()
z.addLine(np.array([0, 0, 0]),
np.array([-self.boxLength, 0, 0]),
radius=0.02) # main bar
z.addLine(np.array([-self.boxLength, 0, 0]),
np.array([-self.boxLength, 0, self.boxLength]),
radius=0.02) # main bar
z.addLine(np.array([-self.boxLength, 0, self.boxLength]),
np.array([0, 0, self.boxLength]),
radius=0.02) # main bar
z.addLine(np.array([0, 0, self.boxLength]),
np.array([0, 0, 0]),
radius=0.02) # main bar
targetMesh = z.getPolyData()
self.targetAffordance = vis.showPolyData(
targetMesh,
'target',
color=[0.0, 1.0, 0.0],
cls=affordanceitems.FrameAffordanceItem,
parent=folder,
alpha=0.3)
self.targetAffordance.actor.SetUserTransform(targetFrame)
self.targetFrame = vis.showFrame(targetFrame,
'target frame',
parent=self.targetAffordance,
visible=False,
scale=0.2)
self.targetFrame = self.targetFrame.transform
params = dict(length=self.boxLength,
otdf_type='target',
friendly_name='target')
self.targetAffordance.setAffordanceParams(params)
self.targetAffordance.updateParamsFromActorTransform()
def createTextureGround(imageFilename, view):
pd = createTexturedPlane()
texture = createTexture(imageFilename)
texture.RepeatOn()
tcoords = vnp.getNumpyFromVtk(pd, 'Texture Coordinates')
tcoords *= 60
t = vtk.vtkTransform()
t.PostMultiply()
t.Scale(200, 200, 200)
t.Translate(0, 0, -0.005)
pd = filterUtils.transformPolyData(pd, t)
obj = vis.showPolyData(pd, 'ground', view=view, alpha=1.0, parent='skybox')
obj.actor.SetTexture(texture)
obj.actor.GetProperty().LightingOff()
def newPolyData(self, name, view, parent=None):
self.handModel.model.setJointPositions(np.zeros(self.handModel.model.numberOfJoints()))
polyData = vtk.vtkPolyData()
self.handModel.model.getModelMesh(polyData)
polyData = filterUtils.transformPolyData(polyData, self.modelToPalm)
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 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 buildStickWorld(percentObsDensity):
print "building stick world"
d = DebugData()
worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d)
print "boundaries done"
worldArea = (worldXmax - worldXmin) * (worldYmax - worldYmin)
print worldArea
obsScalingFactor = 1.0 / 12.0
maxNumObstacles = obsScalingFactor * worldArea
numObstacles = int(percentObsDensity / 100.0 * maxNumObstacles)
print numObstacles
# draw random stick obstacles
obsLength = 2.0
for i in xrange(numObstacles):
firstX = worldXmin + np.random.rand() * (worldXmax - worldXmin)
firstY = worldYmin + np.random.rand() * (worldYmax - worldYmin)
firstEndpt = (firstX, firstY, 0)
randTheta = np.random.rand() * 2.0 * np.pi
secondEndpt = (firstX + obsLength * np.cos(randTheta),
firstY + obsLength * np.sin(randTheta), 0)
d.addLine(firstEndpt, secondEndpt, radius=0.2)
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 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 buildHallwayWorld():
print "building hallway world"
lineRadius = 0.2
d = DebugData()
xMin = -10
xMax = 10
yMin = 0
yMax = 50
for x in [xMin, xMax]:
lineStart = (x, yMin, 0)
lineEnd = (x, yMax, 0)
d.addLine(lineStart, lineEnd, radius=lineRadius)
obj = vis.showPolyData(d.getPolyData(), 'world')
world = World()
world.visObj = obj
return world
def startSwarmVisualization():
global timerCallback, nav_data, nav_cloud
nav_cloud = vtknp.getVtkPolyDataFromNumpyPoints(nav_data)
nav_cloud_obj = vis.showPolyData(shallowCopy(nav_cloud), 'nav data')
nav_cloud_obj.initialized = False
def updateSwarm():
global nav_cloud
if not nav_cloud_obj.initialized:
nav_cloud_obj.mapper.SetColorModeToMapScalars()
nav_cloud_obj.initialized = True
#print nav_data.shape[0], nav_cloud.GetNumberOfPoints()
nav_cloud = vtknp.getVtkPolyDataFromNumpyPoints(nav_data)
nav_cloud_obj.setPolyData(shallowCopy(nav_cloud))
#print nav_cloud_obj.polyData.GetNumberOfPoints()
timerCallback = TimerCallback(targetFps=30)
timerCallback.callback = updateSwarm
timerCallback.start()
def convertStepToSafeRegion(self, step, rpySeed):
assert step.shape[0] >= 3
assert step.shape[1] == 3
shapeVertices = np.array(step).transpose()[:2,:]
s = ddapp.terrain.PolygonSegmentationNonIRIS(shapeVertices, bot_pts=self.footContactPoints)
stepCenter = np.mean(step, axis=0)
startSeed = np.hstack([stepCenter, rpySeed])
r = s.findSafeRegion(startSeed)
if r is not None:
# draw step
d = DebugData()
for p1, p2 in zip(step, step[1:]):
d.addLine(p1, p2)
d.addLine(step[-1], step[0])
folder = om.getOrCreateContainer('Safe terrain regions')
obj = vis.showPolyData(d.getPolyData(), 'step region %d' % len(folder.children()), parent=folder)
obj.properties.addProperty('Enabled for Walking', True)
obj.safe_region = r
view = app.createView()
segmentation._defaultSegmentationView = view
robotStateModel, robotStateJointController = roboturdf.loadRobotModel(
'robot state model',
view,
parent='sensors',
color=roboturdf.getRobotGrayColor(),
visible=True)
segmentationroutines.SegmentationContext.initWithRobot(robotStateModel)
# load poly data
dataDir = app.getTestingDataDirectory()
polyData = ioUtils.readPolyData(
os.path.join(dataDir, 'amazon-pod/01-small-changes.vtp'))
vis.showPolyData(polyData, 'pointcloud snapshot', visible=False)
# remove ground and clip to just the pod:
groundPoints, polyData = segmentation.removeGround(polyData)
vis.showPolyData(polyData, 'scene', visible=False)
polyData = segmentation.addCoordArraysToPolyData(polyData)
polyData = segmentation.thresholdPoints(polyData, 'y', [1, 1.6])
polyData = segmentation.thresholdPoints(polyData, 'x', [-1.2, 0.5])
vis.showPolyData(polyData, 'clipped', visible=False)
# remove outliers
polyData = segmentation.labelOutliers(polyData,
searchRadius=0.03,
neighborsInSearchRadius=40)
polyData = segmentation.thresholdPoints(polyData, 'is_outlier', [0, 0])
vis.showPolyData(polyData, 'inliers', visible=False)
def drawFootstepPlan(self, msg, folder, left_color=None, right_color=None, alpha=1.0):
for step in folder.children():
om.removeFromObjectModel(step)
allTransforms = []
volFolder = getWalkingVolumesFolder()
map(om.removeFromObjectModel, volFolder.children())
slicesFolder = getTerrainSlicesFolder()
map(om.removeFromObjectModel, slicesFolder.children())
for i, footstep in enumerate(msg.footsteps):
trans = footstep.pos.translation
trans = [trans.x, trans.y, trans.z]
quat = footstep.pos.rotation
quat = [quat.w, quat.x, quat.y, quat.z]
footstepTransform = transformUtils.transformFromPose(trans, quat)
allTransforms.append(footstepTransform)
if i < 2:
continue
if footstep.is_right_foot:
mesh = getRightFootMesh()
if (right_color is None):
color = getRightFootColor()
else:
color = right_color
else:
mesh = getLeftFootMesh()
if (left_color is None):
color = getLeftFootColor()
else:
color = left_color
# add gradual shading to steps to indicate destination
frac = float(i)/ float(msg.num_steps-1)
this_color = [0,0,0]
this_color[0] = 0.25*color[0] + 0.75*frac*color[0]
this_color[1] = 0.25*color[1] + 0.75*frac*color[1]
this_color[2] = 0.25*color[2] + 0.75*frac*color[2]
if self.show_contact_slices:
self.drawContactVolumes(footstepTransform, color)
contact_pts_left, contact_pts_right = FootstepsDriver.getContactPts()
if footstep.is_right_foot:
sole_offset = np.mean(contact_pts_right, axis=0)
else:
sole_offset = np.mean(contact_pts_left, axis=0)
t_sole_prev = transformUtils.frameFromPositionMessage(msg.footsteps[i-2].pos)
t_sole_prev.PreMultiply()
t_sole_prev.Translate(sole_offset)
t_sole = transformUtils.copyFrame(footstepTransform)
t_sole.Translate(sole_offset)
yaw = np.arctan2(t_sole.GetPosition()[1] - t_sole_prev.GetPosition()[1],
t_sole.GetPosition()[0] - t_sole_prev.GetPosition()[0])
T_terrain_to_world = transformUtils.frameFromPositionAndRPY([t_sole_prev.GetPosition()[0], t_sole_prev.GetPosition()[1], 0],
[0, 0, math.degrees(yaw)])
path_dist = np.array(footstep.terrain_path_dist)
height = np.array(footstep.terrain_height)
# if np.any(height >= trans[2]):
terrain_pts_in_local = np.vstack((path_dist, np.zeros(len(footstep.terrain_path_dist)), height))
d = DebugData()
for j in range(terrain_pts_in_local.shape[1]-1):
d.addLine(terrain_pts_in_local[:,j], terrain_pts_in_local[:,j+1], radius=0.01)
obj = vis.showPolyData(d.getPolyData(), 'terrain slice', parent=slicesFolder, visible=slicesFolder.getProperty('Visible'), color=[.8,.8,.3])
obj.actor.SetUserTransform(T_terrain_to_world)
renderInfeasibility = False
if renderInfeasibility and footstep.infeasibility > 1e-6:
d = DebugData()
start = allTransforms[i-1].GetPosition()
end = footstepTransform.GetPosition()
d.addArrow(start, end, 0.02, 0.005,
startHead=True,
endHead=True)
vis.showPolyData(d.getPolyData(), 'infeasibility %d -> %d' % (i-2, i-1), parent=folder, color=[1, 0.2, 0.2])
stepName = 'step %d' % (i-1)
obj = vis.showPolyData(mesh, stepName, color=this_color, alpha=alpha, parent=folder)
obj.setIcon(om.Icons.Feet)
frameObj = vis.showFrame(footstepTransform, stepName + ' frame', parent=obj, scale=0.3, visible=False)
obj.actor.SetUserTransform(footstepTransform)
obj.addProperty('Support Contact Groups', footstep.params.support_contact_groups, attributes=om.PropertyAttributes(enumNames=['Whole Foot', 'Front 2/3', 'Back 2/3']))
obj.properties.setPropertyIndex('Support Contact Groups', 0)
obj.footstep_index = i
obj.footstep_property_callback = obj.properties.connectPropertyChanged(functools.partial(self.onFootstepPropertyChanged, obj))
self.drawContactPts(obj, footstep, color=this_color)
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 loadTableTopPointCloud():
dataDir = app.getTestingDataDirectory()
polyData = ioUtils.readPolyData(
os.path.join(dataDir, 'tabletop/table-and-door-scene.vtp'))
return vis.showPolyData(polyData, 'pointcloud snapshot')
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 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 buildSimpleWorld():
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 prepKukaTestDemoSequence(self, inputFile='~/drc-testing-data/tabletop/kinect_collision_environment.vtp'):
filename = os.path.expanduser(inputFile)
scene = ioUtils.readPolyData(filename)
vis.showPolyData(scene,"scene")
self.prepKukaLabScene()
def prepIhmcDemoSequenceFromFile(self):
filename = os.path.expanduser('~/drc-testing-data/ihmc_table/ihmc_table.vtp')
polyData = ioUtils.readPolyData( filename )
vis.showPolyData( polyData,'scene')
self.prepIhmcDemoSequence()
robotStateModel, robotStateJointController = roboturdf.loadRobotModel('robot state model', view, parent='sensors', color=roboturdf.getRobotGrayColor(), visible=True)
segmentationroutines.SegmentationContext.initWithRobot(robotStateModel)
# Move robot to near to valve wall:
# 0degrees
#robotStateJointController.q[5] = math.radians(120)
#robotStateJointController.q[0] = 0
#robotStateJointController.q[1] = 0
# 30,60,90
robotStateJointController.q [5] = math.radians(-120)
robotStateJointController.q [0] = 1
robotStateJointController.q [1] = 1
robotStateJointController.q[2] = 0.85
robotStateJointController.push()
# load poly data
dataDir = app.getTestingDataDirectory()
#polyData = ioUtils.readPolyData(os.path.join(dataDir, 'valve/valve-lever-scene.vtp'))
#polyData = ioUtils.readPolyData(os.path.join(dataDir, 'valve/valve-lever-scene-30.vtp'))
#polyData = ioUtils.readPolyData(os.path.join(dataDir, 'valve/valve-lever-scene-60.vtp'))
polyData = ioUtils.readPolyData(os.path.join(dataDir, 'valve/valve-lever-scene-90.vtp'))
vis.showPolyData(polyData, 'pointcloud snapshot')
segmentation.segmentValveWallAuto(.2, mode='both', removeGroundMethod=segmentation.removeGround )
if app.getTestingInteractiveEnabled():
view.show()
app.showObjectModel()
app.start()
robotStateJointController.q[33] = 0.5 # head down
robotStateJointController.push()
'''
groundHeight = 0.0
viewFrame = segmentation.transformUtils.frameFromPositionAndRPY(
[1, -1, groundHeight + 1.5], [0, 0, -35])
segmentationroutines.SegmentationContext.initWithUser(groundHeight, viewFrame)
# load poly data
dataDir = app.getTestingDataDirectory()
polyData = ioUtils.readPolyData(
os.path.join(dataDir, 'tabletop/table-sparse-stereo.vtp'))
vis.showPolyData(polyData,
'pointcloud snapshot original',
colorByName='rgb_colors')
polyData = segmentationroutines.sparsifyStereoCloud(polyData)
vis.showPolyData(polyData, 'pointcloud snapshot')
# Use only returns near the robot:
polyData = segmentation.addCoordArraysToPolyData(polyData)
polyData = segmentation.thresholdPoints(polyData, 'distance_along_view_x',
[0, 1.3])
segmentation.segmentTableThenFindDrills(polyData,
[1.2864902, -0.93351376, 1.10208917])
if app.getTestingInteractiveEnabled():
v = applogic.getCurrentView()
def onArrowGlyph():
result = segmentation.applyArrowGlyphs(pointCloudObj.polyData)
obj = vis.showPolyData(result, 'disks')
elif msg.plan_type == lcmdrc.plan_status_t.BRACING:
for link in links:
robotHighlighter.highlightLink(link, [1, 0, 0])
else:
for link in links:
robotHighlighter.dehighlightLink(link)
fallDetectorSub = lcmUtils.addSubscriber("PLAN_EXECUTION_STATUS", lcmdrc.plan_status_t, onPlanStatus)
fallDetectorSub.setSpeedLimit(10)
if useDataFiles:
for filename in drcargs.args().data_files:
polyData = io.readPolyData(filename)
if polyData:
vis.showPolyData(polyData, os.path.basename(filename))
if useImageWidget:
imageWidget = cameraview.ImageWidget(cameraview.imageManager, 'CAMERA_LEFT', view)
#imageWidget = cameraview.ImageWidget(cameraview.imageManager, 'KINECT_RGB', view)
if useCameraFrustumVisualizer and cameraview.CameraFrustumVisualizer.isCompatibleWithConfig():
cameraFrustumVisualizer = cameraview.CameraFrustumVisualizer(robotStateModel, cameraview.imageManager, 'CAMERA_LEFT')
class ImageOverlayManager(object):
def __init__(self):
self.viewName = 'CAMERA_LEFT'
#self.viewName = 'KINECT_RGB'
self.size = 400
from ddapp.consoleapp import ConsoleApp
from ddapp.debugVis import DebugData
import ddapp.visualization as vis
# initialize application components
app = ConsoleApp()
view = app.createView()
view.showMaximized()
# create a sphere primitive
d = DebugData()
d.addSphere(center=(0, 0, 0), radius=0.5)
# show the sphere in the visualization window
sphere = vis.showPolyData(d.getPolyData(), 'sphere')
sphere.setProperty('Color', [0, 1, 0])
# start the application
app.start()
