def spawnHandAtCurrentLocation(side='left'):
if (side is 'left'):
tf = transformUtils.copyFrame( getLinkFrame( 'l_hand_face') )
handFactory.placeHandModelWithTransform( tf , app.getCurrentView(), 'left')
else:
tf = transformUtils.copyFrame( getLinkFrame( 'right_pointer_tip') )
handFactory.placeHandModelWithTransform( tf , app.getCurrentView(), 'right')
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()
v.camera().SetPosition([3,3,3])
v.camera().SetFocalPoint([0,0,0])
view.show()
app.showObjectModel()
app.start()
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()
v.camera().SetPosition([3, 3, 3])
v.camera().SetFocalPoint([0, 0, 0])
view.show()
app.showObjectModel()
app.start()
