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 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 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 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 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 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 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 setupStrings():
d = DebugData()
poles = [[-0.36, 0.5, 1.5], [-0.36, -0.5, 1.5], [0, 0.5, 1.5], [0, -0.5, 1.5], [0.36, 0.5, 1.5], [0.36, -0.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 = [
[-0.36, 0.5, 0.09, 0, -0.5, 1.77],
[-0.36, 0.5, 0.74, -0.36, -0.5, 0.95],
[-0.36, 0.5, 1.12, -0.36, -0.5, 1.68],
[-0.36, 0.5, 1.33, 0.36, -0.5, 2.29],
[-0.36, 0.5, 1.6, 0.36, -0.5, 1.62],
[-0.36, 0.5, 1.74, 0.36, -0.5, 1.93],
[-0.36, 0.5, 2.15, -0.36, -0.5, 1.46],
[0, 0.5, 0.765, 0, -0.5, 0.795],
[0, 0.5, 1.15, 0.36, -0.5, 1.15],
[0, 0.5, 1.28, -0.36, -0.5, 0.11],
[0, 0.5, 1.42, 0, -0.5, 1.42],
[0, 0.5, 1.78, 0.36, -0.5, 0.12],
[0, 0.5, 2.05, -0.36, -0.5, 1.835],
[0.36, 0.5, 0.8, -0.36, -0.5, 1.11],
[0.36, 0.5, 1.16, -0.36, -0.5, 1.47],
[0.36, 0.5, 1.61, 0.36, -0.5, 1.19],
[0.36, 0.5, 2.0, 0.36, -0.5, 2.1],
[-0.36, 0.3, 0, -0.36, 0.3, 2.01],
[0, -0.34, 0, 0, -0.34, 1.42],
[0.36, 0, 0, 0.36, 0, 2.05],
]
for string in strings:
p1 = string[:3]
p2 = string[3:]
d.addLine(p1, p2, radius=0.001, color=[255, 0, 0])
vis.updatePolyData(d.getPolyData(), "strings")
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 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 = [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 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 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 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 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 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 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 updateCursor(self, displayPoint):
center = self.displayPointToImagePoint(displayPoint, restrictToImageDimensions=False)
center = np.array(center)
d = DebugData()
d.addLine(center + [0, -3000, 0], center + [0, 3000, 0])
d.addLine(center + [-3000, 0, 0], center + [3000, 0, 0])
self.cursorObj = vis.updatePolyData(d.getPolyData(), 'cursor', alpha=0.5, view=self.view)
self.cursorObj.addToView(self.view)
self.cursorObj.actor.SetUseBounds(False)
self.cursorObj.actor.SetPickable(False)
self.view.render()
def update(self):
t = self.frameProvider.getFrame('SCAN')
p1 = [0.0, 0.0, 0.0]
p2 = [2.0, 0.0, 0.0]
p1 = t.TransformPoint(p1)
p2 = t.TransformPoint(p2)
d = DebugData()
d.addSphere(p1, radius=0.01, color=[0,1,0])
d.addLine(p1, p2, color=[0,1,0])
self.setPolyData(d.getPolyData())
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)
#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 doFTDraw(self, unusedrobotstate):
frames = []
fts = []
vis_names = []
if (hasattr(self.robotStateJointController, 'lastRobotStateMessage') and
self.robotStateJointController.lastRobotStateMessage):
if self.draw_right:
rft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_force +
self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_torque)
rft[0] = -1.*rft[0] # right FT looks to be rotated 180deg around the z axis
rft[1] = -1.*rft[1]
rft[3] = -1.*rft[3]
rft[4] = -1.*rft[4]
rft -= self.ft_right_bias
fts.append(rft)
frames.append(self.robotStateModel.getLinkFrame('r_hand_force_torque'))
vis_names.append('ft_right')
if self.draw_left:
lft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_force +
self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_torque)
lft -= self.ft_left_bias
fts.append(lft)
frames.append(self.robotStateModel.getLinkFrame('l_hand_force_torque'))
vis_names.append('ft_left')
for i in range(0, len(frames)):
frame = frames[i]
ft = fts[i]
offset = [0., 0., 0.]
p1 = frame.TransformPoint(offset)
scale = 1.0/25.0 # todo add slider for this?
scalet = 1.0 / 2.5
p2f = frame.TransformPoint(offset + ft[0:3]*scale)
p2t = frame.TransformPoint(offset + ft[3:6])
normalt = (np.array(p2t) - np.array(p1))
normalt = normalt / np.linalg.norm(normalt)
d = DebugData()
# force
if np.linalg.norm(np.array(p2f) - np.array(p1)) < 0.1:
d.addLine(p1, p2f, color=[1.0, 0.0, 0.0])
else:
d.addArrow(p1, p2f, color=[1.,0.,0.])
# torque
if self.draw_torque:
d.addCircle(p1, normalt, scalet*np.linalg.norm(ft[3:6]))
# frame (largely for debug)
vis.updateFrame(frame, vis_names[i]+'frame', view=self.view, parent='wristft', visible=False, scale=0.2)
vis.updatePolyData(d.getPolyData(), vis_names[i], view=self.view, parent='wristft')
def updateCursor(self, displayPoint):
center = self.displayPointToImagePoint(displayPoint,
restrictToImageDimensions=False)
center = np.array(center)
d = DebugData()
d.addLine(center + [0, -3000, 0], center + [0, 3000, 0])
d.addLine(center + [-3000, 0, 0], center + [3000, 0, 0])
self.cursorObj = vis.updatePolyData(d.getPolyData(),
'cursor',
alpha=0.5,
view=self.view)
self.cursorObj.addToView(self.view)
self.cursorObj.actor.SetUseBounds(False)
self.cursorObj.actor.SetPickable(False)
self.view.render()
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 updateDrawIntersection(frame):
origin = np.array(frame.transform.GetPosition())
#print "origin is now at", origin
d = DebugData()
for i in xrange(numRays):
ray = rays[:,i]
rayTransformed = np.array(frame.transform.TransformNormal(ray))
#print "rayTransformed is", rayTransformed
intersection = computeIntersection(locator, origin, origin + rayTransformed*rayLength)
if intersection is not None:
d.addLine(origin, intersection, color=[1,0,0])
else:
d.addLine(origin, origin+rayTransformed*rayLength, color=[0,1,0])
vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
def buildStickWorld(percentObsDensity):
print "building stick world"
d = DebugData()
worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d)
print "boundaries done"
worldArea = (worldXmax-worldXmin)*(worldYmax-worldYmin)
print worldArea
obsScalingFactor = 1.0/12.0
maxNumObstacles = obsScalingFactor * worldArea
numObstacles = int(percentObsDensity/100.0 * maxNumObstacles)
print numObstacles
# draw random stick obstacles
obsLength = 2.0
for i in xrange(numObstacles):
firstX = worldXmin + np.random.rand()*(worldXmax-worldXmin)
firstY = worldYmin + np.random.rand()*(worldYmax-worldYmin)
firstEndpt = (firstX,firstY,0)
randTheta = np.random.rand() * 2.0*np.pi
secondEndpt = (firstX+obsLength*np.cos(randTheta), firstY+obsLength*np.sin(randTheta), 0)
d.addLine(firstEndpt, secondEndpt, radius=0.2)
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 updateDrawIntersection(self, frame):
origin = np.array(frame.transform.GetPosition())
#print "origin is now at", origin
d = DebugData()
colorMaxRange = [0,1,0]
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 buildStickWorld(percentObsDensity):
print "building stick world"
d = DebugData()
worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d)
print "boundaries done"
worldArea = (worldXmax - worldXmin) * (worldYmax - worldYmin)
print worldArea
obsScalingFactor = 1.0 / 12.0
maxNumObstacles = obsScalingFactor * worldArea
numObstacles = int(percentObsDensity / 100.0 * maxNumObstacles)
print numObstacles
# draw random stick obstacles
obsLength = 2.0
for i in xrange(numObstacles):
firstX = worldXmin + np.random.rand() * (worldXmax - worldXmin)
firstY = worldYmin + np.random.rand() * (worldYmax - worldYmin)
firstEndpt = (firstX, firstY, 0)
randTheta = np.random.rand() * 2.0 * np.pi
secondEndpt = (firstX + obsLength * np.cos(randTheta),
firstY + obsLength * np.sin(randTheta), 0)
d.addLine(firstEndpt, secondEndpt, radius=0.2)
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 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 = 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
def buildHallwayWorld():
print "building hallway world"
lineRadius = 0.2
d = DebugData()
xMin = -10
xMax = 10
yMin = 0
yMax = 50
for x in [xMin, xMax]:
lineStart = (x, yMin, 0)
lineEnd = (x, yMax, 0)
d.addLine(lineStart, lineEnd, radius=lineRadius)
obj = vis.showPolyData(d.getPolyData(), 'world')
world = World()
world.visObj = obj
return world
def updateDrawIntersection(frame):
origin = np.array(frame.transform.GetPosition())
#print "origin is now at", origin
d = DebugData()
for i in xrange(numRays):
ray = rays[:, i]
rayTransformed = np.array(frame.transform.TransformNormal(ray))
#print "rayTransformed is", rayTransformed
intersection = computeIntersection(locator, origin,
origin + rayTransformed * rayLength)
if intersection is not None:
d.addLine(origin, intersection, color=[1, 0, 0])
else:
d.addLine(origin,
origin + rayTransformed * rayLength,
color=[0, 1, 0])
vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
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 = 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
def convertStepToSafeRegion(self, step, rpySeed):
assert step.shape[0] >= 3
assert step.shape[1] == 3
shapeVertices = np.array(step).transpose()[:2,:]
s = ddapp.terrain.PolygonSegmentationNonIRIS(shapeVertices, bot_pts=self.footContactPoints)
stepCenter = np.mean(step, axis=0)
startSeed = np.hstack([stepCenter, rpySeed])
r = s.findSafeRegion(startSeed)
if r is not None:
# draw step
d = DebugData()
for p1, p2 in zip(step, step[1:]):
d.addLine(p1, p2)
d.addLine(step[-1], step[0])
folder = om.getOrCreateContainer('Safe terrain regions')
obj = vis.showPolyData(d.getPolyData(), 'step region %d' % len(folder.children()), parent=folder)
obj.properties.addProperty('Enabled for Walking', True)
obj.safe_region = r
def getCameraFrustumGeometry(self, rayLength):
camPos, rays = self.getCameraFrustumRays()
rays = [rayLength*r for r in rays]
d = DebugData()
d.addLine(camPos, camPos+rays[0])
d.addLine(camPos, camPos+rays[1])
d.addLine(camPos, camPos+rays[2])
d.addLine(camPos, camPos+rays[3])
d.addLine(camPos+rays[0], camPos+rays[1])
d.addLine(camPos+rays[1], camPos+rays[2])
d.addLine(camPos+rays[2], camPos+rays[3])
d.addLine(camPos+rays[3], camPos+rays[0])
return d.getPolyData()
def getCameraFrustumGeometry(self, rayLength):
camPos, rays = self.getCameraFrustumRays()
rays = [rayLength * r for r in rays]
d = DebugData()
d.addLine(camPos, camPos + rays[0])
d.addLine(camPos, camPos + rays[1])
d.addLine(camPos, camPos + rays[2])
d.addLine(camPos, camPos + rays[3])
d.addLine(camPos + rays[0], camPos + rays[1])
d.addLine(camPos + rays[1], camPos + rays[2])
d.addLine(camPos + rays[2], camPos + rays[3])
d.addLine(camPos + rays[3], camPos + rays[0])
return d.getPolyData()
def convertStepToSafeRegion(self, step, rpySeed):
assert step.shape[0] >= 3
assert step.shape[1] == 3
shapeVertices = np.array(step).transpose()[:2,:]
s = ddapp.terrain.PolygonSegmentationNonIRIS(shapeVertices, bot_pts=self.footContactPoints)
stepCenter = np.mean(step, axis=0)
startSeed = np.hstack([stepCenter, rpySeed])
r = s.findSafeRegion(startSeed)
if r is not None:
# draw step
d = DebugData()
for p1, p2 in zip(step, step[1:]):
d.addLine(p1, p2)
d.addLine(step[-1], step[0])
folder = om.getOrCreateContainer('Safe terrain regions')
obj = vis.showPolyData(d.getPolyData(), 'step region %d' % len(folder.children()), parent=folder)
obj.properties.addProperty('Enabled for Walking', True)
obj.safe_region = r
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 spawnTargetAffordance(self):
for obj in om.getObjects():
if obj.getProperty('Name') == 'target':
om.removeFromObjectModel(obj)
targetFrame = transformUtils.frameFromPositionAndRPY([0.6, 0.2, 0.6],
[180, 0, 90])
folder = om.getOrCreateContainer('affordances')
z = DebugData()
z.addLine(np.array([0, 0, 0]),
np.array([-self.boxLength, 0, 0]),
radius=0.02) # main bar
z.addLine(np.array([-self.boxLength, 0, 0]),
np.array([-self.boxLength, 0, self.boxLength]),
radius=0.02) # main bar
z.addLine(np.array([-self.boxLength, 0, self.boxLength]),
np.array([0, 0, self.boxLength]),
radius=0.02) # main bar
z.addLine(np.array([0, 0, self.boxLength]),
np.array([0, 0, 0]),
radius=0.02) # main bar
targetMesh = z.getPolyData()
self.targetAffordance = vis.showPolyData(
targetMesh,
'target',
color=[0.0, 1.0, 0.0],
cls=affordanceitems.FrameAffordanceItem,
parent=folder,
alpha=0.3)
self.targetAffordance.actor.SetUserTransform(targetFrame)
self.targetFrame = vis.showFrame(targetFrame,
'target frame',
parent=self.targetAffordance,
visible=False,
scale=0.2)
self.targetFrame = self.targetFrame.transform
params = dict(length=self.boxLength,
otdf_type='target',
friendly_name='target')
self.targetAffordance.setAffordanceParams(params)
self.targetAffordance.updateParamsFromActorTransform()
def spawnTargetAffordance(self):
for obj in om.getObjects():
if obj.getProperty('Name') == 'target':
om.removeFromObjectModel(obj)
targetFrame = transformUtils.frameFromPositionAndRPY([0.6,0.2,0.6],[180,0,90])
folder = om.getOrCreateContainer('affordances')
z = DebugData()
z.addLine(np.array([0,0,0]), np.array([-self.boxLength,0,0]), radius=0.02) # main bar
z.addLine(np.array([-self.boxLength,0,0]), np.array([-self.boxLength,0,self.boxLength]), radius=0.02) # main bar
z.addLine(np.array([-self.boxLength,0,self.boxLength]), np.array([0,0,self.boxLength]), radius=0.02) # main bar
z.addLine(np.array([0,0,self.boxLength]), np.array([0,0,0]), radius=0.02) # main bar
targetMesh = z.getPolyData()
self.targetAffordance = vis.showPolyData(targetMesh, 'target', color=[0.0, 1.0, 0.0], cls=affordanceitems.FrameAffordanceItem, parent=folder, alpha=0.3)
self.targetAffordance.actor.SetUserTransform(targetFrame)
self.targetFrame = vis.showFrame(targetFrame, 'target frame', parent=self.targetAffordance, visible=False, scale=0.2)
self.targetFrame = self.targetFrame.transform
params = dict(length=self.boxLength, otdf_type='target', friendly_name='target')
self.targetAffordance.setAffordanceParams(params)
self.targetAffordance.updateParamsFromActorTransform()
def fitRunningBoardAtFeet(self):
# get stance frame
startPose = self.getPlanningStartPose()
stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(
self.robotSystem.robotStateModel, useWorldZ=False)
stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame)
# get pointcloud and extract search region covering the running board
polyData = segmentation.getCurrentRevolutionData()
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
_, polyData = segmentation.removeGround(polyData)
polyData = segmentation.cropToBox(polyData, stanceFrame,
[1.0, 1.0, 0.1])
if not polyData.GetNumberOfPoints():
print 'empty search region point cloud'
return
vis.updatePolyData(polyData,
'running board search points',
parent=segmentation.getDebugFolder(),
color=[0, 1, 0],
visible=False)
# extract maximal points along the stance x axis
perpAxis = stanceFrameAxes[0]
edgeAxis = stanceFrameAxes[1]
edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis)
edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints)
vis.updatePolyData(edgePoints,
'edge points',
parent=segmentation.getDebugFolder(),
visible=True)
# ransac fit a line to the edge points
linePoint, lineDirection, fitPoints = segmentation.applyLineFit(
edgePoints)
if np.dot(lineDirection, stanceFrameAxes[1]) < 0:
lineDirection = -lineDirection
linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels',
[1.0, 1.0])
dists = np.dot(
vnp.getNumpyFromVtk(linePoints, 'Points') - linePoint,
lineDirection)
p1 = linePoint + lineDirection * np.min(dists)
p2 = linePoint + lineDirection * np.max(dists)
vis.updatePolyData(fitPoints,
'line fit points',
parent=segmentation.getDebugFolder(),
colorByName='ransac_labels',
visible=False)
# compute a new frame that is in plane with the stance frame
# and matches the orientation and position of the detected edge
origin = np.array(stanceFrame.GetPosition())
normal = np.array(stanceFrameAxes[2])
# project stance origin to edge, then back to foot frame
originProjectedToEdge = linePoint + lineDirection * np.dot(
origin - linePoint, lineDirection)
originProjectedToPlane = segmentation.projectPointToPlane(
originProjectedToEdge, origin, normal)
zaxis = np.array(stanceFrameAxes[2])
yaxis = np.array(lineDirection)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
d = DebugData()
d.addSphere(p1, radius=0.005)
d.addSphere(p2, radius=0.005)
d.addLine(p1, p2)
d.addSphere(originProjectedToEdge, radius=0.001, color=[1, 0, 0])
d.addSphere(originProjectedToPlane, radius=0.001, color=[0, 1, 0])
d.addLine(originProjectedToPlane, origin, color=[0, 1, 0])
d.addLine(originProjectedToEdge, origin, color=[1, 0, 0])
vis.updatePolyData(d.getPolyData(),
'running board edge',
parent=segmentation.getDebugFolder(),
colorByName='RGB255',
visible=False)
# update the running board box affordance position and orientation to
# fit the detected edge
box = self.spawnRunningBoardAffordance()
boxDimensions = box.getProperty('Dimensions')
t = transformUtils.getTransformFromAxesAndOrigin(
xaxis, yaxis, zaxis, originProjectedToPlane)
t.PreMultiply()
t.Translate(-boxDimensions[0] / 2.0, 0.0, -boxDimensions[2] / 2.0)
box.getChildFrame().copyFrame(t)
self.initialize()
def drawFootstepPlan(self, msg, folder, left_color=None, right_color=None, alpha=1.0):
for step in folder.children():
om.removeFromObjectModel(step)
allTransforms = []
volFolder = getWalkingVolumesFolder()
map(om.removeFromObjectModel, volFolder.children())
slicesFolder = getTerrainSlicesFolder()
map(om.removeFromObjectModel, slicesFolder.children())
for i, footstep in enumerate(msg.footsteps):
trans = footstep.pos.translation
trans = [trans.x, trans.y, trans.z]
quat = footstep.pos.rotation
quat = [quat.w, quat.x, quat.y, quat.z]
footstepTransform = transformUtils.transformFromPose(trans, quat)
allTransforms.append(footstepTransform)
if i < 2:
continue
if footstep.is_right_foot:
mesh = getRightFootMesh()
if (right_color is None):
color = getRightFootColor()
else:
color = right_color
else:
mesh = getLeftFootMesh()
if (left_color is None):
color = getLeftFootColor()
else:
color = left_color
# add gradual shading to steps to indicate destination
frac = float(i)/ float(msg.num_steps-1)
this_color = [0,0,0]
this_color[0] = 0.25*color[0] + 0.75*frac*color[0]
this_color[1] = 0.25*color[1] + 0.75*frac*color[1]
this_color[2] = 0.25*color[2] + 0.75*frac*color[2]
if self.show_contact_slices:
self.drawContactVolumes(footstepTransform, color)
contact_pts_left, contact_pts_right = FootstepsDriver.getContactPts()
if footstep.is_right_foot:
sole_offset = np.mean(contact_pts_right, axis=0)
else:
sole_offset = np.mean(contact_pts_left, axis=0)
t_sole_prev = transformUtils.frameFromPositionMessage(msg.footsteps[i-2].pos)
t_sole_prev.PreMultiply()
t_sole_prev.Translate(sole_offset)
t_sole = transformUtils.copyFrame(footstepTransform)
t_sole.Translate(sole_offset)
yaw = np.arctan2(t_sole.GetPosition()[1] - t_sole_prev.GetPosition()[1],
t_sole.GetPosition()[0] - t_sole_prev.GetPosition()[0])
T_terrain_to_world = transformUtils.frameFromPositionAndRPY([t_sole_prev.GetPosition()[0], t_sole_prev.GetPosition()[1], 0],
[0, 0, math.degrees(yaw)])
path_dist = np.array(footstep.terrain_path_dist)
height = np.array(footstep.terrain_height)
# if np.any(height >= trans[2]):
terrain_pts_in_local = np.vstack((path_dist, np.zeros(len(footstep.terrain_path_dist)), height))
d = DebugData()
for j in range(terrain_pts_in_local.shape[1]-1):
d.addLine(terrain_pts_in_local[:,j], terrain_pts_in_local[:,j+1], radius=0.01)
obj = vis.showPolyData(d.getPolyData(), 'terrain slice', parent=slicesFolder, visible=slicesFolder.getProperty('Visible'), color=[.8,.8,.3])
obj.actor.SetUserTransform(T_terrain_to_world)
renderInfeasibility = False
if renderInfeasibility and footstep.infeasibility > 1e-6:
d = DebugData()
start = allTransforms[i-1].GetPosition()
end = footstepTransform.GetPosition()
d.addArrow(start, end, 0.02, 0.005,
startHead=True,
endHead=True)
vis.showPolyData(d.getPolyData(), 'infeasibility %d -> %d' % (i-2, i-1), parent=folder, color=[1, 0.2, 0.2])
stepName = 'step %d' % (i-1)
obj = vis.showPolyData(mesh, stepName, color=this_color, alpha=alpha, parent=folder)
obj.setIcon(om.Icons.Feet)
frameObj = vis.showFrame(footstepTransform, stepName + ' frame', parent=obj, scale=0.3, visible=False)
obj.actor.SetUserTransform(footstepTransform)
obj.addProperty('Support Contact Groups', footstep.params.support_contact_groups, attributes=om.PropertyAttributes(enumNames=['Whole Foot', 'Front 2/3', 'Back 2/3']))
obj.properties.setPropertyIndex('Support Contact Groups', 0)
obj.footstep_index = i
obj.footstep_property_callback = obj.properties.connectPropertyChanged(functools.partial(self.onFootstepPropertyChanged, obj))
self.drawContactPts(obj, footstep, color=this_color)
def fitRunningBoardAtFeet(self):
# get stance frame
startPose = self.getPlanningStartPose()
stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(self.robotSystem.robotStateModel, useWorldZ=False)
stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame)
# get pointcloud and extract search region covering the running board
polyData = segmentation.getCurrentRevolutionData()
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
_, polyData = segmentation.removeGround(polyData)
polyData = segmentation.cropToBox(polyData, stanceFrame, [1.0, 1.0, 0.1])
if not polyData.GetNumberOfPoints():
print 'empty search region point cloud'
return
vis.updatePolyData(polyData, 'running board search points', parent=segmentation.getDebugFolder(), color=[0,1,0], visible=False)
# extract maximal points along the stance x axis
perpAxis = stanceFrameAxes[0]
edgeAxis = stanceFrameAxes[1]
edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis)
edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints)
vis.updatePolyData(edgePoints, 'edge points', parent=segmentation.getDebugFolder(), visible=True)
# ransac fit a line to the edge points
linePoint, lineDirection, fitPoints = segmentation.applyLineFit(edgePoints)
if np.dot(lineDirection, stanceFrameAxes[1]) < 0:
lineDirection = -lineDirection
linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels', [1.0, 1.0])
dists = np.dot(vnp.getNumpyFromVtk(linePoints, 'Points')-linePoint, lineDirection)
p1 = linePoint + lineDirection*np.min(dists)
p2 = linePoint + lineDirection*np.max(dists)
vis.updatePolyData(fitPoints, 'line fit points', parent=segmentation.getDebugFolder(), colorByName='ransac_labels', visible=False)
# compute a new frame that is in plane with the stance frame
# and matches the orientation and position of the detected edge
origin = np.array(stanceFrame.GetPosition())
normal = np.array(stanceFrameAxes[2])
# project stance origin to edge, then back to foot frame
originProjectedToEdge = linePoint + lineDirection*np.dot(origin - linePoint, lineDirection)
originProjectedToPlane = segmentation.projectPointToPlane(originProjectedToEdge, origin, normal)
zaxis = np.array(stanceFrameAxes[2])
yaxis = np.array(lineDirection)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
d = DebugData()
d.addSphere(p1, radius=0.005)
d.addSphere(p2, radius=0.005)
d.addLine(p1, p2)
d.addSphere(originProjectedToEdge, radius=0.001, color=[1,0,0])
d.addSphere(originProjectedToPlane, radius=0.001, color=[0,1,0])
d.addLine(originProjectedToPlane, origin, color=[0,1,0])
d.addLine(originProjectedToEdge, origin, color=[1,0,0])
vis.updatePolyData(d.getPolyData(), 'running board edge', parent=segmentation.getDebugFolder(), colorByName='RGB255', visible=False)
# update the running board box affordance position and orientation to
# fit the detected edge
box = self.spawnRunningBoardAffordance()
boxDimensions = box.getProperty('Dimensions')
t = transformUtils.getTransformFromAxesAndOrigin(xaxis, yaxis, zaxis, originProjectedToPlane)
t.PreMultiply()
t.Translate(-boxDimensions[0]/2.0, 0.0, -boxDimensions[2]/2.0)
box.getChildFrame().copyFrame(t)
self.initialize()
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 drawFootstepPlan(self, msg, folder, left_color=None, right_color=None, alpha=1.0):
for step in folder.children():
om.removeFromObjectModel(step)
allTransforms = []
volFolder = getWalkingVolumesFolder()
map(om.removeFromObjectModel, volFolder.children())
slicesFolder = getTerrainSlicesFolder()
map(om.removeFromObjectModel, slicesFolder.children())
for i, footstep in enumerate(msg.footsteps):
trans = footstep.pos.translation
trans = [trans.x, trans.y, trans.z]
quat = footstep.pos.rotation
quat = [quat.w, quat.x, quat.y, quat.z]
footstepTransform = transformUtils.transformFromPose(trans, quat)
allTransforms.append(footstepTransform)
if i < 2:
continue
if footstep.is_right_foot:
mesh = getRightFootMesh()
if (right_color is None):
color = getRightFootColor()
else:
color = right_color
else:
mesh = getLeftFootMesh()
if (left_color is None):
color = getLeftFootColor()
else:
color = left_color
# add gradual shading to steps to indicate destination
frac = float(i)/ float(msg.num_steps-1)
this_color = [0,0,0]
this_color[0] = 0.25*color[0] + 0.75*frac*color[0]
this_color[1] = 0.25*color[1] + 0.75*frac*color[1]
this_color[2] = 0.25*color[2] + 0.75*frac*color[2]
if self.show_contact_slices:
self.drawContactVolumes(footstepTransform, color)
contact_pts_left, contact_pts_right = FootstepsDriver.getContactPts()
if footstep.is_right_foot:
sole_offset = np.mean(contact_pts_right, axis=0)
else:
sole_offset = np.mean(contact_pts_left, axis=0)
t_sole_prev = transformUtils.frameFromPositionMessage(msg.footsteps[i-2].pos)
t_sole_prev.PreMultiply()
t_sole_prev.Translate(sole_offset)
t_sole = transformUtils.copyFrame(footstepTransform)
t_sole.Translate(sole_offset)
yaw = np.arctan2(t_sole.GetPosition()[1] - t_sole_prev.GetPosition()[1],
t_sole.GetPosition()[0] - t_sole_prev.GetPosition()[0])
T_terrain_to_world = transformUtils.frameFromPositionAndRPY([t_sole_prev.GetPosition()[0], t_sole_prev.GetPosition()[1], 0],
[0, 0, math.degrees(yaw)])
path_dist = np.array(footstep.terrain_path_dist)
height = np.array(footstep.terrain_height)
# if np.any(height >= trans[2]):
terrain_pts_in_local = np.vstack((path_dist, np.zeros(len(footstep.terrain_path_dist)), height))
d = DebugData()
for j in range(terrain_pts_in_local.shape[1]-1):
d.addLine(terrain_pts_in_local[:,j], terrain_pts_in_local[:,j+1], radius=0.01)
obj = vis.showPolyData(d.getPolyData(), 'terrain slice', parent=slicesFolder, visible=slicesFolder.getProperty('Visible'), color=[.8,.8,.3])
obj.actor.SetUserTransform(T_terrain_to_world)
renderInfeasibility = False
if renderInfeasibility and footstep.infeasibility > 1e-6:
d = DebugData()
start = allTransforms[i-1].GetPosition()
end = footstepTransform.GetPosition()
d.addArrow(start, end, 0.02, 0.005,
startHead=True,
endHead=True)
vis.showPolyData(d.getPolyData(), 'infeasibility %d -> %d' % (i-2, i-1), parent=folder, color=[1, 0.2, 0.2])
stepName = 'step %d' % (i-1)
obj = vis.showPolyData(mesh, stepName, color=this_color, alpha=alpha, parent=folder)
obj.setIcon(om.Icons.Feet)
frameObj = vis.showFrame(footstepTransform, stepName + ' frame', parent=obj, scale=0.3, visible=False)
obj.actor.SetUserTransform(footstepTransform)
obj.addProperty('Support Contact Groups', footstep.params.support_contact_groups, attributes=om.PropertyAttributes(enumNames=['Whole Foot', 'Front 2/3', 'Back 2/3']))
obj.properties.setPropertyIndex('Support Contact Groups', 0)
obj.footstep_index = i
obj.footstep_property_callback = obj.properties.connectPropertyChanged(functools.partial(self.onFootstepPropertyChanged, obj))
self.drawContactPts(obj, footstep, color=this_color)
