def processSnippet():
obj = om.getOrCreateContainer('continuous')
om.getOrCreateContainer('cont debug', obj)
if (continuouswalkingDemo.processContinuousStereo):
polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/block_snippet_stereo.vtp'))
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
else:
polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/block_snippet.vtp'))
vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='continuous')
standingFootName = 'l_foot'
standingFootFrame = robotStateModel.getLinkFrame(standingFootName)
vis.updateFrame(standingFootFrame, standingFootName, parent='continuous', visible=False)
# Step 2: find all the surfaces in front of the robot (about 0.75sec)
clusters = segmentation.findHorizontalSurfaces(polyData)
if (clusters is None):
print "No cluster found, stop walking now!"
return
# Step 3: find the corners of the minimum bounding rectangles
blocks,groundPlane = cwdemo.extractBlocksFromSurfaces(clusters, standingFootFrame)
footsteps = cwdemo.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame)
cwdemo.drawFittedSteps(footsteps)
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 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 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 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 findFarRightCorner(self, polyData, linkFrame):
'''
Within a point cloud find the point to the far right from the link
The input is typically the 4 corners of a minimum bounding box
'''
diagonalTransform = transformUtils.frameFromPositionAndRPY([0,0,0], [0,0,45])
diagonalTransform.Concatenate(linkFrame)
vis.updateFrame(diagonalTransform, 'diagonal frame', parent='cont debug', visible=False)
#polyData = shallowCopy(polyData)
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
#vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x')
#vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y')
#vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z')
viewOrigin = diagonalTransform.TransformPoint([0.0, 0.0, 0.0])
viewX = diagonalTransform.TransformVector([1.0, 0.0, 0.0])
viewY = diagonalTransform.TransformVector([0.0, 1.0, 0.0])
viewZ = diagonalTransform.TransformVector([0.0, 0.0, 1.0])
#polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x')
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y')
#polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z')
vis.updatePolyData( polyData, 'cornerPoints', parent='cont debug', visible=False)
farRightIndex = vtkNumpy.getNumpyFromVtk(polyData, 'distance_along_foot_y').argmin()
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
return points[farRightIndex,:]
def findFarRightCorner(self, polyData, linkFrame):
'''
Within a point cloud find the point to the far right from the link
The input is typically the 4 corners of a minimum bounding box
'''
diagonalTransform = transformUtils.frameFromPositionAndRPY([0,0,0], [0,0,45])
diagonalTransform.Concatenate(linkFrame)
vis.updateFrame(diagonalTransform, 'diagonal frame', parent='cont debug', visible=False)
#polyData = shallowCopy(polyData)
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
#vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x')
#vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y')
#vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z')
viewOrigin = diagonalTransform.TransformPoint([0.0, 0.0, 0.0])
viewX = diagonalTransform.TransformVector([1.0, 0.0, 0.0])
viewY = diagonalTransform.TransformVector([0.0, 1.0, 0.0])
viewZ = diagonalTransform.TransformVector([0.0, 0.0, 1.0])
#polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x')
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y')
#polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z')
vis.updatePolyData( polyData, 'cornerPoints', parent='cont debug', visible=False)
farRightIndex = vtkNumpy.getNumpyFromVtk(polyData, 'distance_along_foot_y').argmin()
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
return points[farRightIndex,:]
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 __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 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 update(self, robotModel):
name = 'camera frustum %s' % self.robotModel.getProperty('Name')
obj = om.findObjectByName(name)
if obj and not obj.getProperty('Visible'):
return
vis.updatePolyData(self.getCameraFrustumGeometry(self.rayLength), name, parent=self.robotModel, visible=False)
def updatePointcloudSnapshot(self):
if (self.useLidar is True):
return vis.updatePolyData(segmentation.getCurrentRevolutionData(),
'pointcloud snapshot', parent='segmentation')
else:
return vis.updatePolyData(segmentation.getDisparityPointCloud(4),
'pointcloud snapshot', parent='segmentation')
def updatePointcloudSnapshot(self):
if (self.useLidar is True):
return vis.updatePolyData(segmentation.getCurrentRevolutionData(),
'pointcloud snapshot',
parent='segmentation')
else:
return vis.updatePolyData(segmentation.getDisparityPointCloud(4),
'pointcloud snapshot',
parent='segmentation')
def update(self, robotModel):
name = 'camera frustum %s' % self.robotModel.getProperty('Name')
obj = om.findObjectByName(name)
if obj and not obj.getProperty('Visible'):
return
vis.updatePolyData(self.getCameraFrustumGeometry(self.rayLength),
name,
parent=self.robotModel,
visible=False)
def createSpheres(self, sensorValues):
d = DebugData()
for key in sensorValues.keys():
frame, pos, rpy = self.sensorLocations[key]
t = transformUtils.frameFromPositionAndRPY(pos, rpy)
t.PostMultiply()
t.Concatenate(self.frames[frame])
d.addSphere(t.GetPosition(),
radius=0.005,
color=self.getColor(sensorValues[key], key),
resolution=8)
vis.updatePolyData(d.getPolyData(), self.name, colorByName='RGB255')
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 fitSwitchBox(self, polyData, points):
boxPosition = points[0]
wallPoint = points[1]
# find a frame that is aligned with wall
searchRadius = 0.2
planePoints, normal = segmentation.applyLocalPlaneFit(polyData, points[0], searchRadius=np.linalg.norm(points[1] - points[0]), searchRadiusEnd=1.0)
obj = vis.updatePolyData(planePoints, 'wall plane points', color=[0,1,0], visible=False)
obj.setProperty('Point Size', 7)
viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection()
if np.dot(normal, viewDirection) < 0:
normal = -normal
origin = segmentation.computeCentroid(planePoints)
zaxis = [0,0,1]
xaxis = normal
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
zaxis = np.cross(xaxis, yaxis)
zaxis /= np.linalg.norm(zaxis)
t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
# translate that frame to the box position
t.PostMultiply()
t.Translate(boxPosition)
boxFrame = transformUtils.copyFrame(t)
self.switchPlanner.spawnBoxAffordanceAtFrame(boxFrame)
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 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 fitSwitchBox(self, polyData, points):
boxPosition = points[0]
wallPoint = points[1]
# find a frame that is aligned with wall
searchRadius = 0.2
planePoints, normal = segmentation.applyLocalPlaneFit(polyData, points[0], searchRadius=np.linalg.norm(points[1] - points[0]), searchRadiusEnd=1.0)
obj = vis.updatePolyData(planePoints, 'wall plane points', color=[0,1,0], visible=False)
obj.setProperty('Point Size', 7)
viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection()
if np.dot(normal, viewDirection) < 0:
normal = -normal
origin = segmentation.computeCentroid(planePoints)
zaxis = [0,0,1]
xaxis = normal
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
zaxis = np.cross(xaxis, yaxis)
zaxis /= np.linalg.norm(zaxis)
t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
# translate that frame to the box position
t.PostMultiply()
t.Translate(boxPosition)
boxFrame = transformUtils.copyFrame(t)
self.switchPlanner.spawnBoxAffordanceAtFrame(boxFrame)
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 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 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 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 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 setupObjectInCamera(self, obj):
imageView = self.imageView
obj = vis.updatePolyData(vtk.vtkPolyData(),
self.getTransformedName(obj),
view=imageView.view,
color=obj.getProperty('Color'),
parent=self.getFolderName(),
visible=obj.getProperty('Visible'))
self.addActorToImageOverlay(obj, imageView)
return obj
def loadMap(self, filename, transform=None):
print filename
pd = io.readPolyData(filename)
if transform is not None:
pd = filterUtils.transformPolyData(pd, self.cameraToLocalInit)
pdi = vis.updatePolyData(pd, 'map')
try:
pdi.setProperty('Color By', 'rgb_colors')
except Exception, e:
print "Could not set color to RGB - not an element" #raise e
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 onShowMapButton(self):
vis.updateFrame(self.cameraToLocalInit, 'initial cam' )
filename = os.path.expanduser('~/Kinect_Logs/Kintinuous.pcd')
print filename
pd = io.readPolyData(filename)
pd = filterUtils.transformPolyData(pd, self.cameraToLocalInit )
#t = transformUtils.frameFromPositionAndRPY([0,0,0],[-90,0,-90])
#pd = filterUtils.transformPolyData(pd , t)
pdi = vis.updatePolyData(pd,'map')
pdi.setProperty('Color By', 'rgb_colors')
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 removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[-np.inf, -0.03], whichAxis=1, whichAxisLetter='y', percentile = 95):
yvalues = vnp.getNumpyFromVtk(polyData, 'Points')[:, whichAxis]
backY = np.percentile(yvalues, percentile)
if ( percentile > 50):
searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [backY - 0.1, np.inf])
else:
searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [-np.inf, backY + 0.1])
vis.updatePolyData(searchRegion, 'search region', parent="segmentation", colorByName=whichAxisLetter, visible=False)
# find the plane of the back wall, remove it and the points behind it:
_, origin, normal = segmentation.applyPlaneFit(searchRegion, distanceThreshold=0.02, expectedNormal=expectedNormal, perpendicularAxis=expectedNormal, returnOrigin=True)
points = vnp.getNumpyFromVtk(polyData, 'Points')
dist = np.dot(points - origin, normal)
vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')
backFrame = transformUtils.getTransformFromOriginAndNormal(origin, normal, normalAxis=2)
vis.updateFrame(backFrame, 'back frame', parent='segmentation', scale=0.15 , visible=False)
vis.updatePolyData(polyData, 'dist to back', parent='segmentation', visible=False)
polyData = segmentation.thresholdPoints(polyData, 'dist_to_plane', filterRange)
vis.updatePolyData(polyData, 'back off and all', parent='segmentation', visible=False)
return polyData
def fitObjectsOnShelf(polyData, maxHeight = 0.25):
# find the shelf plane:
polyDataWithoutFront, _ = segmentation.removeMajorPlane(polyData, distanceThreshold=0.02)
polyDataPlaneFit, origin, normal = segmentation.applyPlaneFit(polyDataWithoutFront, expectedNormal=np.array([0.0,0.0,1.0]), perpendicularAxis=np.array([0.0,0.0,1.0]), returnOrigin=True)
vis.updatePolyData(polyDataPlaneFit, 'polyDataPlaneFit', parent='segmentation', visible=False)
shelfSurfacePoints = segmentation.thresholdPoints(polyDataPlaneFit, 'dist_to_plane', [-0.01, 0.01])
shelfCenter = segmentation.computeCentroid(shelfSurfacePoints)
shelfFrame = transformUtils.getTransformFromOriginAndNormal(shelfCenter, normal, normalAxis=2)
vis.showFrame(shelfFrame, 'shelfFrame', parent='segmentation', scale=0.15 , visible=False)
# find the points near to the shelf plane and find objects on it:
points = vnp.getNumpyFromVtk(polyData, 'Points')
dist = np.dot(points - origin, normal)
vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')
shelfPoints = segmentation.thresholdPoints(polyData, 'dist_to_plane', [-0.01, maxHeight])
vis.updatePolyData(shelfPoints, 'shelf', parent='segmentation', visible=False)
data = segmentation.segmentTableScene(shelfPoints, shelfCenter, filterClustering = False )
vis.showClusterObjects(data.clusters + [data.table], parent='segmentation')
# remove the points that we considered from the orginal cloud
dists = vnp.getNumpyFromVtk(polyData, 'dist_to_plane')
diffShelf = ( ((dists > maxHeight) + (dists < -0.01))) + 0.1 -0.1
vnp.addNumpyToVtk(polyData, diffShelf, 'diff_shelf')
polyData = segmentation.thresholdPoints(polyData, 'diff_shelf', [1, 1])
vis.updatePolyData(polyData, 'rest', parent='segmentation', visible=False)
return polyData
def fitDrillBarrel ( drillPoints, forwardDirection, plane_origin, plane_normal):
''' Given a point cloud which ONLY contains points from a barrell drill, standing upright
and the equations of a table its resting on, and the general direction of the robot
Fit a barrell drill
'''
if not drillPoints.GetNumberOfPoints():
return
vis.updatePolyData(drillPoints, 'drill cluster', parent=getDebugFolder(), visible=False)
drillBarrelPoints = thresholdPoints(drillPoints, 'dist_to_plane', [0.177, 0.30])
if not drillBarrelPoints.GetNumberOfPoints():
return
# fit line to drill barrel points
linePoint, lineDirection, _ = applyLineFit(drillBarrelPoints, distanceThreshold=0.5)
if np.dot(lineDirection, forwardDirection) < 0:
lineDirection = -lineDirection
vis.updatePolyData(drillBarrelPoints, 'drill barrel points', parent=getDebugFolder(), visible=False)
pts = vtkNumpy.getNumpyFromVtk(drillBarrelPoints, 'Points')
dists = np.dot(pts-linePoint, lineDirection)
p1 = linePoint + lineDirection*np.min(dists)
p2 = linePoint + lineDirection*np.max(dists)
p1 = projectPointToPlane(p1, plane_origin, plane_normal)
p2 = projectPointToPlane(p2, plane_origin, plane_normal)
d = DebugData()
d.addSphere(p1, radius=0.01)
d.addSphere(p2, radius=0.01)
d.addLine(p1, p2)
vis.updatePolyData(d.getPolyData(), 'drill debug points', color=[0,1,0], parent=getDebugFolder(), visible=False)
drillToBasePoint = np.array([-0.07, 0.0 , -0.12])
zaxis = plane_normal
xaxis = lineDirection
xaxis /= np.linalg.norm(xaxis)
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
t = getTransformFromAxes(xaxis, yaxis, zaxis)
t.PreMultiply()
t.Translate(-drillToBasePoint)
t.PostMultiply()
t.Translate(p1)
return t
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 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 getRecedingTerrainRegion(self, polyData, linkFrame):
''' Find the point cloud in front of the foot frame'''
#polyData = shallowCopy(polyData)
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
#vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x')
#vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y')
#vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z')
viewOrigin = linkFrame.TransformPoint([0.0, 0.0, 0.0])
viewX = linkFrame.TransformVector([1.0, 0.0, 0.0])
viewY = linkFrame.TransformVector([0.0, 1.0, 0.0])
viewZ = linkFrame.TransformVector([0.0, 0.0, 1.0])
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x')
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y')
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z')
polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_x', [0.12, 1.6])
polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_y', [-0.4, 0.4])
polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_z', [-0.4, 0.4])
vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='cont debug', visible=True)
return polyData
def getRecedingTerrainRegion(self, polyData, linkFrame):
''' Find the point cloud in front of the foot frame'''
#polyData = shallowCopy(polyData)
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
#vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x')
#vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y')
#vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z')
viewOrigin = linkFrame.TransformPoint([0.0, 0.0, 0.0])
viewX = linkFrame.TransformVector([1.0, 0.0, 0.0])
viewY = linkFrame.TransformVector([0.0, 1.0, 0.0])
viewZ = linkFrame.TransformVector([0.0, 0.0, 1.0])
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x')
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y')
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z')
polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_x', [0.12, 1.6])
polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_y', [-0.4, 0.4])
polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_z', [-0.4, 0.4])
vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='cont debug', visible=True)
return polyData
def removePlaneAndBeyond(polyData,
expectedNormal=[1, 0, 0],
filterRange=[-np.inf, -0.03],
whichAxis=1,
whichAxisLetter='y',
percentile=95):
yvalues = vnp.getNumpyFromVtk(polyData, 'Points')[:, whichAxis]
backY = np.percentile(yvalues, percentile)
if (percentile > 50):
searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter,
[backY - 0.1, np.inf])
else:
searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter,
[-np.inf, backY + 0.1])
vis.updatePolyData(searchRegion,
'search region',
parent="segmentation",
colorByName=whichAxisLetter,
visible=False)
# find the plane of the back wall, remove it and the points behind it:
_, origin, normal = segmentation.applyPlaneFit(
searchRegion,
distanceThreshold=0.02,
expectedNormal=expectedNormal,
perpendicularAxis=expectedNormal,
returnOrigin=True)
points = vnp.getNumpyFromVtk(polyData, 'Points')
dist = np.dot(points - origin, normal)
vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')
backFrame = transformUtils.getTransformFromOriginAndNormal(origin,
normal,
normalAxis=2)
vis.updateFrame(backFrame,
'back frame',
parent='segmentation',
scale=0.15,
visible=False)
vis.updatePolyData(polyData,
'dist to back',
parent='segmentation',
visible=False)
polyData = segmentation.thresholdPoints(polyData, 'dist_to_plane',
filterRange)
vis.updatePolyData(polyData,
'back off and all',
parent='segmentation',
visible=False)
return polyData
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 onAtlasStepParams(self,msg):
if (msg.desired_step_spec.foot_index ==1):
is_right_foot = True
else:
is_right_foot = False
#print msg.desired_step_spec.foot_index , " and " , is_right_foot
foot = msg.desired_step_spec.foot
footTransform = transformUtils.frameFromPositionAndRPY( foot.position , [0, 0, foot.yaw*180/math.pi])
mesh,color = self.getMeshAndColor(is_right_foot)
#color[1] = 0.75 ; color[2] = 0.25
vis.updateFrame(footTransform, 'bdi foot', parent='foot placements', scale=0.2, visible=False)
#bdi_step_mesh = om.findObjectByName('bdi step')
#om.removeFromObjectModel(bdi_step_mesh)
obj = vis.updatePolyData(mesh, 'bdi step', color=color, alpha=1.0, parent='foot placements', visible=False)
obj.setProperty('Color',QtGui.QColor(color[0]*255.0,color[1]*255.0,color[2]*255.0))
obj.actor.SetUserTransform(footTransform)
def onAtlasStepParams(self,msg):
if (msg.desired_step_spec.foot_index ==1):
is_right_foot = True
else:
is_right_foot = False
#print msg.desired_step_spec.foot_index , " and " , is_right_foot
foot = msg.desired_step_spec.foot
footTransform = transformUtils.frameFromPositionAndRPY( foot.position , [0, 0, foot.yaw*180/math.pi])
mesh,color = self.getMeshAndColor(is_right_foot)
#color[1] = 0.75 ; color[2] = 0.25
vis.updateFrame(footTransform, 'bdi foot', parent='foot placements', scale=0.2, visible=False)
#bdi_step_mesh = om.findObjectByName('bdi step')
#om.removeFromObjectModel(bdi_step_mesh)
obj = vis.updatePolyData(mesh, 'bdi step', color=color, alpha=1.0, parent='foot placements', visible=False)
obj.setProperty('Color',QtGui.QColor(color[0]*255.0,color[1]*255.0,color[2]*255.0))
obj.actor.SetUserTransform(footTransform)
def fitObjectsOnShelf(polyData, maxHeight=0.25):
# find the shelf plane:
polyDataWithoutFront, _ = segmentation.removeMajorPlane(
polyData, distanceThreshold=0.02)
polyDataPlaneFit, origin, normal = segmentation.applyPlaneFit(
polyDataWithoutFront,
expectedNormal=np.array([0.0, 0.0, 1.0]),
perpendicularAxis=np.array([0.0, 0.0, 1.0]),
returnOrigin=True)
vis.updatePolyData(polyDataPlaneFit,
'polyDataPlaneFit',
parent='segmentation',
visible=False)
shelfSurfacePoints = segmentation.thresholdPoints(polyDataPlaneFit,
'dist_to_plane',
[-0.01, 0.01])
shelfCenter = segmentation.computeCentroid(shelfSurfacePoints)
shelfFrame = transformUtils.getTransformFromOriginAndNormal(shelfCenter,
normal,
normalAxis=2)
vis.showFrame(shelfFrame,
'shelfFrame',
parent='segmentation',
scale=0.15,
visible=False)
# find the points near to the shelf plane and find objects on it:
points = vnp.getNumpyFromVtk(polyData, 'Points')
dist = np.dot(points - origin, normal)
vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')
shelfPoints = segmentation.thresholdPoints(polyData, 'dist_to_plane',
[-0.01, maxHeight])
vis.updatePolyData(shelfPoints,
'shelf',
parent='segmentation',
visible=False)
data = segmentation.segmentTableScene(shelfPoints,
shelfCenter,
filterClustering=False)
vis.showClusterObjects(data.clusters + [data.table], parent='segmentation')
# remove the points that we considered from the orginal cloud
dists = vnp.getNumpyFromVtk(polyData, 'dist_to_plane')
diffShelf = (((dists > maxHeight) + (dists < -0.01))) + 0.1 - 0.1
vnp.addNumpyToVtk(polyData, diffShelf, 'diff_shelf')
polyData = segmentation.thresholdPoints(polyData, 'diff_shelf', [1, 1])
vis.updatePolyData(polyData, 'rest', parent='segmentation', visible=False)
return polyData
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 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 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 fitDrillBarrel(drillPoints, forwardDirection, plane_origin, plane_normal):
''' Given a point cloud which ONLY contains points from a barrell drill, standing upright
and the equations of a table its resting on, and the general direction of the robot
Fit a barrell drill
'''
if not drillPoints.GetNumberOfPoints():
return
vis.updatePolyData(drillPoints,
'drill cluster',
parent=getDebugFolder(),
visible=False)
drillBarrelPoints = thresholdPoints(drillPoints, 'dist_to_plane',
[0.177, 0.30])
if not drillBarrelPoints.GetNumberOfPoints():
return
# fit line to drill barrel points
linePoint, lineDirection, _ = applyLineFit(drillBarrelPoints,
distanceThreshold=0.5)
if np.dot(lineDirection, forwardDirection) < 0:
lineDirection = -lineDirection
vis.updatePolyData(drillBarrelPoints,
'drill barrel points',
parent=getDebugFolder(),
visible=False)
pts = vtkNumpy.getNumpyFromVtk(drillBarrelPoints, 'Points')
dists = np.dot(pts - linePoint, lineDirection)
p1 = linePoint + lineDirection * np.min(dists)
p2 = linePoint + lineDirection * np.max(dists)
p1 = projectPointToPlane(p1, plane_origin, plane_normal)
p2 = projectPointToPlane(p2, plane_origin, plane_normal)
d = DebugData()
d.addSphere(p1, radius=0.01)
d.addSphere(p2, radius=0.01)
d.addLine(p1, p2)
vis.updatePolyData(d.getPolyData(),
'drill debug points',
color=[0, 1, 0],
parent=getDebugFolder(),
visible=False)
drillToBasePoint = np.array([-0.07, 0.0, -0.12])
zaxis = plane_normal
xaxis = lineDirection
xaxis /= np.linalg.norm(xaxis)
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
t = getTransformFromAxes(xaxis, yaxis, zaxis)
t.PreMultiply()
t.Translate(-drillToBasePoint)
t.PostMultiply()
t.Translate(p1)
return t
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 update(self, unused):
if (om.findObjectByName('measured cop').getProperty('Visible')
and self.robotStateJointController.lastRobotStateMessage):
if self.dialogVisible == False:
self.warningButton.setVisible(True)
app.getMainWindow().statusBar().insertPermanentWidget(
0, self.warningButton)
self.dialogVisible = True
self.leftInContact = self.robotStateJointController.lastRobotStateMessage.force_torque.l_foot_force_z > 500
self.rightInContact = self.robotStateJointController.lastRobotStateMessage.force_torque.r_foot_force_z > 500
if self.rightInContact or self.leftInContact:
lFootFt = [
self.robotStateJointController.lastRobotStateMessage.
force_torque.l_foot_torque_x,
self.robotStateJointController.lastRobotStateMessage.
force_torque.l_foot_torque_y, 0.0, 0.0, 0.0,
self.robotStateJointController.lastRobotStateMessage.
force_torque.l_foot_force_z
]
rFootFt = [
self.robotStateJointController.lastRobotStateMessage.
force_torque.r_foot_torque_x,
self.robotStateJointController.lastRobotStateMessage.
force_torque.r_foot_torque_y, 0.0, 0.0, 0.0,
self.robotStateJointController.lastRobotStateMessage.
force_torque.r_foot_force_z
]
lFootTransform = self.robotStateModel.getLinkFrame(
self.robotSystem.ikPlanner.leftFootLink)
rFootTransform = self.robotStateModel.getLinkFrame(
self.robotSystem.ikPlanner.rightFootLink)
rFootOrigin = np.array(
rFootTransform.TransformPoint([0, 0, -0.07645]))
lFootOrigin = np.array(
lFootTransform.TransformPoint([0, 0,
-0.07645])) # down to sole
measured_cop = self.ddDrakeWrapper.resolveCenterOfPressure(
self.robotStateModel.model,
[self.lFootFtFrameId, self.rFootFtFrameId],
lFootFt + rFootFt, [0., 0., 1.],
(self.rightInContact * rFootOrigin +
self.leftInContact * lFootOrigin) /
(self.leftInContact + self.rightInContact))
allFootContacts = np.empty([0, 2])
if self.rightInContact:
rFootContacts = np.array([
rFootTransform.TransformPoint(contact_point)
for contact_point in self.LONG_FOOT_CONTACT_POINTS
])
allFootContacts = np.concatenate(
(allFootContacts, rFootContacts[:, 0:2]))
if self.leftInContact:
lFootContacts = np.array([
lFootTransform.TransformPoint(contact_point)
for contact_point in self.LONG_FOOT_CONTACT_POINTS
])
allFootContacts = np.concatenate(
(allFootContacts, lFootContacts[:, 0:2]))
num_pts = allFootContacts.shape[0]
dist = self.ddDrakeWrapper.drakeSignedDistanceInsideConvexHull(
num_pts, allFootContacts.reshape(num_pts * 2, 1),
measured_cop[0:2])
inSafeSupportPolygon = dist >= 0
# map dist to color -- green if inside threshold, red if not
dist = min(max(0, dist), self.DESIRED_INTERIOR_DISTANCE
) / self.DESIRED_INTERIOR_DISTANCE
# nonlinear interpolation here to try to maintain saturation
r = int(255. - 255. * dist**4)
g = int(255. * dist**0.25)
b = 0
colorStatus = [r / 255., g / 255., b / 255.]
colorStatusString = 'rgb(%d, %d, %d)' % (r, g, b)
self.warningButton.setStyleSheet("background-color:" +
colorStatusString)
d = DebugData()
d.addSphere(measured_cop[0:3], radius=0.02)
vis.updatePolyData(d.getPolyData(),
'measured cop',
view=self.view,
parent='robot state model').setProperty(
'Color', colorStatus)
elif self.dialogVisible:
app.getMainWindow().statusBar().removeWidget(self.warningButton)
self.dialogVisible = False
whichAxis=1,
whichAxisLetter='y',
percentile=95)
polyData = removePlaneAndBeyond(polyData,
expectedNormal=[1, 0, 0],
filterRange=[-np.inf, -0.03],
whichAxis=0,
whichAxisLetter='x',
percentile=95)
polyData = removePlaneAndBeyond(polyData,
expectedNormal=[1, 0, 0],
filterRange=[0.03, np.inf],
whichAxis=0,
whichAxisLetter='x',
percentile=5)
vis.updatePolyData(polyData,
'only shelves',
parent='segmentation',
visible=False)
polyData = fitObjectsOnShelf(polyData)
polyData = fitObjectsOnShelf(polyData)
polyData = fitObjectsOnShelf(polyData)
polyData = fitObjectsOnShelf(polyData, maxHeight=0.2)
if app.getTestingInteractiveEnabled():
view.show()
app.showObjectModel()
app.start()
def findMinimumBoundingRectangle(self, polyData, linkFrame):
'''
Find minimum bounding rectangle.
The input is assumed to be a rectangular point cloud of cinder blocks
Returns transform of far right corner (pointing away from robot)
'''
# TODO: for non-z up surfaces, this needs work
# TODO: return other parameters
# Originally From: https://github.com/dbworth/minimum-area-bounding-rectangle
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.02)
#vis.updatePolyData( polyData, 'block top', parent='cont debug', visible=False)
polyDataCentroid = segmentation.computeCentroid(polyData)
pts =vtkNumpy.getNumpyFromVtk( polyData , 'Points' )
xy_points = pts[:,[0,1]]
vis.updatePolyData( get2DAsPolyData(xy_points) , 'xy_points', parent='cont debug', visible=False)
hull_points = qhull_2d.qhull2D(xy_points)
vis.updatePolyData( get2DAsPolyData(hull_points) , 'hull_points', parent='cont debug', visible=False)
# Reverse order of points, to match output from other qhull implementations
hull_points = hull_points[::-1]
# print 'Convex hull points: \n', hull_points, "\n"
# Find minimum area bounding rectangle
(rot_angle, rectArea, rectDepth, rectWidth, center_point, corner_points_ground) = min_bounding_rect.minBoundingRect(hull_points)
vis.updatePolyData( get2DAsPolyData(corner_points_ground) , 'corner_points_ground', parent='cont debug', visible=False)
cornerPoints = np.vstack((corner_points_ground.T, polyDataCentroid[2]*np.ones( corner_points_ground.shape[0]) )).T
cornerPolyData = vtkNumpy.getVtkPolyDataFromNumpyPoints(cornerPoints)
# Create a frame at the far right point - which points away from the robot
farRightCorner = self.findFarRightCorner(cornerPolyData , linkFrame)
viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection()
robotYaw = math.atan2( viewDirection[1], viewDirection[0] )*180.0/np.pi
blockAngle = rot_angle*(180/math.pi)
#print "robotYaw ", robotYaw
#print "blockAngle ", blockAngle
blockAngleAll = np.array([blockAngle , blockAngle+90 , blockAngle+180, blockAngle+270])
#print blockAngleAll
for i in range(0,4):
if(blockAngleAll[i]>180):
blockAngleAll[i]=blockAngleAll[i]-360
#print blockAngleAll
values = abs(blockAngleAll - robotYaw)
#print values
min_idx = np.argmin(values)
if ( (min_idx==1) or (min_idx==3) ):
#print "flip rectDepth and rectWidth as angle is not away from robot"
temp = rectWidth ; rectWidth = rectDepth ; rectDepth = temp
#print "best angle", blockAngleAll[min_idx]
rot_angle = blockAngleAll[min_idx]*math.pi/180.0
# Optional: overwrite all of the above with the yaw of the robt when it was standing in front of the blocks:
if (self.fixBlockYawWithInitial):
rot_angle = self.initialRobotYaw
cornerTransform = transformUtils.frameFromPositionAndRPY( farRightCorner , [0,0, np.rad2deg(rot_angle) ] )
#print "Minimum area bounding box:"
#print "Rotation angle:", rot_angle, "rad (", rot_angle*(180/math.pi), "deg )"
#print "rectDepth:", rectDepth, " rectWidth:", rectWidth, " Area:", rectArea
#print "Center point: \n", center_point # numpy array
#print "Corner points: \n", cornerPoints, "\n" # numpy array
return cornerTransform, rectDepth, rectWidth, rectArea
def replanFootsteps(self, polyData, standingFootName, removeFirstLeftStep=True, doStereoFiltering=True, nextDoubleSupportPose=None):
obj = om.getOrCreateContainer('continuous')
om.getOrCreateContainer('cont debug', obj)
vis.updatePolyData( polyData, 'walking snapshot', parent='cont debug', visible=False)
standingFootFrame = self.robotStateModel.getLinkFrame(standingFootName)
vis.updateFrame(standingFootFrame, standingFootName, parent='cont debug', visible=False)
# TODO: remove the pitch and roll of this frame to support it being on uneven ground
# Step 1: filter the data down to a box in front of the robot:
polyData = self.getRecedingTerrainRegion(polyData, footstepsdriver.FootstepsDriver.getFeetMidPoint(self.robotStateModel))
if (doStereoFiltering is True):
# used for stereo data:
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
polyData = segmentation.labelOutliers(polyData, searchRadius=0.06, neighborsInSearchRadius=15) # 0.06 and 10 originally
vis.updatePolyData(polyData, 'voxel plane points', parent='cont debug', colorByName='is_outlier', visible=False)
polyData = segmentation.thresholdPoints(polyData, 'is_outlier', [0, 0])
vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='cont debug', visible=True)
# Step 2: find all the surfaces in front of the robot (about 0.75sec)
clusters = segmentation.findHorizontalSurfaces(polyData, removeGroundFirst=False, normalEstimationSearchRadius=0.05,
clusterTolerance=0.025, distanceToPlaneThreshold=0.0025, normalsDotUpRange=[0.95, 1.0])
if (clusters is None):
print "No cluster found, stop walking now!"
return
# Step 3: find the corners of the minimum bounding rectangles
blocks,groundPlane = self.extractBlocksFromSurfaces(clusters, standingFootFrame)
# Step 5: Find the two foot positions we should plan with: the next committed tool and the current standing foot
'''
if (self.committedStep is not None):
#print "i got a committedStep. is_right_foot?" , self.committedStep.is_right_foot
if (self.committedStep.is_right_foot):
standingFootTransform = self.robotStateModel.getLinkFrame(self.ikPlanner.leftFootLink)
nextDoubleSupportPose = self.getNextDoubleSupportPose(standingFootTransform, self.committedStep.transform)
else:
standingFootTransform = self.robotStateModel.getLinkFrame(self.ikPlanner.rightFootLink)
nextDoubleSupportPose = self.getNextDoubleSupportPose(self.committedStep.transform, standingFootTransform)
comm_mesh,comm_color = self.getMeshAndColor(self.committedStep.is_right_foot)
comm_color[1] = 0.75 ; comm_color[2] = 0.25
stand_mesh, stand_color = self.getMeshAndColor( not self.committedStep.is_right_foot )
stand_color[1] = 0.75 ; stand_color[2] = 0.25
vis.updateFrame(self.committedStep.transform, 'committed foot', parent='foot placements', scale=0.2, visible=False)
obj = vis.showPolyData(comm_mesh, 'committed step', color=comm_color, alpha=1.0, parent='steps')
obj.actor.SetUserTransform(self.committedStep.transform)
vis.updateFrame(standingFootTransform, 'standing foot', parent='foot placements', scale=0.2, visible=False)
obj = vis.showPolyData(stand_mesh, 'standing step', color=stand_color, alpha=1.0, parent='steps')
obj.actor.SetUserTransform(standingFootTransform)
else:
# don't have a committed footstep, assume we are standing still
nextDoubleSupportPose = self.robotStateJointController.getPose('EST_ROBOT_STATE')
'''
self.displayExpectedPose(nextDoubleSupportPose)
if not self.useManualFootstepPlacement and self.queryPlanner:
footsteps = self.computeFootstepPlanSafeRegions(blocks, nextDoubleSupportPose, standingFootName)
else:
footsteps = self.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame, removeFirstLeftStep)
if not len(footsteps):
return
if self.queryPlanner:
self.sendPlanningRequest(footsteps, nextDoubleSupportPose)
else:
self.drawFittedSteps(footsteps)
def setupObjectInCamera(self, obj):
imageView = self.imageView
obj = vis.updatePolyData(vtk.vtkPolyData(), self.getTransformedName(obj), view=imageView.view, color=obj.getProperty('Color'), parent=self.getFolderName(), visible=obj.getProperty('Visible'))
self.addActorToImageOverlay(obj, imageView)
return obj
vis.showPolyData(polyData, 'scene', visible=False)
polyData = segmentation.addCoordArraysToPolyData(polyData)
polyData = segmentation.thresholdPoints(polyData, 'y', [1, 1.6])
polyData = segmentation.thresholdPoints(polyData, 'x', [-1.2, 0.5])
vis.showPolyData(polyData, 'clipped', visible=False)
# remove outliers
polyData = segmentation.labelOutliers(polyData, searchRadius=0.03, neighborsInSearchRadius=40)
polyData = segmentation.thresholdPoints(polyData, 'is_outlier', [0, 0])
vis.showPolyData(polyData, 'inliers', visible=False)
# remove walls, and points behind temp:
polyData = removePlaneAndBeyond(polyData, expectedNormal=[0,1,0], filterRange=[-np.inf, -0.03], whichAxis=1, whichAxisLetter='y', percentile = 95)
polyData = removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[-np.inf, -0.03], whichAxis=0, whichAxisLetter='x', percentile = 95)
polyData = removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[0.03, np.inf], whichAxis=0, whichAxisLetter='x', percentile = 5)
vis.updatePolyData(polyData, 'only shelves', parent='segmentation', visible=False)
polyData = fitObjectsOnShelf(polyData)
polyData = fitObjectsOnShelf(polyData)
polyData = fitObjectsOnShelf(polyData)
polyData = fitObjectsOnShelf(polyData, maxHeight=0.2)
if app.getTestingInteractiveEnabled():
view.show()
app.showObjectModel()
app.start()
