def createPolyDataFromPrimitive(geom):
if geom.type == lcmdrake.lcmt_viewer_geometry_data.BOX:
d = DebugData()
d.addCube(dimensions=geom.float_data[0:3], center=[0,0,0])
return d.getPolyData()
elif geom.type == lcmdrake.lcmt_viewer_geometry_data.SPHERE:
d = DebugData()
d.addSphere(center=(0,0,0), radius=geom.float_data[0])
return d.getPolyData()
elif geom.type == lcmdrake.lcmt_viewer_geometry_data.CYLINDER:
d = DebugData()
d.addCylinder(center=(0,0,0), axis=(0,0,1), radius=geom.float_data[0], length=geom.float_data[1])
return d.getPolyData()
elif geom.type == lcmdrake.lcmt_viewer_geometry_data.CAPSULE:
d = DebugData()
radius = geom.float_data[0]
length = geom.float_data[1]
d.addCylinder(center=(0,0,0), axis=(0,0,1), radius=radius, length=length)
d.addSphere(center=(0,0,length/2.0), radius=radius)
d.addSphere(center=(0,0,-length/2.0), radius=radius)
return d.getPolyData()
raise Exception('Unsupported geometry type: %s' % geom.type)
def extractBlocksFromSurfaces(self, clusters, linkFrame):
''' find the corners of the minimum bounding rectangles '''
om.removeFromObjectModel(om.findObjectByName('block corners'))
om.removeFromObjectModel(om.findObjectByName('foot placements'))
om.removeFromObjectModel(om.findObjectByName('steps'))
om.getOrCreateContainer('block corners',om.getOrCreateContainer('continuous'))
om.getOrCreateContainer('foot placements',om.getOrCreateContainer('continuous'))
om.getOrCreateContainer('steps',om.getOrCreateContainer('continuous'))
print 'got %d clusters' % len(clusters)
# get the rectangles from the clusters:
blocks = []
for i, cluster in enumerate(clusters):
cornerTransform, rectDepth, rectWidth, rectArea = self.findMinimumBoundingRectangle( cluster, linkFrame )
print 'min bounding rect:', rectDepth, rectWidth, rectArea, cornerTransform.GetPosition()
block = BlockTop(cornerTransform, rectDepth, rectWidth, rectArea)
blocks.append(block)
# filter out blocks that are too big or small
# TODO: pull out these parameters
blocksGood = []
groundPlane = None
for i, block in enumerate(blocks):
if ((block.rectWidth>0.45) or (block.rectDepth>0.90)):
print " ground plane",i,block.rectWidth,block.rectDepth
groundPlane = block
elif ((block.rectWidth<0.30) or (block.rectDepth<0.20)): # was 0.34 and 0.30 for 13 block successful walk with lidar
print "removed block",i,block.rectWidth,block.rectDepth
foobar=[]
else:
blocksGood.append(block)
print "keeping block",i,block.rectWidth,block.rectDepth
blocks = blocksGood
# order by distance from robot's foot
for i, block in enumerate(blocks):
block.distToRobot = np.linalg.norm(np.array(linkFrame.GetPosition()) - np.array(block.cornerTransform.GetPosition()))
blocks.sort(key=operator.attrgetter('distToRobot'))
# draw blocks including the ground plane:
om.removeFromObjectModel(om.findObjectByName('blocks'))
blocksFolder=om.getOrCreateContainer('blocks',om.getOrCreateContainer('continuous'))
for i, block in enumerate(blocks):
vis.updateFrame(block.cornerTransform, 'block corners %d' % i , parent='block corners', scale=0.2, visible=True)
blockCenter = transformUtils.frameFromPositionAndRPY([-block.rectDepth/2,block.rectWidth/2,0.0], [0,0,0])
blockCenter.Concatenate(block.cornerTransform)
d = DebugData()
d.addCube([ block.rectDepth, block.rectWidth,0.005],[0,0,0])
obj = vis.showPolyData(d.getPolyData(),'block %d' % i, color=[1,0,1],parent=blocksFolder)
obj.actor.SetUserTransform(blockCenter)
if (groundPlane is not None):
vis.updateFrame(groundPlane.cornerTransform, 'ground plane', parent='block corners', scale=0.2, visible=True)
blockCenter = transformUtils.frameFromPositionAndRPY([-groundPlane.rectDepth/2,groundPlane.rectWidth/2,0.0], [0,0,0])
blockCenter.Concatenate(groundPlane.cornerTransform)
d = DebugData()
d.addCube([ groundPlane.rectDepth, groundPlane.rectWidth,0.005],[0,0,0])
obj = vis.showPolyData(d.getPolyData(),'ground plane', color=[1,1,0],alpha=0.1, parent=blocksFolder)
obj.actor.SetUserTransform(blockCenter)
return blocks,groundPlane
def extractBlocksFromSurfaces(self, clusters, linkFrame):
''' find the corners of the minimum bounding rectangles '''
om.removeFromObjectModel(om.findObjectByName('block corners'))
om.removeFromObjectModel(om.findObjectByName('foot placements'))
om.removeFromObjectModel(om.findObjectByName('steps'))
om.getOrCreateContainer('block corners',om.getOrCreateContainer('continuous'))
om.getOrCreateContainer('foot placements',om.getOrCreateContainer('continuous'))
om.getOrCreateContainer('steps',om.getOrCreateContainer('continuous'))
print 'got %d clusters' % len(clusters)
# get the rectangles from the clusters:
blocks = []
for i, cluster in enumerate(clusters):
cornerTransform, rectDepth, rectWidth, rectArea = self.findMinimumBoundingRectangle( cluster, linkFrame )
print 'min bounding rect:', rectDepth, rectWidth, rectArea, cornerTransform.GetPosition()
block = BlockTop(cornerTransform, rectDepth, rectWidth, rectArea)
blocks.append(block)
# filter out blocks that are too big or small
# TODO: pull out these parameters
blocksGood = []
groundPlane = None
for i, block in enumerate(blocks):
if ((block.rectWidth>0.45) or (block.rectDepth>0.90)):
print " ground plane",i,block.rectWidth,block.rectDepth
groundPlane = block
elif ((block.rectWidth<0.30) or (block.rectDepth<0.20)): # was 0.34 and 0.30 for 13 block successful walk with lidar
print "removed block",i,block.rectWidth,block.rectDepth
foobar=[]
else:
blocksGood.append(block)
print "keeping block",i,block.rectWidth,block.rectDepth
blocks = blocksGood
# order by distance from robot's foot
for i, block in enumerate(blocks):
block.distToRobot = np.linalg.norm(np.array(linkFrame.GetPosition()) - np.array(block.cornerTransform.GetPosition()))
blocks.sort(key=operator.attrgetter('distToRobot'))
# draw blocks including the ground plane:
om.removeFromObjectModel(om.findObjectByName('blocks'))
blocksFolder=om.getOrCreateContainer('blocks',om.getOrCreateContainer('continuous'))
for i, block in enumerate(blocks):
vis.updateFrame(block.cornerTransform, 'block corners %d' % i , parent='block corners', scale=0.2, visible=True)
blockCenter = transformUtils.frameFromPositionAndRPY([-block.rectDepth/2,block.rectWidth/2,0.0], [0,0,0])
blockCenter.Concatenate(block.cornerTransform)
d = DebugData()
d.addCube([ block.rectDepth, block.rectWidth,0.005],[0,0,0])
obj = vis.showPolyData(d.getPolyData(),'block %d' % i, color=[1,0,1],parent=blocksFolder)
obj.actor.SetUserTransform(blockCenter)
if (groundPlane is not None):
vis.updateFrame(groundPlane.cornerTransform, 'ground plane', parent='block corners', scale=0.2, visible=True)
blockCenter = transformUtils.frameFromPositionAndRPY([-groundPlane.rectDepth/2,groundPlane.rectWidth/2,0.0], [0,0,0])
blockCenter.Concatenate(groundPlane.cornerTransform)
d = DebugData()
d.addCube([ groundPlane.rectDepth, groundPlane.rectWidth,0.005],[0,0,0])
obj = vis.showPolyData(d.getPolyData(),'ground plane', color=[1,1,0],alpha=0.1, parent=blocksFolder)
obj.actor.SetUserTransform(blockCenter)
return blocks,groundPlane
def updateGeometryFromProperties(self):
d = DebugData()
d.addCube(self.getProperty('Dimensions'), (0,0,0), subdivisions=self.getProperty('Subdivisions'))
self.setPolyData(d.getPolyData())
def updateGeometryFromProperties(self):
d = DebugData()
d.addCube(self.getProperty('Dimensions'), (0, 0, 0),
subdivisions=self.getProperty('Subdivisions'))
self.setPolyData(d.getPolyData())
