def fitSupport(pickPoint=[0.92858565, 0.00213802, 0.30315629]):
om.removeFromObjectModel(om.findObjectByName('cylinder'))
polyData = getPointCloud()
t = vtk.vtkTransform()
t.Translate(pickPoint)
polyData = segmentation.cropToBox(polyData, t, [0.3, 0.3, 0.5])
addHSVArrays(polyData)
vis.updatePolyData(polyData,
'crop region',
colorByName='rgb_colors',
visible=False)
zMax = getMaxZCoordinate(polyData)
cyl = makeCylinder()
cyl.setProperty('Radius', 0.03)
cyl.setProperty('Length', zMax)
origin = segmentation.computeCentroid(polyData)
origin[2] = zMax / 2.0
t = transformUtils.frameFromPositionAndRPY(origin, [0, 0, 0])
cyl.getChildFrame().copyFrame(t)
def processSingleBlock(robotStateModel, whichFile=0):
if (whichFile == 0):
polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/terrain_simple_ihmc.vtp'))
vis.updatePolyData( polyData, 'terrain_simple_ihmc.vtp', parent='continuous')
else:
polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/terrain_stairs_ihmc.vtp'))
cwdemo.chosenTerrain = 'stairs'
cwdemo.supportContact = lcmdrc.footstep_params_t.SUPPORT_GROUPS_MIDFOOT_TOE
vis.updatePolyData( polyData, 'terrain_stairs_ihmc.vtp', parent='continuous')
if drcargs.args().directorConfigFile.find('atlas') != -1:
standingFootName = 'l_foot'
else:
standingFootName = 'leftFoot'
standingFootFrame = robotStateModel.getLinkFrame(standingFootName)
vis.updateFrame(standingFootFrame, standingFootName, parent='continuous', visible=False)
# Step 1: filter the data down to a box in front of the robot:
polyData = cwdemo.getRecedingTerrainRegion(polyData, footstepsDriver.getFeetMidPoint(robotStateModel))
# 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,match_idx,groundPlane = cwdemo.extractBlocksFromSurfaces(clusters, standingFootFrame)
footsteps = cwdemo.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame)
cwdemo.drawFittedSteps(footsteps)
def rotateAndSegmentPointCloud(self):
print "rotating and pointcloud"
firstFrameToWorld = self.rotateReconstructionToStandardOrientation()
self.firstFrameToWorldTransform = firstFrameToWorld
pointOnTable = np.array(
firstFrameToWorld.TransformPoint(
self.measurementPanel.pickPoints[0]))
pointAboveTable = np.array(
firstFrameToWorld.TransformPoint(
self.measurementPanel.pickPoints[1]))
print "segmenting table"
d = self.segmentTable(pointOnTable=pointOnTable,
pointAboveTable=pointAboveTable,
visualize=False)
vis.updatePolyData(d['abovePolyData'],
'above table segmentation',
colorByName='RGB',
parent=self.visFolder)
print "saving segmentation"
self.saveAboveTablePolyData()
self.initializeFields()
def updateDrawPolyApprox(self, frame):
distances = self.Sensor.raycastAll(frame)
polyCoefficients = self.SensorApproximator.polyFitConstrainedLP(distances)
if polyCoefficients == None:
polyCoefficients = [0,0]
d = DebugData()
x = self.SensorApproximator.approxThetaVector
y = x * 0.0
for index,val in enumerate(y):
y[index] = self.horner(x[index],polyCoefficients)
origin = np.array(frame.transform.GetPosition())
origin[2] = -0.001
for i in xrange(self.SensorApproximator.numApproxPoints):
if y[i] > 0:
ray = self.SensorApproximator.approxRays[:,i]
rayTransformed = np.array(frame.transform.TransformNormal(ray))
intersection = origin + rayTransformed * y[i]
intersection[2] = -0.001
d.addLine(origin, intersection, color=[0,0,1])
vis.updatePolyData(d.getPolyData(), 'polyApprox', colorByName='RGB255')
def cropPointCloud(self,
point=None,
pointCloud=None,
radius=0.3,
visualize=True):
"""
Crop pointcloud using sphere around given point and radius
:param point: defaults to point chosen using measurement panel
:param pointCloud: defaults to 'reconstruction'
:return: cropped point cloud, polyData
"""
if point is None:
if len(self.measurementPanel.pickPoints) == 0:
print "you haven't selected any points using measurement panel"
return
point = self.measurementPanel.pickPoints[-1]
if pointCloud is None:
pointCloud = om.findObjectByName('reconstruction').polyData
croppedPointCloud = segmentation.cropToSphere(pointCloud,
point,
radius=radius)
if visualize:
vis.updatePolyData(croppedPointCloud, 'cropped pointcloud')
return croppedPointCloud
def updateDrawNormals(self, frame):
leftDistances = self.raycastLeftOrRight(frame, self.leftRays)
rightDistances = self.raycastLeftOrRight(frame, self.rightRays)
d = DebugData()
x = self.SensorApproximator.approxThetaVector
y = x * 0.0
for index,val in enumerate(y):
y[index] = self.horner(x[index],polyCoefficients)
origin = np.array(frame.transform.GetPosition())
origin[2] = -0.001
for i in xrange(self.numRays):
#ray = self.SensorApproximator.approxRays[:,i]
#rayTransformed = np.array(frame.transform.TransformNormal(ray))
#intersection = origin + rayTransformed * y[i]
#intersection[2] = -0.001
p1 = leftDistances[i]
p2 = rightDistances[i]
d.addLine(p1, p2, color=[0,0.1,1])
vis.updatePolyData(d.getPolyData(), 'polyApprox', colorByName='RGB255')
def updateDrawIntersection(self, frame, locator="None"):
if locator == "None":
locator = self.locator
origin = np.array(frame.transform.GetPosition())
# print "origin is now at", origin
d = DebugData()
sliderIdx = self.slider.value
controllerType = self.getControllerTypeFromCounter(sliderIdx)
colorMaxRange = self.colorMap[controllerType]
for i in xrange(self.Sensor.numRays):
ray = self.Sensor.rays[:, i]
rayTransformed = np.array(frame.transform.TransformNormal(ray))
# print "rayTransformed is", rayTransformed
intersection = self.Sensor.raycast(locator, origin, origin + rayTransformed * self.Sensor.rayLength)
if intersection is not None:
d.addLine(origin, intersection, color=[1, 0, 0])
else:
d.addLine(origin, origin + rayTransformed * self.Sensor.rayLength, color=colorMaxRange)
vis.updatePolyData(d.getPolyData(), "rays", colorByName="RGB255")
def onVtkObject(name):
obj = objectMap.take(name)
if not obj:
return
if isinstance(obj, vtk.vtkPolyData):
vis.updatePolyData(obj, name)
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, match_idx, groundPlane = cwdemo.extractBlocksFromSurfaces(clusters, standingFootFrame)
footsteps = cwdemo.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame)
cwdemo.drawFittedSteps(footsteps)
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 initializeFields(self):
self.visFolder = om.getOrCreateContainer('global registration')
if self.logFolder is None:
self.logFolder = "logs/scratch"
self.pathDict = utils.getFilenames(self.logFolder)
self.objectAlignmentResults = dict(
) # stores results of object alignment tool
self.objectAlignmentTool = None
# load the above table poly data if it exists
self.aboveTablePolyData = None
if os.path.isfile(self.pathDict['aboveTablePointcloud']):
print "loading above table pointcloud"
polyData = ioUtils.readPolyData(
self.pathDict['aboveTablePointcloud'])
self.aboveTablePolyData = filterUtils.transformPolyData(
polyData, self.firstFrameToWorldTransform)
vis.updatePolyData(self.aboveTablePolyData,
'above table pointcloud',
parent=self.visFolder,
colorByName='RGB')
self.storedObjects = dict()
self.loadStoredObjects()
def runRegistration(algorithm,
scenePointCloud,
modelPointCloud,
visualize=True,
**kwargs):
assert algorithm in ["GoICP", "Super4PCS"]
if (algorithm == "GoICP"):
sceneToModelTransform = GoICP.run(scenePointCloud, modelPointCloud,
**kwargs)
elif algorithm == "Super4PCS":
sceneToModelTransform = Super4PCS.run(scenePointCloud,
modelPointCloud, **kwargs)
if visualize:
alignedModelPointCloud = filterUtils.transformPolyData(
modelPointCloud, sceneToModelTransform.GetLinearInverse())
parent = om.getOrCreateContainer('registration')
vis.updatePolyData(scenePointCloud,
'scene pointcloud',
parent=parent)
vis.updatePolyData(alignedModelPointCloud,
'aligned model pointcloud',
parent=parent,
color=[1, 0, 0])
return sceneToModelTransform
def processSingleBlock(robotStateModel, whichFile=0):
if whichFile == 0:
polyData = ioUtils.readPolyData(os.path.join(dataDir, "terrain/terrain_simple_ihmc.vtp"))
vis.updatePolyData(polyData, "terrain_simple_ihmc.vtp", parent="continuous")
else:
polyData = ioUtils.readPolyData(os.path.join(dataDir, "terrain/terrain_flagstones_ihmc.vtp"))
cwdemo.chosenTerrain = "stairs"
cwdemo.supportContact = lcmdrc.footstep_params_t.SUPPORT_GROUPS_MIDFOOT_TOE
vis.updatePolyData(polyData, "terrain_stairs_ihmc.vtp", parent="continuous")
if drcargs.args().directorConfigFile.find("atlas") != -1:
standingFootName = "l_foot"
else:
standingFootName = "leftFoot"
standingFootFrame = robotStateModel.getLinkFrame(standingFootName)
vis.updateFrame(standingFootFrame, standingFootName, parent="continuous", visible=False)
# Step 1: filter the data down to a box in front of the robot:
polyData = cwdemo.getRecedingTerrainRegion(polyData, footstepsDriver.getFeetMidPoint(robotStateModel))
# 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, match_idx, groundPlane = cwdemo.extractBlocksFromSurfaces(clusters, standingFootFrame)
footsteps = cwdemo.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame)
cwdemo.drawFittedSteps(footsteps)
def visualize_background():
if not os.path.exists(background_ply_file):
return
parent = om.getOrCreateContainer("scene")
poly_data = ioUtils.readPolyData(background_ply_file)
vis.updatePolyData(poly_data, 'table', parent=parent)
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 = cwdemo.ikPlanner.leftFootLink
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,match_idx,groundPlane = cwdemo.extractBlocksFromSurfaces(clusters, standingFootFrame)
footsteps = cwdemo.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame)
cwdemo.drawFittedSteps(footsteps)
def onVtkObject(name):
obj = objectMap.take(name)
if not obj:
return
if isinstance(obj, vtk.vtkPolyData):
vis.updatePolyData(obj, name)
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 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 testCreateBackground(self):
d = DebugData()
d.addCube((1,1,0.1), (0,0,0), color=[1,1,1])
self.backgroundPolyData = d.getPolyData()
if self.vis:
view = self.views['background']
vis.updatePolyData(self.backgroundPolyData, 'background', view=view, colorByName='RGB255')
view.resetCamera()
def _update_sensor(self, sensor, frame_name):
"""Updates sensor's rays.
Args:
sensor: Sensor.
frame_name: Frame name.
"""
vis.updatePolyData(sensor.to_polydata(), frame_name,
colorByName="RGB255")
def testCreateBackground(self):
d = DebugData()
d.addCube((1,1,0.1), (0,0,0), color=[1,1,1])
self.backgroundPolyData = d.getPolyData()
if self.vis:
view = self.views['background']
vis.updatePolyData(self.backgroundPolyData, 'background', view=view, colorByName='RGB255')
view.resetCamera()
def showDebugPoint(p, name='debug point', update=False, visible=True):
d = DebugData()
d.addSphere(p, radius=0.01, color=[1, 0, 0])
if update:
vis.updatePolyData(d.getPolyData(), name)
else:
vis.showPolyData(d.getPolyData(),
name,
colorByName='RGB255',
visible=visible)
def _update_sensor(self, sensor, frame_name):
"""Updates sensor's rays.
Args:
sensor: Sensor.
frame_name: Frame name.
"""
vis.updatePolyData(sensor.to_polydata(),
frame_name,
colorByName="RGB255")
def drawModel(self, model):
modelFolder = om.getOrCreateContainer(model.name,
parentObj=self.getRootFolder())
markerFolder = om.getOrCreateContainer('markers',
parentObj=modelFolder)
modelName = model.name
markerObjects = markerFolder.children()
if len(markerObjects) != model.nummarkers:
for obj in markerObjects:
om.removeFromObjectModel(obj)
modelColor = vis.getRandomColor()
markerObjects = self.createMarkerObjects(model.nummarkers,
markerFolder, modelName,
modelColor)
self.models[modelName] = markerObjects
if len(markerObjects):
modelColor = markerObjects[0].getProperty('Color')
for i, marker in enumerate(model.markers):
xyz = np.array(marker.xyz) * self.unitConversion
markerFrame = vtk.vtkTransform()
markerFrame.Translate(xyz)
markerObjects[i].getChildFrame().copyFrame(markerFrame)
if self.drawEdges:
d = DebugData()
for m1 in model.markers:
xyz = np.array(m1.xyz) * self.unitConversion
for m2 in model.markers:
xyz2 = np.array(m2.xyz) * self.unitConversion
d.addLine(xyz, xyz2)
edges = shallowCopy(d.getPolyData())
vis.updatePolyData(edges,
modelName + ' edges',
color=modelColor,
parent=modelFolder)
else:
edgesObj = om.findObjectByName(modelName + ' edges')
om.removeFromObjectModel(edgesObj)
computeModelFrame = True
if computeModelFrame:
pos = np.array(model.segments[0].T) * self.unitConversion
rpy = np.array(model.segments[0].A)
modelFrame = transformUtils.frameFromPositionAndRPY(
pos, np.degrees(rpy))
vis.updateFrame(modelFrame,
modelName + ' frame',
parent=modelFolder,
scale=0.1)
def spin():
polyData = vtk.vtkPolyData()
reader.GetPointCloud(polyData)
frame_id = reader.GetFrameId()
sec = reader.GetSec()
nsec = reader.GetNsec()
message = str(polyData.GetNumberOfPoints()) + " points, "
message += frame_id + ", " + str(sec) + "." + str(nsec)
print(message)
vis.updatePolyData(polyData, 'point cloud')
def visualize_reconstruction(self, view, point_size=None, vis_uncropped=False):
if point_size is None:
point_size = self.config['point_size']
self.reconstruction_vis_obj = vis.updatePolyData(self.poly_data, 'Fusion Reconstruction',
view=view, colorByName='RGB')
self.reconstruction_vis_obj.setProperty('Point Size', point_size)
if vis_uncropped:
vis_obj = vis.updatePolyData(self.poly_data_uncropped, 'Uncropped Fusion Reconstruction',
view=view, colorByName='RGB')
vis_obj.setProperty('Point Size', point_size)
def spin():
polyData = vtk.vtkPolyData()
reader.GetMesh(polyData)
vis.updatePolyData(polyData, 'mesh')
print "Number of points (a)", polyData.GetNumberOfPoints()
if (polyData.GetNumberOfPoints() == 0):
return
polyDataPC = vtk.vtkPolyData()
reader.GetPointCloud(polyDataPC)
vis.updatePolyData(polyDataPC, 'output')
print "Number of points (b)", polyDataPC.GetNumberOfPoints()
def createSpheres(self, sensorValues):
d = DebugData()
for key in list(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 visualize_reconstruction(self, view, vis_uncropped=False, name=None):
if name is None:
vis_name = "Fusion Reconstruction " + self.name
else:
vis_name = name
self.reconstruction_vis_obj = vis.updatePolyData(self.poly_data, vis_name,
view=view, colorByName='RGB')
if vis_uncropped:
vis_obj = vis.updatePolyData(self.poly_data_uncropped, 'Uncropped Fusion Reconstruction',
view=view, colorByName='RGB')
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 updateDrawIntersectionManual(self, frame):
print "I am getting called too"
d = DebugData()
originHigher = np.array(frame.transform.GetPosition())
originHigher[2] = 1.0
sliderIdx = self.slider.value
for i in xrange(self.Sensor.numRays):
ray = self.Sensor.rays[:,i]
rayTransformed = np.array(frame.transform.TransformNormal(ray))
distance = self.raycastDataManual[sliderIdx,i]
d.addLine(originHigher, originHigher+rayTransformed*distance, color=[0,1,1])
vis.updatePolyData(d.getPolyData(), 'raysManual', 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 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 fitObjectToPointcloud(self,
objectName,
pointCloud=None,
downsampleObject=True,
objectPolyData=None,
filename=None,
visualize=True,
algorithm="GoICP"):
if objectPolyData is None:
if filename is None:
filename = utils.getObjectMeshFilename(objectName)
objectPolyData = ioUtils.readPolyData(filename)
# downsample the object poly data
objectPolyDataForAlgorithm = objectPolyData
if downsampleObject:
objectPolyDataForAlgorithm = segmentation.applyVoxelGrid(
objectPolyData, leafSize=0.002)
if pointCloud is None:
pointCloud = om.findObjectByName('reconstruction').polyData
sceneToModelTransform = GlobalRegistration.runRegistration(
algorithm, pointCloud, objectPolyDataForAlgorithm)
objectToWorld = sceneToModelTransform.GetLinearInverse()
self.objectToWorldTransform[objectName] = objectToWorld
if visualize:
alignedObj = vis.updatePolyData(objectPolyData,
objectName + ' aligned')
alignedObj.actor.SetUserTransform(objectToWorld)
return objectToWorld
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 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 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 buildGlobalGoal(scale):
#print "building circle world"
d = DebugData()
worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d, scale=scale, boundaryType="Square", withData=False)
#print "boundaries done"
firstX = worldXmin + np.random.rand()*(worldXmax-worldXmin)
firstY = worldYmin + np.random.rand()*(worldYmax-worldYmin)
firstEndpt = (firstX,firstY,0.2)
secondEndpt = (firstX,firstY,-0.2)
#d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn())
d.addLine(firstEndpt, secondEndpt, radius=1.0, color=[0.5,1,0])
obj = vis.updatePolyData(d.getPolyData(), 'global_goal', colorByName='RGB255')
world = World()
world.visObj = obj
world.global_goal_x = firstX
world.global_goal_y = firstY
print "I should have actually built a goal"
return world
def testCreateForeground(self):
d = DebugData()
dims = np.array([0.2,0.2,0.2])
center = (0,0,dims[2]/2.0)
d.addCube(dims, center, color=[0,1,0])
d.addPolyData(self.backgroundPolyData)
self.foregroundPolyData = d.getPolyData()
if self.vis:
view=self.views['foreground']
vis.updatePolyData(self.foregroundPolyData, 'foreground', view=view, colorByName='RGB255')
view.resetCamera()
def testCreateForeground(self):
d = DebugData()
dims = np.array([0.2,0.2,0.2])
center = (0,0,dims[2]/2.0)
d.addCube(dims, center, color=[0,1,0])
d.addPolyData(self.backgroundPolyData)
self.foregroundPolyData = d.getPolyData()
if self.vis:
view=self.views['foreground']
vis.updatePolyData(self.foregroundPolyData, 'foreground', view=view, colorByName='RGB255')
view.resetCamera()
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 drawActionSetFull(self, go_nowhere=False):
# print "I am drawing the action set"
d = DebugData()
for index, value in enumerate(self.pos_trajectories):
for time_step_index in xrange(2 * self.numPointsToDraw - 1):
firstX = value[0, time_step_index]
firstY = value[1, time_step_index]
firstZ = value[2, time_step_index]
secondX = value[0, time_step_index + 1]
secondY = value[1, time_step_index + 1]
secondZ = value[2, time_step_index + 1]
firstEndpt = (firstX, firstY, firstZ)
secondEndpt = (secondX, secondY, secondZ)
if time_step_index >= 10:
color = [0.8, 0, 0.8]
else:
color = [0.1, 0.1, 1.0]
if go_nowhere:
firstEndpt = [0, 0, 0]
secondEndpt = [0, 0, 0.001]
d.addLine(firstEndpt, secondEndpt, radius=0.02, color=color)
obj = vis.updatePolyData(d.getPolyData(), "action_set", colorByName="RGB255")
def draw(self):
d = DebugData()
points = [p if p is not None else self.hoverPos for p in self.points]
# draw points
for p in points:
if p is not None:
d.addSphere(p, radius=0.008)
if self.drawLines:
# draw lines
for a, b in zip(points, points[1:]):
if b is not None:
d.addLine(a, b)
# connect end points
if points[-1] is not None and self.drawClosedLoop:
d.addLine(points[0], points[-1])
self.annotationObj = vis.updatePolyData(d.getPolyData(),
self.annotationName,
parent=self.annotationFolder,
view=self.view)
self.annotationObj.setProperty('Color', [1,0,0])
self.annotationObj.actor.SetPickable(False)
def buildGlobalGoal():
#print "building circle world"
d = DebugData()
worldXmin = 25
worldXmax = 50
worldYmin = -20
worldYmax = 20
firstX = worldXmin + np.random.rand()*(worldXmax-worldXmin)
firstY = worldYmin + np.random.rand()*(worldYmax-worldYmin)
firstEndpt = (firstX,firstY,0.2)
secondEndpt = (firstX,firstY,-0.2)
#d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn())
d.addLine(firstEndpt, secondEndpt, radius=0.3, color=[0.5,1,0])
obj = vis.updatePolyData(d.getPolyData(), 'global_goal', colorByName='RGB255')
world = World()
world.visObj = obj
world.global_goal_x = firstX
world.global_goal_y = firstY
return world
def drawForce(self, name, linkName, forceLocation, force, color, key=None):
forceDirection = force / np.linalg.norm(force)
# om.removeFromObjectModel(om.findObjectByName(name))
linkToWorld = self.robotStateModel.getLinkFrame(linkName)
forceLocationInWorld = np.array(
linkToWorld.TransformPoint(forceLocation))
forceDirectionInWorld = np.array(
linkToWorld.TransformDoubleVector(forceDirection))
# point = forceLocationInWorld - 0.1*forceDirectionInWorld
# d = DebugData()
# # d.addArrow(point, forceLocationInWorld, headRadius=0.025, tubeRadius=0.005, color=color)
# d.addSphere(forceLocationInWorld, radius=0.01)
# d.addLine(point, forceLocationInWorld, radius=0.005)
transformForVis = transformUtils.getTransformFromOriginAndNormal(
forceLocationInWorld, forceDirectionInWorld)
obj = vis.updatePolyData(self.plungerPolyData, name, color=color)
obj.actor.SetUserTransform(transformForVis)
if key is not None and om.findObjectByName(name) is not None:
obj.addProperty('magnitude',
self.externalForces[key]['forceMagnitude'])
obj.addProperty('linkName', linkName)
obj.addProperty('key', key)
obj.connectRemovedFromObjectModel(self.removeForceFromFrameObject)
obj.properties.connectPropertyChanged(
functools.partial(self.onPropertyChanged, obj))
return obj
def onMouseMove(self, displayPoint, modifiers=None):
om.removeFromObjectModel(om.findObjectByName('link selection'))
self.linkName = None
self.pickedPoint = None
selection = self.getSelection(displayPoint)
if selection is None:
return
pickedPoint, linkName, normal, pickedCellId = selection
d = DebugData()
d.addSphere(pickedPoint, radius=0.01)
d.addLine(pickedPoint, np.array(pickedPoint) + 0.1 * np.array(normal), radius=0.005)
obj = vis.updatePolyData(d.getPolyData(), 'link selection', color=[0,1,0])
obj.actor.SetPickable(False)
self.linkName = linkName
self.pickedPoint = pickedPoint
self.normal = normal
self.pickedCellId = pickedCellId
modifiers = QtGui.QApplication.keyboardModifiers()
if modifiers == QtCore.Qt.ControlModifier and self.cellCaptureMode:
self.provisionallyCaptureCell(linkName, pickedCellId)
def draw(self):
d = DebugData()
points = list(self.points)
if self.hoverPos is not None:
points.append(self.hoverPos)
# draw points
radius = 5
scale = (2 * self.view.camera().GetParallelScale()) / (
self.view.renderer().GetSize()[1])
for p in points:
d.addSphere(p, radius=radius * scale)
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 = 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 updatePlanFrames(self):
if self.getViewMode() != 'frames':
return
numberOfSamples = self.getNumberOfSamples()
meshes = self.planPlayback.getPlanPoseMeshes(
self.plan, self.playbackJointController, self.playbackRobotModel,
numberOfSamples)
d = DebugData()
startColor = [0.8, 0.8, 0.8]
endColor = [85 / 255.0, 255 / 255.0, 255 / 255.0]
colorFunc = scipy.interpolate.interp1d([0, numberOfSamples - 1],
[startColor, endColor],
axis=0,
kind='slinear')
for i, mesh in reversed(list(enumerate(meshes))):
d.addPolyData(mesh, color=colorFunc(i))
pd = d.getPolyData()
clean = vtk.vtkCleanPolyData()
clean.SetInput(pd)
clean.Update()
pd = clean.GetOutput()
self.planFramesObj = vis.updatePolyData(d.getPolyData(),
'robot plan',
alpha=1.0,
visible=False,
colorByName='RGB255',
parent='planning')
self.showPlanFrames()
def draw(self):
d = DebugData()
points = [p if p is not None else self.hoverPos for p in self.points]
# draw points
for p in points:
if p is not None:
d.addSphere(p, radius=0.008)
if self.drawLines:
# draw lines
for a, b in zip(points, points[1:]):
if b is not None:
d.addLine(a, b)
# connect end points
if points[-1] is not None and self.drawClosedLoop:
d.addLine(points[0], points[-1])
self.annotationObj = vis.updatePolyData(d.getPolyData(),
self.annotationName,
parent=self.annotationFolder,
view=self.view)
self.annotationObj.setProperty('Color', [1, 0, 0])
self.annotationObj.actor.SetPickable(False)
def drawModel(self, model):
modelFolder = om.getOrCreateContainer(model.name, parentObj=self.getRootFolder())
markerFolder = om.getOrCreateContainer("markers", parentObj=modelFolder)
modelName = model.name
markerObjects = markerFolder.children()
if len(markerObjects) != model.nummarkers:
for obj in markerObjects:
om.removeFromObjectModel(obj)
modelColor = vis.getRandomColor()
markerObjects = self.createMarkerObjects(model.nummarkers, markerFolder, modelName, modelColor)
self.models[modelName] = markerObjects
if len(markerObjects):
modelColor = markerObjects[0].getProperty("Color")
for i, marker in enumerate(model.markers):
xyz = np.array(marker.xyz) * self.unitConversion
markerFrame = vtk.vtkTransform()
markerFrame.Translate(xyz)
markerObjects[i].getChildFrame().copyFrame(markerFrame)
if self.drawEdges:
d = DebugData()
for m1 in model.markers:
xyz = np.array(m1.xyz) * self.unitConversion
for m2 in model.markers:
xyz2 = np.array(m2.xyz) * self.unitConversion
d.addLine(xyz, xyz2)
edges = shallowCopy(d.getPolyData())
vis.updatePolyData(edges, modelName + " edges", color=modelColor, parent=modelFolder)
else:
edgesObj = om.findObjectByName(modelName + " edges")
om.removeFromObjectModel(edgesObj)
computeModelFrame = True
if computeModelFrame:
pos = np.array(model.segments[0].T) * self.unitConversion
rpy = np.array(model.segments[0].A)
modelFrame = transformUtils.frameFromPositionAndRPY(pos, np.degrees(rpy))
vis.updateFrame(modelFrame, modelName + " frame", parent=modelFolder, scale=0.1)
def buildLineSegmentTestWorld(percentObsDensity, nonRandom=False, circleRadius=3, scale=1.0, randomSeed=5,
obstaclesInnerFraction=1.0):
#print "building circle world"
if nonRandom:
np.random.seed(randomSeed)
d = DebugData()
worldXmin = 3
worldXmax = 15
worldYmin = -10
worldYmax = 10
#print "boundaries done"
worldArea = (worldXmax-worldXmin)*(worldYmax-worldYmin)
#print worldArea
obsScalingFactor = 5.0/12.0
maxNumObstacles = obsScalingFactor * worldArea
numObstacles = int(obstaclesInnerFraction**2 * percentObsDensity/100.0 * maxNumObstacles)
print numObstacles, " is num obstacles"
# draw random stick obstacles
obsLength = 2.0
obsXmin = worldXmin + (1-obstaclesInnerFraction)/2.0*(worldXmax - worldXmin)
obsXmax = worldXmax - (1-obstaclesInnerFraction)/2.0*(worldXmax - worldXmin)
obsYmin = worldYmin + (1-obstaclesInnerFraction)/2.0*(worldYmax - worldYmin)
obsYmax = worldYmax - (1-obstaclesInnerFraction)/2.0*(worldYmax - worldYmin)
for i in xrange(numObstacles):
firstX = obsXmin + np.random.rand()*(obsXmax-obsXmin)
firstY = obsYmin + np.random.rand()*(obsYmax-obsYmin)
secondX = obsXmin + np.random.rand()*(obsXmax-obsXmin)
secondY = obsYmin + np.random.rand()*(obsYmax-obsYmin)
firstEndpt = (firstX,firstY,0.2)
secondEndpt = (firstX,firstY,-0.2)
#d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn())
d.addLine(firstEndpt, secondEndpt, radius=0.5)
obj = vis.updatePolyData(d.getPolyData(), 'world')
world = World()
world.visObj = obj
world.Xmax = worldXmax
world.Xmin = worldXmin
world.Ymax = worldYmax
world.Ymin = worldYmin
world.numObstacles = numObstacles
world.percentObsDensity = percentObsDensity
return world
def 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 drawActionSetEmpty(self):
# print "I am drawing the action set"
d = DebugData()
for index, value in enumerate(self.pos_trajectories):
for time_step_index in xrange(2 * self.numPointsToDraw - 1):
d.addLine([0, 0, 0], [0, 0, 0], radius=0.01, color=[1, 1, 1])
obj = vis.updatePolyData(d.getPolyData(), "action_set", colorByName="RGB255")
def drawFirstIntersections(self, frame, firstRaycast):
origin = np.array(frame.transform.GetPosition())
d = DebugData()
firstRaycastLocations = self.Sensor.invertRaycastsToLocations(self.frame, firstRaycast)
for i in xrange(self.Sensor.numRays):
endpoint = firstRaycastLocations[i,:]
if firstRaycast[i] == 20.0:
d.addLine(origin, endpoint, color=[0,1,0])
else:
d.addLine(origin, endpoint, color=[1,0,0])
vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations)
self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData)
self.Sensor.setLocator(self.LineSegmentLocator)
def gravitizeAccelSphere(accel_sphere_poly_data, gravity_max, color=[1,0.3,0.0], alpha=0.5):
d = DebugData()
center = [0, 0, -gravity_max]
t = vtk.vtkTransform()
t.Translate(center)
polyData = filterUtils.transformPolyData(accel_sphere_poly_data, t)
obj = vis.updatePolyData(polyData, 'accel_sphere', color=color, alpha=alpha)
return polyData
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 testDepthImageToPointcloud(self):
ds = self.depthScanners['foreground']
camera = ds.view.camera()
depth_img = ds.getDepthImage()
depth_img_raw, _, _ = ds.getDepthImageAndPointCloud()
depth_img_numpy = ds.getDepthImageAsNumpyArray()
print "depth min %.3f, depth max = %.3f" %(np.min(depth_img_numpy), np.max(depth_img_numpy))
# self.changeDetectionImageVisualizer.setImage(depth_img)
f = vtk.vtkDepthImageToPointCloud()
f.SetCamera(camera)
f.SetInputData(depth_img)
f.Update()
polyData = f.GetOutput()
print "polyData.GetNumberOfPoints() = ", polyData.GetNumberOfPoints()
view = self.changeDetectionPointCloudView
vis.updatePolyData(polyData, 'depth_im_to_pointcloud')
