def setupStrings():
d = DebugData()
poles = [[-.36,.5,1.5],[-.36,-.5,1.5],[0,.5,1.5],[0,-.5,1.5],[.36,.5,1.5],[.36,-.5,1.5]]
for pole in poles:
d.addCylinder(pole, [0,0,1], 3, radius=0.021)
vis.updatePolyData(d.getPolyData(), 'poles')
d = DebugData()
strings = [
[-.36, .5, .09, 0, -.5, 1.77],
[-.36, .5, .74, -.36, -.5, .95],
[-.36, .5, 1.12, -.36, -.5, 1.68],
[-.36, .5, 1.33, .36, -.5, 2.29],
[-.36, .5, 1.6, .36, -.5, 1.62],
[-.36, .5, 1.74, .36, -.5, 1.93],
[-.36, .5, 2.15, -.36, -.5, 1.46],
[0, .5, .765, 0, -.5, .795],
[0, .5, 1.15, .36, -.5, 1.15],
[0, .5, 1.28, -.36, -.5, .11],
[0, .5, 1.42, 0, -.5, 1.42],
[0, .5, 1.78, .36, -.5, .12],
[0, .5, 2.05, -.36, -.5, 1.835],
[.36, .5, .8, -.36, -.5, 1.11],
[.36, .5, 1.16, -.36, -.5, 1.47],
[.36, .5, 1.61, .36, -.5, 1.19],
[.36, .5, 2.0, .36, -.5, 2.1],
[-.36, .3, 0, -.36, .3, 2.01],
[0, -.34, 0, 0, -.34, 1.42],
[.36, 0, 0, .36, 0, 2.05],
]
for string in strings:
p1 = string[:3]
p2 = string[3:]
d.addLine(p1,p2,radius=.001,color=[255,0,0])
vis.updatePolyData(d.getPolyData(), 'strings')
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 == 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 buildDonutWorld():
print "building donut world"
lineRadius = 0.2
hallwayWidth = 10
numSegments = 6
circleRadius = 50
d = DebugData()
angleList = np.linspace(0, 2 * np.pi, numSegments + 1)
for i in range(numSegments):
theta = angleList[i]
theta_next = angleList[i + 1]
for radius in [circleRadius, circleRadius + hallwayWidth]:
lineStart = [radius * np.cos(theta), radius * np.sin(theta), 0]
lineEnd = [
radius * np.cos(theta_next), radius * np.sin(theta_next), 0
]
d.addLine(lineStart, lineEnd, radius=lineRadius)
obj = vis.showPolyData(d.getPolyData(), 'world')
world = World()
world.visObj = obj
return world
def buildSimpleWorld():
print "this is a test"
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 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 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 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 __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 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 testFindWalls(self):
raycastDistance = self.sensor.raycastAllFromCurrentFrameLocation()
wallsFound = self.findWalls(raycastDistance, addNoise=True)
carTransform = om.findObjectByName('robot frame').transform
d = DebugData()
for wallData in wallsFound:
intercept = wallData['ransacFit'][0]
slope = wallData['ransacFit'][1]
wallDirectionInCarFrame = np.array([1.0, slope, 0.0])
wallPointInCarFrame = np.array([0.0, intercept, 0.0])
# now need to transform these to world frame in order to plot them.
wallPointWorldFrame = np.array(
carTransform.TransformPoint(wallPointInCarFrame))
wallDirectionWorldFrame = np.array(
carTransform.TransformVector(wallDirectionInCarFrame))
wallDirectionWorldFrame = 1 / np.linalg.norm(
wallDirectionWorldFrame) * wallDirectionWorldFrame
lineLength = 15.0
lineOrigin = wallPointWorldFrame - lineLength * wallDirectionWorldFrame
lineEnd = wallPointWorldFrame + lineLength * wallDirectionWorldFrame
d.addLine(lineOrigin, lineEnd, radius=0.3, color=[0, 0, 1])
vis.updatePolyData(d.getPolyData(),
'line estimate',
colorByName='RGB255')
return wallsFound
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 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 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 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 = [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:
d.addLine(points[0], points[-1])
self.annotationObj = vis.updatePolyData(d.getPolyData(),
self.annotationName,
parent=self.annotationFolder)
self.annotationObj.setProperty('Color', [1, 0, 0])
self.annotationObj.actor.SetPickable(False)
def main():
global app, view, nav_data
nav_data = np.array([[0, 0, 0]])
lcmUtils.addSubscriber(".*_NAV$", node_nav_t, handleNavData)
app = ConsoleApp()
app.setupGlobals(globals())
app.showPythonConsole()
view = app.createView()
view.show()
global d
d = DebugData()
d.addLine([0, 0, 0], [1, 0, 0], color=[0, 1, 0])
d.addSphere((0, 0, 0), radius=0.02, color=[1, 0, 0])
#vis.showPolyData(d.getPolyData(), 'my debug geometry', colorByName='RGB255')
startSwarmVisualization()
app.start()
def buildWarehouseWorld(percentObsDensity,
nonRandom=False,
circleRadius=0.1,
scale=None,
randomSeed=5,
obstaclesInnerFraction=1.0):
if nonRandom:
np.random.seed(randomSeed)
d = DebugData()
worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(
d, scale=scale, boundaryType="Warehouse")
numObstacles = 8
obsLength = 2.0
worldLength = worldXmax - worldXmin
print worldXmin
print worldXmax
obstacleZone = [
worldXmin + 0.2 * worldLength, worldXmax - 0.2 * worldLength
]
print obstacleZone
obstacleLength = obstacleZone[1] - obstacleZone[0]
incrementSize = obstacleLength * 1.0 / numObstacles
print incrementSize
leftOrRight = -1.0
for i in xrange(numObstacles):
firstX = obstacleZone[0] + incrementSize * i
leftOrRight = leftOrRight * -1.0
firstEndpt = (firstX, leftOrRight * worldYmax, 0.0)
secondEndpt = (firstX, 0.0, 0.0)
#d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn())
d.addLine(firstEndpt, secondEndpt, radius=circleRadius)
obj = vis.showPolyData(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 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 onNewWalkingGoal(self, walkingGoal=None):
walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel)
frameObj = vis.updateFrame(walkingGoal,
'walking goal',
parent='planning',
scale=0.25)
frameObj.setProperty('Edit', True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
if self.placer:
self.placer.stop()
terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
if terrain:
pos = np.array(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])
frameObj.transform.Translate(
pos - np.array(frameObj.transform.GetPosition()))
d = DebugData()
d.addSphere((0, 0, 0), radius=0.03)
handle = vis.showPolyData(d.getPolyData(),
'walking goal terrain handle',
parent=frameObj,
visible=True,
color=[1, 1, 0])
handle.actor.SetUserTransform(frameObj.transform)
self.placer = PlacerWidget(app.getCurrentRenderView(), handle,
terrain)
def onFramePropertyModified(propertySet, propertyName):
if propertyName == 'Edit':
if propertySet.getProperty(propertyName):
self.placer.start()
else:
self.placer.stop()
frameObj.properties.connectPropertyChanged(onFramePropertyModified)
onFramePropertyModified(frameObj, 'Edit')
frameObj.connectFrameModified(self.onWalkingGoalModified)
self.onWalkingGoalModified(frameObj)
def buildCircleWorld(percentObsDensity,
nonRandom=False,
circleRadius=3,
scale=None,
randomSeed=5,
obstaclesInnerFraction=1.0):
#print "building circle world"
if nonRandom:
np.random.seed(randomSeed)
d = DebugData()
worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(
d, scale=scale, boundaryType="Square")
#print "boundaries done"
worldArea = (worldXmax - worldXmin) * (worldYmax - worldYmin)
#print worldArea
obsScalingFactor = 1.0 / 12.0
maxNumObstacles = obsScalingFactor * worldArea
numObstacles = int(obstaclesInnerFraction**2 * percentObsDensity /
100.0 * maxNumObstacles)
#print numObstacles
# 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)
firstEndpt = (firstX, firstY, +0.2)
secondEndpt = (firstX, firstY, -0.2)
#d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn())
d.addLine(firstEndpt, secondEndpt, radius=circleRadius)
obj = vis.showPolyData(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 rayDebug(position, ray):
d = DebugData()
d.addLine(position, position + ray * 5.0)
drcView = app.getViewManager().findView('DRC View')
obj = vis.updatePolyData(d.getPolyData(),
'camera ray',
view=drcView,
color=[0, 1, 0])
obj.actor.GetProperty().SetLineWidth(2)
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, uuid=newUUID(), pose=((0.5,0.0,1.0), (1,0,0,0)))
return affordanceFromDescription(desc)
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 drawTargetPath(self):
path = DebugData()
for i in range(1,len(self.targetPath)):
p0 = self.targetPath[i-1].GetPosition()
p1 = self.targetPath[i].GetPosition()
path.addLine ( np.array( p0 ) , np.array( p1 ), radius= 0.005)
pathMesh = path.getPolyData()
self.targetPathMesh = vis.showPolyData(pathMesh, 'target frame desired path', color=[0.0, 0.3, 1.0], parent=self.targetAffordance, alpha=0.6)
self.targetPathMesh.actor.SetUserTransform(self.targetFrame)
def drawContactPts(self, obj, footstep, **kwargs):
if footstep.is_right_foot:
_, contact_pts = FootstepsDriver.getContactPts()
else:
contact_pts, _ = FootstepsDriver.getContactPts()
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 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.affordanceFromDescription(desc)
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 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 loadFootMeshes():
meshes = []
for i in range(0,2):
d = DebugData()
for footMeshFile in _footMeshFiles[i]:
d.addPolyData(ioUtils.readPolyData( footMeshFile , computeNormals=True))
t = vtk.vtkTransform()
t.Scale(0.98, 0.98, 0.98)
pd = filterUtils.transformPolyData(d.getPolyData(), t)
meshes.append(pd)
return meshes
