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 createPolyDataFromPrimitive(geom):
if geom.type == lcmrl.viewer_geometry_data_t.BOX:
d = DebugData()
d.addCube(dimensions=geom.float_data[0:3], center=[0, 0, 0])
return d.getPolyData()
elif geom.type == lcmrl.viewer_geometry_data_t.SPHERE:
d = DebugData()
d.addSphere(center=(0, 0, 0), radius=geom.float_data[0])
return d.getPolyData()
elif geom.type == lcmrl.viewer_geometry_data_t.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 == lcmrl.viewer_geometry_data_t.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()
elif hasattr(lcmrl.viewer_geometry_data_t, "ELLIPSOID") and geom.type == lcmrl.viewer_geometry_data_t.ELLIPSOID:
d = DebugData()
radii = geom.float_data[0:3]
d.addEllipsoid(center=(0, 0, 0), radii=radii)
return d.getPolyData()
raise Exception("Unsupported geometry type: %s" % geom.type)
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 __init__(self,
velocity=40.0,
scale=0.5,
exploration=0.5,
size=100,
model="A10.obj"):
"""Constructs a Robot.
Args:
velocity: Velocity of the robot in the forward direction.
scale: Scale of the model.
exploration: Exploration rate.
model: Object model to use.
"""
self._size = size
self._target = (0, 0, 0)
self._initPos = (0, 0)
self._exploration = exploration
self._changetheta = 0
self._inlaser = True
# t = vtk.vtkTransform()
# t.Scale(scale, scale, scale)
# polydata = ioUtils.readPolyData(model)
# polydata = filterUtils.transformPolyData(polydata, t)
data = DebugData()
center = [0, 0, 1.0 / 2 - 0.5]
dimensions = [10, 5, 1.0]
data.addCube(dimensions, center)
polydata = data.getPolyData()
super(Robot, self).__init__(velocity, polydata)
self._ctrl = Controller()
def createPolyDataFromPrimitive(geom):
if geom.type == lcmrl.viewer_geometry_data_t.BOX:
d = DebugData()
d.addCube(dimensions=geom.float_data[0:3], center=[0,0,0])
return d.getPolyData()
elif geom.type == lcmrl.viewer_geometry_data_t.SPHERE:
d = DebugData()
d.addSphere(center=(0,0,0), radius=geom.float_data[0])
return d.getPolyData()
elif geom.type == lcmrl.viewer_geometry_data_t.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 == lcmrl.viewer_geometry_data_t.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()
elif hasattr(lcmrl.viewer_geometry_data_t, "ELLIPSOID") and geom.type == lcmrl.viewer_geometry_data_t.ELLIPSOID:
d = DebugData()
radii = geom.float_data[0:3]
d.addEllipsoid(center=(0,0,0), radii=radii)
return d.getPolyData()
raise Exception('Unsupported geometry type: %s' % geom.type)
def setScale(self, scale):
data = DebugData()
center = [0, 0, 1.0 / 2 - 0.5]
dimensions = [scale, scale / 2, 1.0]
data.addCube(dimensions, center)
polydata = data.getPolyData()
self._polydata = polydata
self._raw_polydata = polydata
def testCreateBackground(self):
d = DebugData()
d.addCube((1,1,0.1), (0,0,0), color=[1,1,1])
self.backgroundPolyData = d.getPolyData()
if self.vis:
view = self.views['background']
vis.updatePolyData(self.backgroundPolyData, 'background', view=view, colorByName='RGB255')
view.resetCamera()
def testCreateBackground(self):
d = DebugData()
d.addCube((1,1,0.1), (0,0,0), color=[1,1,1])
self.backgroundPolyData = d.getPolyData()
if self.vis:
view = self.views['background']
vis.updatePolyData(self.backgroundPolyData, 'background', view=view, colorByName='RGB255')
view.resetCamera()
def spawnCropBox(self, dims=None):
if dims is None:
dim_x = CONFIG['crop_box']['dimensions']['x']
dim_y = CONFIG['crop_box']['dimensions']['y']
dim_z = CONFIG['crop_box']['dimensions']['z']
dims = [dim_x, dim_y, dim_z]
transform = director_utils.transformFromPose(CONFIG['crop_box']['transform'])
d = DebugData()
d.addCube(dims, [0,0,0], color=[0,1,0])
self.cube_vis = vis.updatePolyData(d.getPolyData(), 'Crop Cube', colorByName='RGB255')
vis.addChildFrame(self.cube_vis)
self.cube_vis.getChildFrame().copyFrame(transform)
self.cube_vis.setProperty('Alpha', 0.3)
def testCreateForeground(self):
d = DebugData()
dims = np.array([0.2,0.2,0.2])
center = (0,0,dims[2]/2.0)
d.addCube(dims, center, color=[0,1,0])
d.addPolyData(self.backgroundPolyData)
self.foregroundPolyData = d.getPolyData()
if self.vis:
view=self.views['foreground']
vis.updatePolyData(self.foregroundPolyData, 'foreground', view=view, colorByName='RGB255')
view.resetCamera()
def updateCameraMesh():
scale = cameraObj.getChildFrame().getProperty('Scale') * 10.0
rayLength = scale
d = DebugData()
d.addCube(dimensions=[0.04, 0.08, 0.06], center=[-0.02, 0.0, 0.0], color=[1,0.5,0])
d.addLine([0.0, 0.0, 0.0], [0.01, 0.0, 0.0], radius=0.023, color=[1,0.5,0])
cameraModelMesh = d.getPolyData()
t = vtk.vtkTransform()
t.Scale(scale, scale, scale)
cameraModelMesh = filterUtils.transformPolyData(cameraModelMesh, t)
cameraMesh = getCameraFrustumMesh(depthScanner.view, rayLength)
cameraMesh = filterUtils.transformPolyData(cameraMesh, getCameraTransform(depthScanner.view.camera()).GetLinearInverse())
cameraMesh = filterUtils.appendPolyData([cameraMesh, cameraModelMesh])
cameraObj.setPolyData(cameraMesh)
def __init__(self, velocity=25.0, scale=5.0):
"""Constructs a Robot.
Args:
velocity: Velocity of the robot in the forward direction.
scale: Scale of the model.
exploration: Exploration rate.
model: Object model to use.
"""
data = DebugData()
center = [0, 0, 1.0 / 2 - 0.5]
dimensions = [scale, scale / 2, 1.0]
data.addCube(dimensions, center)
polydata = data.getPolyData()
super(Commonship, self).__init__(velocity, polydata)
def testCreateForeground(self):
d = DebugData()
dims = np.array([0.2,0.2,0.2])
center = (0,0,dims[2]/2.0)
d.addCube(dims, center, color=[0,1,0])
d.addPolyData(self.backgroundPolyData)
self.foregroundPolyData = d.getPolyData()
if self.vis:
view=self.views['foreground']
vis.updatePolyData(self.foregroundPolyData, 'foreground', view=view, colorByName='RGB255')
view.resetCamera()
def __init__(self, velocity, radius, bounds, height=1.0):
"""Constructs a Robot.
Args:
velocity: Velocity of the robot in the forward direction.
radius: Radius of the obstacle.
"""
data = DebugData()
self._bounds = bounds
self._radius = radius
self._height = height
center = [0, 0, height / 2 - 0.5]
axis = [0, 0, 1] # Upright cylinder.
data.addCylinder(center, axis, height, radius)
data.addCube()
polydata = data.getPolyData()
super(Obstacle, self).__init__(velocity, polydata)
def spawnCropBox(self, dims=None):
if dims is None:
dim_x = CONFIG['crop_box']['dimensions']['x']
dim_y = CONFIG['crop_box']['dimensions']['y']
dim_z = CONFIG['crop_box']['dimensions']['z']
dims = [dim_x, dim_y, dim_z]
transform = director_utils.transformFromPose(
CONFIG['crop_box']['transform'])
d = DebugData()
d.addCube(dims, [0, 0, 0], color=[0, 1, 0])
self.cube_vis = vis.updatePolyData(d.getPolyData(),
'Crop Cube',
colorByName='RGB255')
vis.addChildFrame(self.cube_vis)
self.cube_vis.getChildFrame().copyFrame(transform)
self.cube_vis.setProperty('Alpha', 0.3)
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 updateCameraMesh():
scale = cameraObj.getChildFrame().getProperty('Scale') * 10.0
rayLength = scale
d = DebugData()
d.addCube(dimensions=[0.04, 0.08, 0.06],
center=[-0.02, 0.0, 0.0],
color=[1, 0.5, 0])
d.addLine([0.0, 0.0, 0.0], [0.01, 0.0, 0.0],
radius=0.023,
color=[1, 0.5, 0])
cameraModelMesh = d.getPolyData()
t = vtk.vtkTransform()
t.Scale(scale, scale, scale)
cameraModelMesh = filterUtils.transformPolyData(cameraModelMesh, t)
cameraMesh = getCameraFrustumMesh(depthScanner.view, rayLength)
cameraMesh = filterUtils.transformPolyData(
cameraMesh,
getCameraTransform(depthScanner.view.camera()).GetLinearInverse())
cameraMesh = filterUtils.appendPolyData([cameraMesh, cameraModelMesh])
cameraObj.setPolyData(cameraMesh)
def createBox(params):
d = DebugData()
d.addCube(dimensions=params["lengths"], center=(0, 0, 0))
return [d.getPolyData()]
d.addEllipsoid((0, 0, 0), radii=(0.5, 0.35, 0.2))
show(d, (4, 2, 0))
d = DebugData()
d.addTorus(radius=0.5, thickness=0.2)
show(d, (6, 2, 0))
d = DebugData()
d.addCone(origin=(0,0,0), normal=(0,1,0), radius=0.3, height=0.8, color=[1, 1, 0])
show(d, (0, 4, 0))
d = DebugData()
d.addCube(dimensions=[0.8, 0.5, 0.3], center=[0, 0, 0], color=[0, 1, 1])
show(d, (2, 4, 0))
d = DebugData()
d.addPlane(origin=[0, 0, 0], normal=[0, 0, 1], width=0.8, height=0.7, resolution=10, color=[0, 1, 0])
show(d, (4, 4, 0)).setProperty('Surface Mode', 'Surface with edges')
d = DebugData()
d.addCapsule(center=[0, 0, 0], axis=[1, 0, 0], length=1.0, radius=0.1, color=[0.5, 0.5, 1])
show(d, (6, 4, 0))
d = DebugData()
polyData = vnp.numpyToPolyData(np.random.random((1000, 3)))
vnp.addNumpyToVtk(polyData, np.arange(polyData.GetNumberOfPoints()), 'point_ids')
def createBox(params):
d = DebugData()
color = params.get("color", DEFAULT_COLOR)[:3]
d.addCube(dimensions=params["lengths"], center=(0, 0, 0), color=color)
return [d.getPolyData()]
def updateGeometryFromProperties(self):
d = DebugData()
d.addCube(self.getProperty('Dimensions'), (0, 0, 0),
subdivisions=self.getProperty('Subdivisions'))
self.setPolyData(d.getPolyData())
def createBox(params):
d = DebugData()
color = params.get("color", DEFAULT_COLOR)[:3]
d.addCube(dimensions=params["lengths"], center=(0, 0, 0), color=color)
return [d.getPolyData()]
def updateGeometryFromProperties(self):
d = DebugData()
d.addCube(self.getProperty('Dimensions'), (0,0,0), subdivisions=self.getProperty('Subdivisions'))
self.setPolyData(d.getPolyData())
