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 add_target(self, target):
data = DebugData()
center = [target[0], target[1], 1]
axis = [0, 0, 1] # Upright cylinder.
data.addCylinder(center, axis, 2, 3)
om.removeFromObjectModel(om.findObjectByName("target"))
self._add_polydata(data.getPolyData(), "target", [0, 0.8, 0])
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 createCylinder(params):
d = DebugData()
d.addCylinder(center=(0, 0, 0),
axis=(0, 0, 1),
radius=params["radius"],
length=params["length"])
return [d.getPolyData()]
def __init__(self, points, polyline=True, circle=None):
"""Constructs an Obstacle.
Args:
:type points: point array for polyline obstacle
:type polyline: if obstacle a polyline or not
:type circle: if obstacle in circular shape or not
"""
data = DebugData()
if polyline:
polydata = self.add_vtk_polygon(points)
polydata = filterUtils.appendPolyData([polydata])
elif circle is not None:
center = [0, 0, 0]
axis = [0, 0, 1] # Upright cylinder.
data.addCylinder(center, axis, 0.3, 0.1)
polydata = data.getPolyData()
else:
center = [points[0], points[1], -1]
axis = [0, 0, 1] # Upright cylinder.
data.addCircle(center, axis, 5)
polydata = data.getPolyData()
self._state = np.array([0., 0., 0.])
self.altitude = 0.
self._velocity = 0.
self._raw_polydata = polydata
self._polydata = polydata
def add_target(self, target):
data = DebugData()
center = [target[0], target[1], 1]
axis = [0, 0, 1] # Upright cylinder.
data.addCylinder(center, axis, 2, 3)
om.removeFromObjectModel(om.findObjectByName("target"))
self._add_polydata(data.getPolyData(), "target", [0, 0.8, 0])
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 updateGeometryFromProperties(self):
d = DebugData()
length = self.getProperty('Length')
d.addCylinder(center=(0, 0, 0),
axis=(0, 0, 1),
length=self.getProperty('Length'),
radius=self.getProperty('Radius'))
self.setPolyData(d.getPolyData())
def createCylinder(params):
d = DebugData()
color = params.get("color", DEFAULT_COLOR)[:3]
d.addCylinder(center=(0, 0, 0),
axis=(0, 0, 1),
radius=params["radius"],
length=params["length"],
color=color)
return [d.getPolyData()]
def createCylinder(params):
d = DebugData()
color = params.get("color", DEFAULT_COLOR)[:3]
d.addCylinder(center=(0, 0, 0),
axis=(0, 0, 1),
radius=params["radius"],
length=params["length"],
color=color)
return [d.getPolyData()]
def createCapsule(params):
d = DebugData()
radius = params["radius"]
length = params["length"]
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()]
def createCapsule(params):
d = DebugData()
radius = params["radius"]
length = params["length"]
color = params.get("color", DEFAULT_COLOR)[:3]
d.addCylinder(center=(0, 0, 0),
axis=(0, 0, 1),
radius=radius,
length=length,
color=color)
d.addSphere(center=(0, 0, length / 2.0), radius=radius, color=color)
d.addSphere(center=(0, 0, -length / 2.0), radius=radius, color=color)
return [d.getPolyData()]
def createCapsule(params):
d = DebugData()
radius = params["radius"]
length = params["length"]
color = params.get("color", DEFAULT_COLOR)[:3]
d.addCylinder(center=(0, 0, 0),
axis=(0, 0, 1),
radius=radius,
length=length,
color=color)
d.addSphere(center=(0, 0, length / 2.0), radius=radius, color=color)
d.addSphere(center=(0, 0, -length / 2.0), radius=radius, color=color)
return [d.getPolyData()]
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)
polydata = data.getPolyData()
super(Obstacle, self).__init__(velocity, polydata)
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)
polydata = data.getPolyData()
super(Obstacle, self).__init__(velocity, polydata)
def updateGeometryFromProperties(self):
d = DebugData()
length = self.getProperty('Length')
d.addCylinder(center=(0,0,0), axis=(0,0,1), length=self.getProperty('Length'), radius=self.getProperty('Radius'))
self.setPolyData(d.getPolyData())
