def updatePlanFrames(self):
if self.getViewMode() != 'frames':
return
numberOfSamples = self.getNumberOfSamples()
meshes = self.planPlayback.getPlanPoseMeshes(
self.plan, self.playbackJointController, self.playbackRobotModel,
numberOfSamples)
d = DebugData()
startColor = [0.8, 0.8, 0.8]
endColor = [85 / 255.0, 255 / 255.0, 255 / 255.0]
colorFunc = scipy.interpolate.interp1d([0, numberOfSamples - 1],
[startColor, endColor],
axis=0,
kind='slinear')
for i, mesh in reversed(list(enumerate(meshes))):
d.addPolyData(mesh, color=colorFunc(i))
pd = d.getPolyData()
clean = vtk.vtkCleanPolyData()
clean.SetInput(pd)
clean.Update()
pd = clean.GetOutput()
self.planFramesObj = vis.updatePolyData(d.getPolyData(),
'robot plan',
alpha=1.0,
visible=False,
colorByName='RGB255',
parent='planning')
self.showPlanFrames()
def draw(self):
d = DebugData()
points = list(self.points)
if self.hoverPos is not None:
points.append(self.hoverPos)
# draw points
for p in points:
d.addSphere(p, radius=5)
if self.drawLines and len(points) > 1:
for a, b in zip(points, points[1:]):
d.addLine(a, b)
# connect end points
# d.addLine(points[0], points[-1])
if self.annotationObj:
self.annotationObj.setPolyData(d.getPolyData())
else:
self.annotationObj = vis.updatePolyData(d.getPolyData(), 'annotation', parent='segmentation', color=[1,0,0], view=self.view)
self.annotationObj.addToView(self.view)
self.annotationObj.actor.SetPickable(False)
self.annotationObj.actor.GetProperty().SetLineWidth(2)
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 updatePlanFrames(self):
if self.getViewMode() != 'frames':
return
numberOfSamples = self.getNumberOfSamples()
meshes = self.planPlayback.getPlanPoseMeshes(self.plan, self.playbackJointController, self.playbackRobotModel, numberOfSamples)
d = DebugData()
startColor = [0.8, 0.8, 0.8]
endColor = [85/255.0, 255/255.0, 255/255.0]
colorFunc = scipy.interpolate.interp1d([0, numberOfSamples-1], [startColor, endColor], axis=0, kind='slinear')
for i, mesh in reversed(list(enumerate(meshes))):
d.addPolyData(mesh, color=colorFunc(i))
pd = d.getPolyData()
clean = vtk.vtkCleanPolyData()
clean.SetInput(pd)
clean.Update()
pd = clean.GetOutput()
self.planFramesObj = vis.updatePolyData(d.getPolyData(), 'robot plan', alpha=1.0, visible=False, colorByName='RGB255', parent='planning')
self.showPlanFrames()
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 updateIntersection(frame):
origin = np.array(frame.transform.GetPosition())
rayLength = 5
for i in range(0, numRays):
ray = rays[:, i]
rayTransformed = np.array(frame.transform.TransformNormal(ray))
intersection = computeIntersection(locator, origin,
origin + rayTransformed * rayLength)
name = 'ray intersection ' + str(i)
if intersection is not None:
om.removeFromObjectModel(om.findObjectByName(name))
d = DebugData()
d.addLine(origin, intersection)
color = [1, 0, 0]
# d.addSphere(intersection, radius=0.04)
vis.updatePolyData(d.getPolyData(), name, color=color)
else:
om.removeFromObjectModel(om.findObjectByName(name))
d = DebugData()
d.addLine(origin, origin + rayTransformed * rayLength)
color = [0, 1, 0]
# d.addSphere(intersection, radius=0.04)
vis.updatePolyData(d.getPolyData(), name, color=color)
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 == 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 draw(self):
d = DebugData()
points = list(self.points)
if self.hoverPos is not None:
points.append(self.hoverPos)
# draw points
for p in points:
d.addSphere(p, radius=5)
if self.drawLines and len(points) > 1:
for a, b in zip(points, points[1:]):
d.addLine(a, b)
# connect end points
# d.addLine(points[0], points[-1])
if self.annotationObj:
self.annotationObj.setPolyData(d.getPolyData())
else:
self.annotationObj = vis.updatePolyData(d.getPolyData(),
'annotation',
parent='segmentation',
color=[1, 0, 0],
view=self.view)
self.annotationObj.addToView(self.view)
self.annotationObj.actor.SetPickable(False)
self.annotationObj.actor.GetProperty().SetLineWidth(2)
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 showDebugPoint(p, name='debug point', update=False, visible=True):
d = DebugData()
d.addSphere(p, radius=0.01, color=[1, 0, 0])
if update:
vis.updatePolyData(d.getPolyData(), name)
else:
vis.showPolyData(d.getPolyData(),
name,
colorByName='RGB255',
visible=visible)
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 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 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 updateDrawPolyApprox(self, frame):
distances = self.Sensor.raycastAll(frame)
polyCoefficients = self.SensorApproximator.polyFitConstrainedLP(distances)
if polyCoefficients == None:
polyCoefficients = [0,0]
d = DebugData()
x = self.SensorApproximator.approxThetaVector
y = x * 0.0
for index,val in enumerate(y):
y[index] = self.horner(x[index],polyCoefficients)
origin = np.array(frame.transform.GetPosition())
origin[2] = -0.001
for i in xrange(self.SensorApproximator.numApproxPoints):
if y[i] > 0:
ray = self.SensorApproximator.approxRays[:,i]
rayTransformed = np.array(frame.transform.TransformNormal(ray))
intersection = origin + rayTransformed * y[i]
intersection[2] = -0.001
d.addLine(origin, intersection, color=[0,0,1])
vis.updatePolyData(d.getPolyData(), 'polyApprox', colorByName='RGB255')
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 clear_locator(self):
d = DebugData()
for ship, frame in self._commonships:
d.addPolyData(ship.to_positioned_polydata())
self.locator = vtk.vtkCellLocator()
self.locator.SetDataSet(d.getPolyData())
self.locator.BuildLocator()
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 newMesh():
d = DebugData()
d.addArrow((0,0,0), (0,0,0.3))
pd = d.getPolyData()
meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd)
desc = dict(classname='MeshAffordanceItem', Name='test mesh', Filename=meshId, pose=((0.5,0.0,1.0), (1,0,0,0)))
return affordanceManager.newAffordanceFromDescription(desc)
def to_polydata(self, bottle_detected=False):
"""Converts the sensor to polydata."""
d = DebugData()
origin = np.array([self._state[0], self._state[1], self.z_distance])
for hit, intersection, ray in zip(self._hit,
self._intersections,
self._rays):
if hit:
color = [1., 0.45882353, 0.51372549]
endpoint = intersection
else:
color = [0., 0.6, 0.58823529]
endpoint = origin + ray
d.addLine(origin, endpoint, color=color, radius=0.05)
# add vision sensor
center = [self._state[0], self._state[1], 0]
axis = [0, 0, 1]
if bottle_detected:
color = [0., 0.8, 0.]
else:
color = [0., 0.6, 0.58823529]
d.addCircle(center, axis, 5, color=color)
return d.getPolyData()
def createPolyLine(params):
d = DebugData()
points = [np.asarray(p) for p in params["points"]]
color = params.get("color", DEFAULT_COLOR)[:3]
radius = params.get("radius", 0.01)
startHead = params.get("start_head", False)
endHead = params.get("end_head", False)
headRadius = params.get("head_radius", 0.05)
headLength = params.get("head_length", headRadius)
isClosed = params.get("closed", False)
if startHead:
normal = points[0] - points[1]
normal = normal / np.linalg.norm(normal)
points[0] = points[0] - 0.5 * headLength * normal
d.addCone(origin=points[0],
normal=normal,
radius=headRadius,
height=headLength,
color=color,
fill=True)
if endHead:
normal = points[-1] - points[-2]
normal = normal / np.linalg.norm(normal)
points[-1] = points[-1] - 0.5 * headLength * normal
d.addCone(origin=points[-1],
normal=normal,
radius=headRadius,
height=headLength,
color=color,
fill=True)
d.addPolyLine(points, isClosed, radius=radius, color=color)
return [d.getPolyData()]
def updateGeometryFromProperties(self):
filename = self.getProperty('Filename')
scale = self.getProperty('Scale')
if not filename:
polyData = vtk.vtkPolyData()
else:
polyData = self.getMeshManager().get(filename)
if not polyData:
if not os.path.isabs(filename):
filename = os.path.join(director.getDRCBaseDir(), filename)
if os.path.isfile(filename):
polyData = ioUtils.readPolyData(filename)
if not scale == [1, 1, 1]:
transform = vtk.vtkTransform()
transform.Scale(scale)
polyData = filterUtils.transformPolyData(
polyData, transform)
else:
# use axes as a placeholder mesh
d = DebugData()
d.addFrame(vtk.vtkTransform(), scale=0.1, tubeRadius=0.005)
polyData = d.getPolyData()
self.setPolyData(polyData)
def __init__(self, robotSystem, view):
self.robotStateModel = robotSystem.robotStateModel
self.robotStateJointController = robotSystem.robotStateJointController
self.robotSystem = robotSystem
self.lFootFtFrameId = self.robotStateModel.model.findLinkID(
self.robotSystem.ikPlanner.leftFootLink)
self.rFootFtFrameId = self.robotStateModel.model.findLinkID(
self.robotSystem.ikPlanner.rightFootLink)
self.leftInContact = 0
self.rightInContact = 0
self.view = view
self.ddDrakeWrapper = PythonQt.dd.ddDrakeWrapper()
self.warningButton = QtGui.QLabel('COP Warning')
self.warningButton.setStyleSheet("background-color:white")
self.dialogVisible = False
d = DebugData()
vis.updatePolyData(d.getPolyData(),
'measured cop',
view=self.view,
parent='robot state model')
om.findObjectByName('measured cop').setProperty('Visible', False)
self.robotStateModel.connectModelChanged(self.update)
def newMesh():
d = DebugData()
d.addArrow((0,0,0), (0,0,0.3))
pd = d.getPolyData()
meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd)
desc = dict(classname='MeshAffordanceItem', Name='test mesh', Filename=meshId, pose=((0.5,0.0,1.0), (1,0,0,0)))
return affordanceManager.newAffordanceFromDescription(desc)
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 buildGlobalGoal(scale):
#print "building circle world"
d = DebugData()
worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d, scale=scale, boundaryType="Square", withData=False)
#print "boundaries done"
firstX = worldXmin + np.random.rand()*(worldXmax-worldXmin)
firstY = worldYmin + np.random.rand()*(worldYmax-worldYmin)
firstEndpt = (firstX,firstY,0.2)
secondEndpt = (firstX,firstY,-0.2)
#d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn())
d.addLine(firstEndpt, secondEndpt, radius=1.0, color=[0.5,1,0])
obj = vis.updatePolyData(d.getPolyData(), 'global_goal', colorByName='RGB255')
world = World()
world.visObj = obj
world.global_goal_x = firstX
world.global_goal_y = firstY
print "I should have actually built a goal"
return world
def addTestData():
d = DebugData()
# d.addSphere((0,0,0), radius=0.5)
d.addArrow((0,0,0), (0,0,1), color=[1,0,0])
d.addArrow((0,0,1), (0,.5,1), color=[0,1,0])
vis.showPolyData(d.getPolyData(), 'debug data', colorByName='RGB255')
view.resetCamera()
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 updateDrawIntersection(self, frame):
origin = np.array(frame.transform.GetPosition())
#print "origin is now at", origin
d = DebugData()
sliderIdx = self.slider.value
controllerType = self.getControllerTypeFromCounter(sliderIdx)
colorMaxRange = self.colorMap[controllerType]
# if the QValue was empty then color it green
if self.emptyQValue[sliderIdx]:
colorMaxRange = [1, 1, 0] # this is yellow
for i in xrange(self.Sensor.numRays):
ray = self.Sensor.rays[:, i]
rayTransformed = np.array(frame.transform.TransformNormal(ray))
#print "rayTransformed is", rayTransformed
intersection = self.Sensor.raycast(
self.locator, origin,
origin + rayTransformed * self.Sensor.rayLength)
if intersection is not None:
d.addLine(origin, intersection, color=[1, 0, 0])
else:
d.addLine(origin,
origin + rayTransformed * self.Sensor.rayLength,
color=colorMaxRange)
vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
def setRegion(self, safe_region):
debug = DebugData()
pos = safe_region.point
try:
xy_verts = safe_region.xy_polytope()
if xy_verts.shape[1] == 0:
raise QhullError("No points returned")
xyz_verts = np.vstack((xy_verts, pos[2] + 0.02 + np.zeros((1, xy_verts.shape[1]))))
xyz_verts = np.hstack((xyz_verts, np.vstack((xy_verts, pos[2] + 0.015 + np.zeros((1, xy_verts.shape[1]))))))
# print xyz_verts.shape
polyData = vnp.getVtkPolyDataFromNumpyPoints(xyz_verts.T.copy())
vol_mesh = filterUtils.computeDelaunay3D(polyData)
for j in range(xy_verts.shape[1]):
z = pos[2] + 0.005
p1 = np.hstack((xy_verts[:,j], z))
if j < xy_verts.shape[1] - 1:
p2 = np.hstack((xy_verts[:,j+1], z))
else:
p2 = np.hstack((xy_verts[:,0], z))
debug.addLine(p1, p2, color=[.7,.7,.7], radius=0.003)
debug.addPolyData(vol_mesh)
# self.setPolyData(vol_mesh)
self.setPolyData(debug.getPolyData())
self.safe_region = safe_region
except QhullError:
print "Could not generate convex hull (polytope is likely unbounded)."
def setRegion(self, safe_region):
debug = DebugData()
pos = safe_region.point
try:
xy_verts = safe_region.xy_polytope()
if xy_verts.shape[1] == 0:
raise QhullError("No points returned")
xyz_verts = np.vstack((xy_verts, pos[2] + 0.02 + np.zeros(
(1, xy_verts.shape[1]))))
xyz_verts = np.hstack((xyz_verts,
np.vstack(
(xy_verts, pos[2] + 0.015 + np.zeros(
(1, xy_verts.shape[1]))))))
# print xyz_verts.shape
polyData = vnp.getVtkPolyDataFromNumpyPoints(xyz_verts.T.copy())
vol_mesh = filterUtils.computeDelaunay3D(polyData)
for j in range(xy_verts.shape[1]):
z = pos[2] + 0.005
p1 = np.hstack((xy_verts[:, j], z))
if j < xy_verts.shape[1] - 1:
p2 = np.hstack((xy_verts[:, j + 1], z))
else:
p2 = np.hstack((xy_verts[:, 0], z))
debug.addLine(p1, p2, color=[.7, .7, .7], radius=0.003)
debug.addPolyData(vol_mesh)
# self.setPolyData(vol_mesh)
self.setPolyData(debug.getPolyData())
self.safe_region = safe_region
except QhullError:
print(
"Could not generate convex hull (polytope is likely unbounded)."
)
def handle_message(self, msg):
# Limits the rate of message handling, since redrawing is done in the
# message handler.
self._sub.setSpeedLimit(0.1)
# Removes the folder completely.
om.removeFromObjectModel(om.findObjectByName(self._folder_name))
# Recreates folder.
folder = om.getOrCreateContainer(self._folder_name)
# A map from pair of body names to a list of contact forces
collision_pair_to_forces = {}
for arrow in msg.arrow_info:
point = np.array([
arrow.arrow_origin[0], arrow.arrow_origin[1],
arrow.arrow_origin[2]
])
vec = np.array([
arrow.arrow_vector[0], arrow.arrow_vector[1],
arrow.arrow_vector[2]
])
d = DebugData()
d.addArrow(start=point,
end=vec + point,
tubeRadius=0.005,
headRadius=0.01)
vis.showPolyData(d.getPolyData(),
str(0),
parent=folder,
color=[arrow.rgb[0], arrow.rgb[1], arrow.rgb[2]])
def draw(self):
d = DebugData()
points = [p if p is not None else self.hoverPos for p in self.points]
# draw points
for p in points:
if p is not None:
d.addSphere(p, radius=0.008)
if self.drawLines:
# draw lines
for a, b in zip(points, points[1:]):
if b is not None:
d.addLine(a, b)
# connect end points
if points[-1] is not None and self.drawClosedLoop:
d.addLine(points[0], points[-1])
self.annotationObj = vis.updatePolyData(d.getPolyData(),
self.annotationName,
parent=self.annotationFolder,
view=self.view)
self.annotationObj.setProperty('Color', [1,0,0])
self.annotationObj.actor.SetPickable(False)
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 onMouseMove(self, displayPoint, modifiers=None):
om.removeFromObjectModel(om.findObjectByName('link selection'))
self.linkName = None
self.pickedPoint = None
selection = self.getSelection(displayPoint)
if selection is None:
return
pickedPoint, linkName, normal, pickedCellId = selection
d = DebugData()
d.addSphere(pickedPoint, radius=0.01)
d.addLine(pickedPoint, np.array(pickedPoint) + 0.1 * np.array(normal), radius=0.005)
obj = vis.updatePolyData(d.getPolyData(), 'link selection', color=[0,1,0])
obj.actor.SetPickable(False)
self.linkName = linkName
self.pickedPoint = pickedPoint
self.normal = normal
self.pickedCellId = pickedCellId
modifiers = QtGui.QApplication.keyboardModifiers()
if modifiers == QtCore.Qt.ControlModifier and self.cellCaptureMode:
self.provisionallyCaptureCell(linkName, pickedCellId)
def buildRobot(x=0,y=0):
d = DebugData()
d.addCone((x,y,0), (1,0,0), height=0.2, radius=0.1)
obj = vis.showPolyData(d.getPolyData(), 'robot')
frame = vis.addChildFrame(obj)
return obj
def updateGeometryFromProperties(self):
filename = self.getProperty('Filename')
scale = self.getProperty('Scale')
if not filename:
polyData = vtk.vtkPolyData()
else:
polyData = self.getMeshManager().get(filename)
if not polyData:
if not os.path.isabs(filename):
filename = os.path.join(director.getDRCBaseDir(), filename)
if os.path.isfile(filename):
polyData = ioUtils.readPolyData(filename)
if not scale == [1, 1, 1]:
transform = vtk.vtkTransform()
transform.Scale(scale)
transformFilter = vtk.vtkTransformPolyDataFilter()
transformFilter.SetInput(polyData)
transformFilter.SetTransform(transform)
transformFilter.Update()
polyData = transformFilter.GetOutput()
else:
# use axes as a placeholder mesh
d = DebugData()
d.addFrame(vtk.vtkTransform(), scale=0.1, tubeRadius=0.005)
polyData = d.getPolyData()
self.setPolyData(polyData)
def drawActionSetFull(self, go_nowhere=False):
# print "I am drawing the action set"
d = DebugData()
for index, value in enumerate(self.pos_trajectories):
for time_step_index in xrange(2 * self.numPointsToDraw - 1):
firstX = value[0, time_step_index]
firstY = value[1, time_step_index]
firstZ = value[2, time_step_index]
secondX = value[0, time_step_index + 1]
secondY = value[1, time_step_index + 1]
secondZ = value[2, time_step_index + 1]
firstEndpt = (firstX, firstY, firstZ)
secondEndpt = (secondX, secondY, secondZ)
if time_step_index >= 10:
color = [0.8, 0, 0.8]
else:
color = [0.1, 0.1, 1.0]
if go_nowhere:
firstEndpt = [0, 0, 0]
secondEndpt = [0, 0, 0.001]
d.addLine(firstEndpt, secondEndpt, radius=0.02, color=color)
obj = vis.updatePolyData(d.getPolyData(), "action_set", colorByName="RGB255")
def drawCenterOfMass(model):
stanceFrame = footstepsDriver.getFeetMidPoint(model)
com = list(model.model.getCenterOfMass())
com[2] = stanceFrame.GetPosition()[2]
d = DebugData()
d.addSphere(com, radius=0.015)
obj = vis.updatePolyData(d.getPolyData(), 'COM %s' % model.getProperty('Name'), color=[1,0,0], visible=False, parent=model)
def buildGlobalGoal():
#print "building circle world"
d = DebugData()
worldXmin = 25
worldXmax = 50
worldYmin = -20
worldYmax = 20
firstX = worldXmin + np.random.rand()*(worldXmax-worldXmin)
firstY = worldYmin + np.random.rand()*(worldYmax-worldYmin)
firstEndpt = (firstX,firstY,0.2)
secondEndpt = (firstX,firstY,-0.2)
#d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn())
d.addLine(firstEndpt, secondEndpt, radius=0.3, color=[0.5,1,0])
obj = vis.updatePolyData(d.getPolyData(), 'global_goal', colorByName='RGB255')
world = World()
world.visObj = obj
world.global_goal_x = firstX
world.global_goal_y = firstY
return world
def getCameraFrustumMesh(view, rayLength=1.0):
origin = np.array(view.camera().GetPosition())
def getCameraRay(displayPoint):
_, ray = vis.getRayFromDisplayPoint(view, displayPoint)
ray = ray-origin
ray /= np.linalg.norm(ray)
return ray
viewWidth, viewHeight = view.renderWindow().GetSize()
rays = [getCameraRay(x) for x in [[0.0, 0.0], [viewWidth, 0.0], [viewWidth, viewHeight], [0.0, viewHeight]]]
rays = [rayLength*r for r in rays]
camPos = origin
lineRadius = 0.0
color = [1.0, 1.0, 1.0]
d = DebugData()
d.addLine(camPos, camPos+rays[0], radius=lineRadius, color=color)
d.addLine(camPos, camPos+rays[1], radius=lineRadius, color=color)
d.addLine(camPos, camPos+rays[2], radius=lineRadius, color=color)
d.addLine(camPos, camPos+rays[3], radius=lineRadius, color=color)
d.addLine(camPos+rays[0], camPos+rays[1], radius=lineRadius, color=color)
d.addLine(camPos+rays[1], camPos+rays[2], radius=lineRadius, color=color)
d.addLine(camPos+rays[2], camPos+rays[3], radius=lineRadius, color=color)
d.addLine(camPos+rays[3], camPos+rays[0], radius=lineRadius, color=color)
pd = d.getPolyData()
return pd
def getMarkerGeometry(self):
if self.markerGeometry is None:
d = DebugData()
d.addSphere(np.zeros(3), radius=0.007, resolution=8)
self.markerGeometry = shallowCopy(d.getPolyData())
return self.markerGeometry
def updateDrawNormals(self, frame):
leftDistances = self.raycastLeftOrRight(frame, self.leftRays)
rightDistances = self.raycastLeftOrRight(frame, self.rightRays)
d = DebugData()
x = self.SensorApproximator.approxThetaVector
y = x * 0.0
for index,val in enumerate(y):
y[index] = self.horner(x[index],polyCoefficients)
origin = np.array(frame.transform.GetPosition())
origin[2] = -0.001
for i in xrange(self.numRays):
#ray = self.SensorApproximator.approxRays[:,i]
#rayTransformed = np.array(frame.transform.TransformNormal(ray))
#intersection = origin + rayTransformed * y[i]
#intersection[2] = -0.001
p1 = leftDistances[i]
p2 = rightDistances[i]
d.addLine(p1, p2, color=[0,0.1,1])
vis.updatePolyData(d.getPolyData(), 'polyApprox', colorByName='RGB255')
def updateDrawIntersection(self, frame, locator="None"):
if locator == "None":
locator = self.locator
origin = np.array(frame.transform.GetPosition())
# print "origin is now at", origin
d = DebugData()
sliderIdx = self.slider.value
controllerType = self.getControllerTypeFromCounter(sliderIdx)
colorMaxRange = self.colorMap[controllerType]
for i in xrange(self.Sensor.numRays):
ray = self.Sensor.rays[:, i]
rayTransformed = np.array(frame.transform.TransformNormal(ray))
# print "rayTransformed is", rayTransformed
intersection = self.Sensor.raycast(locator, origin, origin + rayTransformed * self.Sensor.rayLength)
if intersection is not None:
d.addLine(origin, intersection, color=[1, 0, 0])
else:
d.addLine(origin, origin + rayTransformed * self.Sensor.rayLength, color=colorMaxRange)
vis.updatePolyData(d.getPolyData(), "rays", colorByName="RGB255")
def draw(self):
d = DebugData()
points = [p if p is not None else self.hoverPos for p in self.points]
# draw points
for p in points:
if p is not None:
d.addSphere(p, radius=0.008)
if self.drawLines:
# draw lines
for a, b in zip(points, points[1:]):
if b is not None:
d.addLine(a, b)
# connect end points
if points[-1] is not None and self.drawClosedLoop:
d.addLine(points[0], points[-1])
self.annotationObj = vis.updatePolyData(d.getPolyData(),
self.annotationName,
parent=self.annotationFolder,
view=self.view)
self.annotationObj.setProperty('Color', [1, 0, 0])
self.annotationObj.actor.SetPickable(False)
def drawCenterOfMass(model):
stanceFrame = footstepsDriver.getFeetMidPoint(model)
com = list(model.model.getCenterOfMass())
com[2] = stanceFrame.GetPosition()[2]
d = DebugData()
d.addSphere(com, radius=0.015)
obj = vis.updatePolyData(d.getPolyData(), 'COM %s' % model.getProperty('Name'), color=[1,0,0], visible=False, parent=model)
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 addTestData():
d = DebugData()
# d.addSphere((0,0,0), radius=0.5)
d.addArrow((0, 0, 0), (0, 0, 1), color=[1, 0, 0])
d.addArrow((0, 0, 1), (0, .5, 1), color=[0, 1, 0])
vis.showPolyData(d.getPolyData(), 'debug data', colorByName='RGB255')
view.resetCamera()
def handle_message(self, msg):
# Limits the rate of message handling, since redrawing is done in the
# message handler.
self._sub.setSpeedLimit(30)
# Removes the folder completely.
om.removeFromObjectModel(om.findObjectByName(self._folder_name))
# Recreates folder.
folder = om.getOrCreateContainer(self._folder_name)
# Though strangely named, DebugData() is the object through which
# drawing is done in DrakeVisualizer.
d = DebugData()
# Iterate over all triangles, drawing the contact surface as a triangle
# outlined in blue.
for surface in msg.hydroelastic_contacts:
for tri in surface.triangles:
va = np.array([tri.p_WA[0], tri.p_WA[1], tri.p_WA[2]])
vb = np.array([tri.p_WB[0], tri.p_WB[1], tri.p_WB[2]])
vc = np.array([tri.p_WC[0], tri.p_WC[1], tri.p_WC[2]])
d.addLine(p1=va, p2=vb, radius=0, color=[0, 0, 1])
d.addLine(p1=vb, p2=vc, radius=0, color=[0, 0, 1])
d.addLine(p1=va, p2=vc, radius=0, color=[0, 0, 1])
key = (str(surface.body1_name), str(surface.body2_name))
vis.showPolyData(
d.getPolyData(), str(key), parent=folder, color=[0, 0, 1])
def getMarkerGeometry(self):
if self.markerGeometry is None:
d = DebugData()
d.addSphere(np.zeros(3), radius=0.007, resolution=12)
self.markerGeometry = shallowCopy(d.getPolyData())
return self.markerGeometry
def _get_line(self, x, y, num, color):
data = DebugData()
data.addLine(x, y, radius=0.7, color=[1, 1, 1])
polydata = data.getPolyData()
self._add_polydata(polydata, "line" + str(num), color)
return polydata
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 updateGeometryFromProperties(self):
d = DebugData()
length = self.getProperty('Length')
d.addCapsule(center=(0, 0, 0),
axis=(0, 0, 1),
length=self.getProperty('Length'),
radius=self.getProperty('Radius'))
self.setPolyData(d.getPolyData())
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 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 updateGeometryFromProperties(self):
radius = self.getProperty('Radius')
circlePoints = np.linspace(0, 2*np.pi, self.getProperty('Segments')+1)
spokes = [(0.0, np.sin(x), np.cos(x)) for x in circlePoints]
spokes = [radius*np.array(x)/np.linalg.norm(x) for x in spokes]
d = DebugData()
for a, b in zip(spokes, spokes[1:]):
d.addCapsule(center=(a+b)/2.0, axis=(b-a), length=np.linalg.norm(b-a), radius=self.getProperty('Tube Radius'))
self.setPolyData(d.getPolyData())
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 loadFeet():
meshDir = os.path.join(app.getDRCBase(), 'software/models/atlas_v3/meshes')
meshes = []
for foot in ['l', 'r']:
d = DebugData()
d.addPolyData(io.readPolyData(os.path.join(meshDir, '%s_talus.stl' % foot)))
d.addPolyData(io.readPolyData(os.path.join(meshDir, '%s_foot.stl' % foot)))
meshes.append(d.getPolyData())
return meshes
def buildLineSegmentTestWorld(percentObsDensity, nonRandom=False, circleRadius=3, scale=1.0, randomSeed=5,
obstaclesInnerFraction=1.0):
#print "building circle world"
if nonRandom:
np.random.seed(randomSeed)
d = DebugData()
worldXmin = 3
worldXmax = 15
worldYmin = -10
worldYmax = 10
#print "boundaries done"
worldArea = (worldXmax-worldXmin)*(worldYmax-worldYmin)
#print worldArea
obsScalingFactor = 5.0/12.0
maxNumObstacles = obsScalingFactor * worldArea
numObstacles = int(obstaclesInnerFraction**2 * percentObsDensity/100.0 * maxNumObstacles)
print numObstacles, " is num obstacles"
# 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)
secondX = obsXmin + np.random.rand()*(obsXmax-obsXmin)
secondY = 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=0.5)
obj = vis.updatePolyData(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 onSpawnMesh(self):
d = DebugData()
d.addArrow((0,0,0), (0,0,0.3))
pd = d.getPolyData()
meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd)
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname='MeshAffordanceItem', Name='mesh', Filename=meshId, uuid=newUUID(), pose=pose)
return self.affordanceManager.newAffordanceFromDescription(desc)
