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 = [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 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 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 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 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 onNewWalkingGoal(self, walkingGoal=None):
walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel)
frameObj = vis.updateFrame(walkingGoal,
'walking goal',
parent='planning',
scale=0.25)
frameObj.setProperty('Edit', True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
if self.placer:
self.placer.stop()
terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
if terrain:
pos = np.array(frameObj.transform.GetPosition())
polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
if polyData.GetNumberOfPoints():
polyData = segmentation.labelDistanceToLine(
polyData, pos, pos + [0, 0, 1])
polyData = segmentation.thresholdPoints(
polyData, 'distance_to_line', [0.0, 0.1])
if polyData.GetNumberOfPoints():
pos[2] = np.nanmax(
vnp.getNumpyFromVtk(polyData, 'Points')[:, 2])
frameObj.transform.Translate(
pos - np.array(frameObj.transform.GetPosition()))
d = DebugData()
d.addSphere((0, 0, 0), radius=0.03)
handle = vis.showPolyData(d.getPolyData(),
'walking goal terrain handle',
parent=frameObj,
visible=True,
color=[1, 1, 0])
handle.actor.SetUserTransform(frameObj.transform)
self.placer = PlacerWidget(app.getCurrentRenderView(), handle,
terrain)
def onFramePropertyModified(propertySet, propertyName):
if propertyName == 'Edit':
if propertySet.getProperty(propertyName):
self.placer.start()
else:
self.placer.stop()
frameObj.properties.connectPropertyChanged(onFramePropertyModified)
onFramePropertyModified(frameObj, 'Edit')
frameObj.connectFrameModified(self.onWalkingGoalModified)
self.onWalkingGoalModified(frameObj)
def drawContactPts(self, obj, footstep, **kwargs):
leftPoints, rightPoints = FootstepsDriver.getContactPts(footstep.params.support_contact_groups)
contact_pts = rightPoints if footstep.is_right_foot else leftPoints
d = DebugData()
for pt in contact_pts:
d.addSphere(pt, radius=0.01)
d_obj = vis.showPolyData(d.getPolyData(), "contact points", parent=obj, **kwargs)
d_obj.actor.SetUserTransform(obj.actor.GetUserTransform())
def drawContactPts(self, obj, footstep, **kwargs):
leftPoints, rightPoints = FootstepsDriver.getContactPts(footstep.params.support_contact_groups)
contact_pts = rightPoints if footstep.is_right_foot else leftPoints
d = DebugData()
for pt in contact_pts:
d.addSphere(pt, radius=0.01)
d_obj = vis.showPolyData(d.getPolyData(), "contact points", parent=obj, **kwargs)
d_obj.actor.SetUserTransform(obj.actor.GetUserTransform())
def drawContactPts(self, obj, footstep, **kwargs):
if footstep.is_right_foot:
_, contact_pts = FootstepsDriver.getContactPts()
else:
contact_pts, _ = FootstepsDriver.getContactPts()
d = DebugData()
for pt in contact_pts:
d.addSphere(pt, radius=0.01)
d_obj = vis.showPolyData(d.getPolyData(), "contact points", parent=obj, **kwargs)
d_obj.actor.SetUserTransform(obj.actor.GetUserTransform())
def showBoardDebug(affs=None):
referenceFrame = vtk.vtkTransform()
referenceFrame.Translate(0, 0, 5.0)
affs = affs or om.getObjects()
for obj in affs:
if isinstance(obj, BlockAffordanceItem):
d = DebugData()
d.addSphere(computeClosestCorner(obj, referenceFrame), radius=0.015)
showPolyData(d.getPolyData(), 'closest corner', parent='board debug', visible=True)
showFrame(computeGroundFrame(obj, referenceFrame), 'ground frame', parent='board debug', visible=True)
def __init__(self, uid, view, seed_pose, irisDriver, existing_region=None):
d = DebugData()
self.uid = uid
vis.PolyDataItem.__init__(self, "IRIS region {:d}".format(uid), d.getPolyData(), view)
self.transform = seed_pose
d.addSphere((0,0,0), radius=0.02)
self.seedObj = vis.showPolyData(d.getPolyData(), 'region seed', parent=om.getOrCreateContainer('IRIS region seeds'))
self.seedObj.actor.SetUserTransform(self.transform)
self.frameObj = vis.showFrame(self.transform, 'region seed frame',
scale=0.2,
visible=False,
parent=self.seedObj)
self.frameObj.setProperty('Edit', True)
self.frameObj.widget.HandleRotationEnabledOff()
terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
if terrain:
rep = self.frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
pos = np.array(self.frameObj.transform.GetPosition())
polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
if polyData.GetNumberOfPoints():
polyData = segmentation.labelDistanceToLine(polyData, pos, pos+[0,0,1])
polyData = segmentation.thresholdPoints(polyData, 'distance_to_line', [0.0, 0.1])
if polyData.GetNumberOfPoints():
pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, 'Points')[:,2])
self.frameObj.transform.Translate(pos - np.array(self.frameObj.transform.GetPosition()))
self.placer = PlacerWidget(view, self.seedObj, terrain)
self.placer.start()
else:
self.frameObj.setProperty('Edit', True)
self.frameObj.setProperty('Visible', True)
self.driver = irisDriver
self.safe_region = None
self.addProperty('Visible', True)
self.addProperty('Enabled for Walking', True)
self.addProperty('Alpha', 1.0)
self.addProperty('Color', QtGui.QColor(200,200,20))
self.frameObj.connectFrameModified(self.onFrameModified)
if existing_region is None:
self.onFrameModified(self.frameObj)
else:
self.setRegion(existing_region)
self.setProperty('Alpha', 0.5)
self.setProperty('Color', QtGui.QColor(220,220,220))
def 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 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 onNewWalkingGoal(self, walkingGoal=None):
walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel)
frameObj = vis.updateFrame(walkingGoal, 'walking goal', parent='planning', scale=0.25)
frameObj.setProperty('Edit', True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
if self.placer:
self.placer.stop()
terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
if terrain:
pos = np.array(frameObj.transform.GetPosition())
polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
if polyData.GetNumberOfPoints():
polyData = segmentation.labelDistanceToLine(polyData, pos, pos+[0,0,1])
polyData = segmentation.thresholdPoints(polyData, 'distance_to_line', [0.0, 0.1])
if polyData.GetNumberOfPoints():
pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, 'Points')[:,2])
frameObj.transform.Translate(pos - np.array(frameObj.transform.GetPosition()))
d = DebugData()
d.addSphere((0,0,0), radius=0.03)
handle = vis.showPolyData(d.getPolyData(), 'walking goal terrain handle', parent=frameObj, visible=True, color=[1,1,0])
handle.actor.SetUserTransform(frameObj.transform)
self.placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain)
def onFramePropertyModified(propertySet, propertyName):
if propertyName == 'Edit':
if propertySet.getProperty(propertyName):
self.placer.start()
else:
self.placer.stop()
frameObj.properties.connectPropertyChanged(onFramePropertyModified)
onFramePropertyModified(frameObj, 'Edit')
frameObj.connectFrameModified(self.onWalkingGoalModified)
self.onWalkingGoalModified(frameObj)
def showBoardDebug(affs=None):
referenceFrame = vtk.vtkTransform()
referenceFrame.Translate(0, 0, 5.0)
affs = affs or om.getObjects()
for obj in affs:
if isinstance(obj, BlockAffordanceItem):
d = DebugData()
d.addSphere(computeClosestCorner(obj, referenceFrame),
radius=0.015)
showPolyData(d.getPolyData(),
'closest corner',
parent='board debug',
visible=True)
showFrame(computeGroundFrame(obj, referenceFrame),
'ground frame',
parent='board debug',
visible=True)
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 createPolyDataFromPrimitive(geom):
if geom.type == lcmdrake.lcmt_viewer_geometry_data.BOX:
d = DebugData()
d.addCube(dimensions=geom.float_data[0:3], center=[0,0,0])
return d.getPolyData()
elif geom.type == lcmdrake.lcmt_viewer_geometry_data.SPHERE:
d = DebugData()
d.addSphere(center=(0,0,0), radius=geom.float_data[0])
return d.getPolyData()
elif geom.type == lcmdrake.lcmt_viewer_geometry_data.CYLINDER:
d = DebugData()
d.addCylinder(center=(0,0,0), axis=(0,0,1), radius=geom.float_data[0], length=geom.float_data[1])
return d.getPolyData()
elif geom.type == lcmdrake.lcmt_viewer_geometry_data.CAPSULE:
d = DebugData()
radius = geom.float_data[0]
length = geom.float_data[1]
d.addCylinder(center=(0,0,0), axis=(0,0,1), radius=radius, length=length)
d.addSphere(center=(0,0,length/2.0), radius=radius)
d.addSphere(center=(0,0,-length/2.0), radius=radius)
return d.getPolyData()
raise Exception('Unsupported geometry type: %s' % geom.type)
def debugDrawFootPoints(model):
pts_left, pts_right = FootstepsDriver.getContactPts()
d = DebugData()
for linkName in [_leftFootLink, _rightFootLink]:
t = model.getLinkFrame(linkName)
d.addFrame(t, scale=0.2)
if (linkName is _leftFootLink):
pts = pts_left
else:
pts = pts_right
footMidPoint = np.mean(pts, axis=0)
for p in pts.tolist() + [footMidPoint.tolist()]:
t.TransformPoint(p, p)
d.addSphere(p, radius=0.015)
midpt = FootstepsDriver.getFeetMidPoint(model)
d.addFrame(midpt, scale=0.2)
vis.showPolyData(d.getPolyData(), 'foot points debug', parent='debug', colorByName='RGB255')
def debugDrawFootPoints(model):
pts_left, pts_right = FootstepsDriver.getContactPts()
d = DebugData()
for linkName in [_leftFootLink, _rightFootLink]:
t = model.getLinkFrame(linkName)
d.addFrame(t, scale=0.2)
if (linkName is _leftFootLink):
pts = pts_left
else:
pts = pts_right
footMidPoint = np.mean(pts, axis=0)
for p in pts.tolist() + [footMidPoint.tolist()]:
t.TransformPoint(p, p)
d.addSphere(p, radius=0.015)
midpt = FootstepsDriver.getFeetMidPoint(model)
d.addFrame(midpt, scale=0.2)
vis.showPolyData(d.getPolyData(), 'foot points debug', parent='debug', colorByName='RGB255')
def draw(self):
d = DebugData()
points = [p if p is not None else self.hoverPos for p in self.points]
# draw points
for p in points:
if p is not None:
d.addSphere(p, radius=0.008)
if self.drawLines:
# draw lines
for a, b in zip(points, points[1:]):
if b is not None:
d.addLine(a, b)
# connect end points
if points[-1] is not None:
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 updateGeometryFromProperties(self):
d = DebugData()
d.addSphere((0, 0, 0), self.getProperty('Radius'))
self.setPolyData(d.getPolyData())
from ddapp.consoleapp import ConsoleApp
from ddapp.debugVis import DebugData
import ddapp.visualization as vis
# initialize application components
app = ConsoleApp()
view = app.createView()
view.showMaximized()
# create a sphere primitive
d = DebugData()
d.addSphere(center=(0,0,0), radius=0.5)
# show the sphere in the visualization window
sphere = vis.showPolyData(d.getPolyData(), 'sphere')
sphere.setProperty('Color', [0,1,0])
# start the application
app.start()
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 __init__(self, uid, view, seed_pose, irisDriver, existing_region=None):
d = DebugData()
self.uid = uid
vis.PolyDataItem.__init__(self, "IRIS region {:d}".format(uid),
d.getPolyData(), view)
self.transform = seed_pose
d.addSphere((0, 0, 0), radius=0.02)
self.seedObj = vis.showPolyData(
d.getPolyData(),
'region seed',
parent=om.getOrCreateContainer('IRIS region seeds'))
self.seedObj.actor.SetUserTransform(self.transform)
self.frameObj = vis.showFrame(self.transform,
'region seed frame',
scale=0.2,
visible=False,
parent=self.seedObj)
self.frameObj.setProperty('Edit', True)
self.frameObj.widget.HandleRotationEnabledOff()
terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
if terrain:
rep = self.frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
pos = np.array(self.frameObj.transform.GetPosition())
polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
if polyData.GetNumberOfPoints():
polyData = segmentation.labelDistanceToLine(
polyData, pos, pos + [0, 0, 1])
polyData = segmentation.thresholdPoints(
polyData, 'distance_to_line', [0.0, 0.1])
if polyData.GetNumberOfPoints():
pos[2] = np.nanmax(
vnp.getNumpyFromVtk(polyData, 'Points')[:, 2])
self.frameObj.transform.Translate(
pos - np.array(self.frameObj.transform.GetPosition()))
self.placer = PlacerWidget(view, self.seedObj, terrain)
self.placer.start()
else:
self.frameObj.setProperty('Edit', True)
self.frameObj.setProperty('Visible', True)
self.driver = irisDriver
self.safe_region = None
self.addProperty('Visible', True)
self.addProperty('Enabled for Walking', True)
self.addProperty('Alpha', 1.0)
self.addProperty('Color', QtGui.QColor(200, 200, 20))
self.frameObj.connectFrameModified(self.onFrameModified)
if existing_region is None:
self.onFrameModified(self.frameObj)
else:
self.setRegion(existing_region)
self.setProperty('Alpha', 0.5)
self.setProperty('Color', QtGui.QColor(220, 220, 220))
def 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
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 updateGeometryFromProperties(self):
d = DebugData()
d.addSphere((0,0,0), self.getProperty('Radius'))
self.setPolyData(d.getPolyData())
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
from ddapp.consoleapp import ConsoleApp
from ddapp.debugVis import DebugData
import ddapp.visualization as vis
# initialize application components
app = ConsoleApp()
view = app.createView()
view.showMaximized()
# create a sphere primitive
d = DebugData()
d.addSphere(center=(0, 0, 0), radius=0.5)
# show the sphere in the visualization window
sphere = vis.showPolyData(d.getPolyData(), 'sphere')
sphere.setProperty('Color', [0, 1, 0])
# start the application
app.start()
