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 __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 projectDrillDemoInCamera():
q = om.findObjectByName('camera overlay')
om.removeFromObjectModel(q)
imageView = cameraview.views['CAMERA_LEFT']
imageView.imageActor.SetOpacity(.2)
drawFrameInCamera(drillDemo.drill.frame.transform, 'drill frame',visible=False)
tf = transformUtils.copyFrame( drillDemo.drill.frame.transform )
tf.PreMultiply()
tf.Concatenate( drillDemo.drill.drillToButtonTransform )
drawFrameInCamera(tf, 'drill button')
tf2 = transformUtils.copyFrame( tf )
tf2.PreMultiply()
tf2.Concatenate( transformUtils.frameFromPositionAndRPY( [0,0,0] , [180,0,0] ) )
drawFrameInCamera(tf2, 'drill button flip')
drawObjectInCamera('drill',visible=False)
drawObjectInCamera('sensed pointer tip')
obj = om.findObjectByName('sensed pointer tip frame')
if (obj is not None):
drawFrameInCamera(obj.transform, 'sensed pointer tip frame',visible=False)
#drawObjectInCamera('left robotiq',visible=False)
#drawObjectInCamera('right pointer',visible=False)
v = imageView.view
v.render()
def moveDrill(self,Pos=[0,0,0],RPY=[0,0,0],Style='Local'):
linkMap = { 'left' : 'l_hand_face', 'right': 'r_hand_face'}
linkName = linkMap[self.graspingHand]
affordance = om.findObjectByName('drill')
affordanceReach = om.findObjectByName('reach frame')
frame = om.findObjectByName('drill frame')
drillTransform = affordance.actor.GetUserTransform()
reach = transformUtils.copyFrame(affordanceReach.actor.GetUserTransform())
drillTransformCopy = transformUtils.copyFrame(affordance.actor.GetUserTransform())
drillToReach = vtkTransform()
drillToReach.Identity()
drillToReach.PostMultiply()
drillToReach.Concatenate(drillTransformCopy)
drillToReach.Concatenate(reach.GetLinearInverse())
handfaceToWorld = self.ikPlanner.getLinkFrameAtPose(linkName, self.getPlanningStartPose())
# find a transform that move forward wrt hand palm
delta = transformUtils.frameFromPositionAndRPY(Pos, RPY)
drillTransform.Identity()
drillTransform.PostMultiply()
drillTransform.Concatenate(drillToReach)
drillTransform.Concatenate(delta)
drillTransform.Concatenate(handfaceToWorld)
def toggleFootstepWidget(displayPoint, view, useHorizontalWidget=False):
obj, _ = vis.findPickedObject(displayPoint, view)
if not obj:
return False
name = obj.getProperty('Name')
if name in ('footstep widget', 'footstep widget frame'):
om.removeFromObjectModel(om.findObjectByName('footstep widget'))
return True
match = re.match('^step (\d+)$', name)
if not match:
return False
stepIndex = int(match.group(1))
existingWidget = om.findObjectByName('footstep widget')
if existingWidget:
previousStep = existingWidget.stepIndex
print 'have existing widget for step:', stepIndex
om.removeFromObjectModel(existingWidget)
if previousStep == stepIndex:
print 'returning because widget was for selected step'
return True
footMesh = shallowCopy(obj.polyData)
footFrame = transformUtils.copyFrame(obj.getChildFrame().transform)
if useHorizontalWidget:
rpy = [0.0, 0.0, transformUtils.rollPitchYawFromTransform(footFrame)[2]]
footFrame = transformUtils.frameFromPositionAndRPY(footFrame.GetPosition(), np.degrees(rpy))
footObj = vis.showPolyData(footMesh, 'footstep widget', parent='planning', alpha=0.2)
footObj.stepIndex = stepIndex
frameObj = vis.showFrame(footFrame, 'footstep widget frame', parent=footObj, scale=0.2)
footObj.actor.SetUserTransform(frameObj.transform)
footObj.setProperty('Color', obj.getProperty('Color'))
frameObj.setProperty('Edit', True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
walkGoal = om.findObjectByName('walking goal')
if walkGoal:
walkGoal.setProperty('Edit', False)
def onFootWidgetChanged(frame):
footstepsDriver.onStepModified(stepIndex - 1, frame)
frameObj.connectFrameModified(onFootWidgetChanged)
return True
def onExecClicked(self):
self.commitFootstepPlan(self.lastFootstepPlan)
om.removeFromObjectModel(om.findObjectByName('footstep widget'))
walkGoal = om.findObjectByName('walking goal')
if walkGoal:
walkGoal.setProperty('Edit', False)
self.execButton.setEnabled(False)
def onClearClicked(self):
om.removeFromObjectModel(om.findObjectByName('walking goal'))
om.removeFromObjectModel(om.findObjectByName('footstep widget'))
om.removeFromObjectModel(om.findObjectByName('LCM GL'))
self.clearFootstepPlan()
if self.toolbarWidget:
self.execButton.setEnabled(False)
def colorizeMaps(enabled):
if enabled:
om.findObjectByName('Map Server').source.colorizeCallback = colorizeMapCallback
for name in _colorizeMapNames:
colorizeMapCallback(om.findObjectByName(name))
else:
om.findObjectByName('Map Server').source.colorizeCallback = None
def onExecClicked(self):
self.commitFootstepPlan(self.lastFootstepPlan)
om.removeFromObjectModel(om.findObjectByName('footstep widget'))
walkGoal = om.findObjectByName('walking goal')
if walkGoal:
walkGoal.setProperty('Edit', False)
self.execButton.setEnabled(False)
def onClearClicked(self):
om.removeFromObjectModel(om.findObjectByName('walking goal'))
om.removeFromObjectModel(om.findObjectByName('footstep widget'))
om.removeFromObjectModel(om.findObjectByName('LCM GL'))
self.clearFootstepPlan()
if self.toolbarWidget:
self.execButton.setEnabled(False)
def ungraspAffordance(self, affordanceName):
try:
del self.frameSyncs[affordanceName]
except KeyError:
pass
if not self.frameSyncs:
om.removeFromObjectModel(om.findObjectByName('l_hand frame'))
om.removeFromObjectModel(om.findObjectByName('r_hand frame'))
def ungraspAffordance(self, affordanceName):
try:
del self.frameSyncs[affordanceName]
except KeyError:
pass
if not self.frameSyncs:
om.removeFromObjectModel(om.findObjectByName('l_hand frame'))
om.removeFromObjectModel(om.findObjectByName('r_hand frame'))
def apply3DFit(self):
if om.findObjectByName('drill') is None:
self.log('No 3D fit of drill. Click Spawn Drill button to provide a fit.')
msg = lcmdrc.pfgrasp_command_t()
msg.command = lcmdrc.pfgrasp_command_t.RUN_ONE_ITER_W_3D_PRIOR
affordanceReach = om.findObjectByName('grasp frame')
affordanceReach.actor.GetUserTransform().GetPosition(msg.pos)
lcmUtils.publish('PFGRASP_CMD', msg)
def colorizeMaps(enabled):
if enabled:
om.findObjectByName(
'Map Server').source.colorizeCallback = colorizeMapCallback
for name in _colorizeMapNames:
colorizeMapCallback(om.findObjectByName(name))
else:
om.findObjectByName('Map Server').source.colorizeCallback = None
def spawnDrillAffordance(self):
if om.findObjectByName('drill') is None:
self.drillDemo.spawnDrillAffordance()
if self.graspingHand == 'left':
self.moveDrill()
else:
self.moveDrill(RPY=[0,180,0])
om.findObjectByName('drill frame').connectFrameModified(self.onModifiedDrillFrame)
def initialize(self):
self.dt = self.options['dt']
self.controllerTypeOrder = [
'defaultRandom', 'learnedRandom', 'learned', 'default'
]
self.setDefaultOptions()
self.Sensor = SensorObj(rayLength=self.options['Sensor']['rayLength'],
numRays=self.options['Sensor']['numRays'])
self.Controller = ControllerObj(self.Sensor)
self.Car = CarPlant(controller=self.Controller,
velocity=self.options['Car']['velocity'])
self.Reward = Reward(
self.Sensor,
collisionThreshold=self.collisionThreshold,
actionCost=self.options['Reward']['actionCost'],
collisionPenalty=self.options['Reward']['collisionPenalty'],
raycastCost=self.options['Reward']['raycastCost'])
self.setSARSA()
# create the things needed for simulation
om.removeFromObjectModel(om.findObjectByName('world'))
self.world = World.buildCircleWorld(
percentObsDensity=self.options['World']['percentObsDensity'],
circleRadius=self.options['World']['circleRadius'],
nonRandom=self.options['World']['nonRandomWorld'],
scale=self.options['World']['scale'],
randomSeed=self.options['World']['randomSeed'],
obstaclesInnerFraction=self.options['World']
['obstaclesInnerFraction'])
om.removeFromObjectModel(om.findObjectByName('robot'))
self.robot, self.frame = World.buildRobot()
self.locator = World.buildCellLocator(self.world.visObj.polyData)
self.Sensor.setLocator(self.locator)
self.frame = self.robot.getChildFrame()
self.frame.setProperty('Scale', 3)
self.frame.setProperty('Edit', True)
self.frame.widget.HandleRotationEnabledOff()
rep = self.frame.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
self.supervisedTrainingTime = self.options['runTime'][
'supervisedTrainingTime']
self.learningRandomTime = self.options['runTime']['learningRandomTime']
self.learningEvalTime = self.options['runTime']['learningEvalTime']
self.defaultControllerTime = self.options['runTime'][
'defaultControllerTime']
self.Car.setFrame(self.frame)
print "Finished initialization"
def getInputPointCloud(self):
polyData = segmentation.getCurrentRevolutionData()
if polyData is None:
obj = om.findObjectByName('scene')
if obj:
polyData = obj.polyData
else: # fall back to map in case we used mapping rather than loading of a scene
obj = om.findObjectByName('map')
if obj:
polyData = obj.polyData
return polyData
def initialize(self):
self.dt = self.options["dt"]
self.controllerTypeOrder = ["defaultRandom", "learnedRandom", "learned", "default"]
self.setDefaultOptions()
self.Sensor = SensorObj(
rayLength=self.options["Sensor"]["rayLength"], numRays=self.options["Sensor"]["numRays"]
)
self.Controller = ControllerObj(self.Sensor)
self.Car = CarPlant(controller=self.Controller, velocity=self.options["Car"]["velocity"])
self.Reward = Reward(
self.Sensor,
collisionThreshold=self.collisionThreshold,
actionCost=self.options["Reward"]["actionCost"],
collisionPenalty=self.options["Reward"]["collisionPenalty"],
raycastCost=self.options["Reward"]["raycastCost"],
)
self.setSARSA()
# create the things needed for simulation
om.removeFromObjectModel(om.findObjectByName("world"))
self.world = World.buildCircleWorld(
percentObsDensity=self.options["World"]["percentObsDensity"],
circleRadius=self.options["World"]["circleRadius"],
nonRandom=self.options["World"]["nonRandomWorld"],
scale=self.options["World"]["scale"],
randomSeed=self.options["World"]["randomSeed"],
obstaclesInnerFraction=self.options["World"]["obstaclesInnerFraction"],
)
om.removeFromObjectModel(om.findObjectByName("robot"))
self.robot, self.frame = World.buildRobot()
self.locator = World.buildCellLocator(self.world.visObj.polyData)
self.Sensor.setLocator(self.locator)
self.frame = self.robot.getChildFrame()
self.frame.setProperty("Scale", 3)
self.frame.setProperty("Edit", True)
self.frame.widget.HandleRotationEnabledOff()
rep = self.frame.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
self.supervisedTrainingTime = self.options["runTime"]["supervisedTrainingTime"]
self.learningRandomTime = self.options["runTime"]["learningRandomTime"]
self.learningEvalTime = self.options["runTime"]["learningEvalTime"]
self.defaultControllerTime = self.options["runTime"]["defaultControllerTime"]
self.Car.setFrame(self.frame)
print "Finished initialization"
def onMergeIntoPointCloud():
allPointClouds = om.findObjectByName('point clouds')
if allPointClouds:
allPointClouds = [
i.getProperty('Name') for i in allPointClouds.children()
]
sel = QtGui.QInputDialog.getItem(None,
"Point Cloud Merging",
"Pick point cloud to merge into:",
allPointClouds,
current=0,
editable=False)
sel = om.findObjectByName(sel)
# Make a copy of each in same frame
polyDataInto = vtk.vtkPolyData()
polyDataInto.ShallowCopy(sel.polyData)
if sel.getChildFrame():
polyDataInto = segmentation.transformPolyData(
polyDataInto,
sel.getChildFrame().transform)
polyDataFrom = vtk.vtkPolyData()
polyDataFrom.DeepCopy(pointCloudObj.polyData)
if pointCloudObj.getChildFrame():
polyDataFrom = segmentation.transformPolyData(
polyDataFrom,
pointCloudObj.getChildFrame().transform)
# Actual merge
append = filterUtils.appendPolyData([polyDataFrom, polyDataInto])
if sel.getChildFrame():
polyDataInto = segmentation.transformPolyData(
polyDataInto,
sel.getChildFrame().transform.GetInverse())
# resample
append = segmentationroutines.applyVoxelGrid(append, 0.01)
append = segmentation.addCoordArraysToPolyData(append)
# Recenter the frame
sel.setPolyData(append)
t = vtk.vtkTransform()
t.PostMultiply()
t.Translate(filterUtils.computeCentroid(append))
segmentation.makeMovable(sel, t)
# Hide the old one
if pointCloudObj.getProperty('Name') in allPointClouds:
pointCloudObj.setProperty('Visible', False)
def planReach(self):
ikPlanner = self.robotSystem.ikPlanner
startPose = self.getPlanningStartPose()
startPoseName = 'q_reach_start'
endPoseName = 'q_reach_end'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
side = 'right'
movingReachConstraint = ikPlanner.createMovingReachConstraints(startPoseName, lockBase=True, lockBack=True, lockArm=True, side=side)
palmToHand = ikPlanner.getPalmToHandLink(side=side)
targetFrame = om.findObjectByName('reach frame').transform
poseConstraints = ikPlanner.createPositionOrientationGraspConstraints(side, targetFrame, graspToHandLinkFrame=palmToHand, angleToleranceInDegrees=5.0)
constraints = []
constraints.extend(movingReachConstraint)
constraints.extend(poseConstraints)
constraintSet = ikplanner.ConstraintSet(ikPlanner, constraints, endPoseName, startPoseName)
constraintSet.ikParameters = IkParameters(maxDegreesPerSecond=30)
seedPose = ikPlanner.getMergedPostureFromDatabase(startPose, 'surprise:switch', 'above_switch', side='right')
seedPoseName = 'q_above_switch'
self.robotSystem.ikPlanner.addPose(seedPose, seedPoseName)
constraintSet.seedPoseName = seedPoseName
constraintSet.nominalPoseName = seedPoseName
endPose, info = constraintSet.runIk()
plan = constraintSet.planEndPoseGoal()
self.addPlan(plan)
def __init__(self, view):
self.view = view
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddFrameVisualization.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.botFrameUpdater = BotFrameUpdater(self.ui.botFramesListWidget)
robotModel = om.findObjectByName('robot state model')
self.linkFrameUpdater = LinkFrameUpdater(robotModel, self.ui.linkFramesListWidget)
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
self.ui.scrollArea.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize)
self.eventFilter.connect('handleEvent(QObject*, QEvent*)', self.onEvent)
PythonQt.dd.ddGroupBoxHider(self.ui.botFramesGroup)
PythonQt.dd.ddGroupBoxHider(self.ui.linkFramesGroup)
self.updateTimer = TimerCallback(targetFps=60)
self.updateTimer.callback = self.updateFrames
self.updateTimer.start()
def computeRewardFromFrameLocation(self, u=0.0):
carFrame = om.findObjectByName('robot frame')
raycastDistance = self.sensorObj.raycastAll(carFrame)
carState = 0.0 # just a placeholder for now
S = (carState, raycastDistance)
return self.computeReward(S, u)
def planGraspLineMotion(self):
self.turnPointwiseOffSlow()
startPose = self.getPlanningStartPose()
graspFrame = vtk.vtkTransform()
graspFrame.Identity()
graspFrame.PostMultiply()
if self.graspingHand == 'right':
graspFrame.Concatenate(transformUtils.frameFromPositionAndRPY([0,0,0], [0,180,0]))
graspFrame.Concatenate(transformUtils.copyFrame(om.findObjectByName('grasp frame').actor.GetUserTransform()))
constraintSet = self.ikPlanner.planEndEffectorGoal(startPose, self.graspingHand, graspFrame, \
lockBase=False, lockBack=True)
# constraint orientation
p,q = self.ikPlanner.createPositionOrientationGraspConstraints(self.graspingHand, graspFrame)
q.tspan=[0.5,1]
constraintSet.constraints.append(q)
# constraint line axis
positionConstraint, orientationConstraint, axisConstraint = self.ikPlanner.createMoveOnLineConstraints(startPose, graspFrame)
## broken robot arm has a new joint limit
if self.graspingHand == 'left':
constraintSet.constraints.append(self.createBrokenArmConstraint())
constraintSet.constraints.append(axisConstraint)
constraintSet.constraints[-1].tspan = [0.5,np.inf]
endPose, info = constraintSet.runIk()
#print endPose
if info > 10:
self.log("in Target received: Bad movement")
return
graspPlan = constraintSet.runIkTraj()
def planPinchReach(self, maxDegreesPerSecond=None):
if maxDegreesPerSecond is None:
maxDegreesPerSecond = 10
ikPlanner = self.ikPlanner
targetFrame = om.findObjectByName('pinch reach frame').transform
pinchToHand = self.getPinchToHandFrame()
startPose = self.getPlanningStartPose()
constraintSet = self.computeGraspPose(startPose,
targetFrame,
graspToHand=pinchToHand)
constraintSet.ikParameters = IkParameters(
maxDegreesPerSecond=maxDegreesPerSecond)
seedPose = ikPlanner.getMergedPostureFromDatabase(startPose,
'surprise:switch',
'above_switch',
side='right')
seedPoseName = 'q_above_switch'
self.robotSystem.ikPlanner.addPose(seedPose, seedPoseName)
constraintSet.seedPoseName = seedPoseName
constraintSet.nominalPoseName = seedPoseName
endPose, info = constraintSet.runIk()
plan = constraintSet.planEndPoseGoal()
self.addPlan(plan)
def getBDIAdjustedFootstepsFolder():
obj = om.findObjectByName('BDI adj footstep plan')
if obj is None:
obj = om.getOrCreateContainer('BDI adj footstep plan')
obj.setIcon(om.Icons.Feet)
om.collapse(obj)
return obj
def getTerrainSlicesFolder():
obj = om.findObjectByName('terrain slices')
if obj is None:
obj = om.getOrCreateContainer('terrain slices', parentObj=getFootstepsFolder())
obj.setProperty('Visible', False)
om.collapse(obj)
return obj
def updateReachFrame(self):
graspFrame = transformUtils.copyFrame(self.pinchToBox)
boxFrame = om.findObjectByName('Switch Box').getChildFrame().transform
graspFrame.PostMultiply()
graspFrame.Concatenate(boxFrame)
vis.updateFrame(graspFrame, 'pinch reach frame', scale=0.2)
def getDebugFolder():
obj = om.findObjectByName('debug')
if obj is None:
obj = om.getOrCreateContainer('debug',
om.getOrCreateContainer('segmentation'))
om.collapse(obj)
return obj
def planReach(self):
ikPlanner = self.robotSystem.ikPlanner
startPose = self.getPlanningStartPose()
startPoseName = 'q_reach_start'
endPoseName = 'q_reach_end'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
side = 'right'
movingReachConstraint = ikPlanner.createMovingReachConstraints(startPoseName, lockBase=True, lockBack=True, lockArm=True, side=side)
palmToHand = ikPlanner.getPalmToHandLink(side=side)
targetFrame = om.findObjectByName('reach frame').transform
poseConstraints = ikPlanner.createPositionOrientationGraspConstraints(side, targetFrame, graspToHandLinkFrame=palmToHand, angleToleranceInDegrees=5.0)
constraints = []
constraints.extend(movingReachConstraint)
constraints.extend(poseConstraints)
constraintSet = ikplanner.ConstraintSet(ikPlanner, constraints, endPoseName, startPoseName)
constraintSet.ikParameters = IkParameters(maxDegreesPerSecond=30)
seedPose = ikPlanner.getMergedPostureFromDatabase(startPose, 'surprise:switch', 'above_switch', side='right')
seedPoseName = 'q_above_switch'
self.robotSystem.ikPlanner.addPose(seedPose, seedPoseName)
constraintSet.seedPoseName = seedPoseName
constraintSet.nominalPoseName = seedPoseName
endPose, info = constraintSet.runIk()
plan = constraintSet.planEndPoseGoal()
self.addPlan(plan)
def updateReachFrame(self):
graspFrame = transformUtils.copyFrame(self.pinchToBox)
boxFrame = om.findObjectByName('Switch Box').getChildFrame().transform
graspFrame.PostMultiply()
graspFrame.Concatenate(boxFrame)
vis.updateFrame(graspFrame, 'pinch reach frame', scale=0.2)
def getBDIAdjustedFootstepsFolder():
obj = om.findObjectByName('BDI adj footstep plan')
if obj is None:
obj = om.getOrCreateContainer('BDI adj footstep plan')
obj.setIcon(om.Icons.Feet)
om.collapse(obj)
return obj
def colorizeMapsOff():
obj = om.findObjectByName('Map Server')
obj.source.colorizeCallback = None
alpha = 0.7
pointSize = 1.0
obj.setProperty('Alpha', alpha)
obj.setProperty('Point Size', pointSize)
def run(self):
polyData = self.getPointCloud()
annotation = self.getAnnotationInput()
annotationPoint = annotation.annotationPoints[0]
planePoints, normal = segmentation.applyLocalPlaneFit(
polyData, annotationPoint, searchRadius=0.1, searchRadiusEnd=0.2)
viewDirection = segmentation.SegmentationContext.getGlobalInstance(
).getViewDirection()
if np.dot(normal, viewDirection) < 0:
normal = -normal
xaxis = normal
zaxis = [0, 0, 1]
yaxis = np.cross(zaxis, xaxis)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
yaxis /= np.linalg.norm(yaxis)
t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
t.PostMultiply()
t.Translate(annotationPoint)
polyData = annotation.polyData
polyData = segmentation.transformPolyData(polyData,
t.GetLinearInverse())
annotation.setProperty('Visible', False)
om.removeFromObjectModel(om.findObjectByName('wall'))
obj = vis.showPolyData(polyData, 'wall')
obj.actor.SetUserTransform(t)
vis.showFrame(t, 'wall frame', scale=0.2, parent=obj)
def _addPlanItem(plan, name, itemClass):
assert plan is not None
item = itemClass(name)
item.plan = plan
om.removeFromObjectModel(om.findObjectByName(name))
om.addToObjectModel(item, om.getOrCreateContainer('segmentation'))
return item
def getGroundFrame(self):
frame = om.findObjectByName(
self.properties.getProperty('Ground frame name'))
if not frame:
self.fail('could not find ground frame')
return frame
def _addPlanItem(plan, name, itemClass):
assert plan is not None
item = itemClass(name)
item.plan = plan
om.removeFromObjectModel(om.findObjectByName(name))
om.addToObjectModel(item, om.getOrCreateContainer('segmentation'))
return item
def colorizeMapsOff():
obj = om.findObjectByName('Map Server')
obj.source.colorizeCallback = None
alpha = 0.7
pointSize = 1.0
obj.setProperty('Alpha', alpha)
obj.setProperty('Point Size', pointSize)
def run(self):
polyData = self.getPointCloud()
om.removeFromObjectModel(om.findObjectByName('pointcloud snapshot'))
vis.showPolyData(polyData,
'pointcloud snapshot',
parent='segmentation',
visible=False)
def showFusedMaps(self):
om.removeFromObjectModel(om.findObjectByName('stereo'))
om.getOrCreateContainer('stereo')
q = imageManager.queue
cameraToLocalNow = vtk.vtkTransform()
utime = q.getCurrentImageTime('CAMERA_TSDF')
q.getTransform('CAMERA_LEFT','local', utime,cameraToLocalNow)
cameraToLocalFusedNow = vtk.vtkTransform()
q.getTransform('CAMERA_LEFT_ALT','local', utime,cameraToLocalFusedNow)
for i in range(len(self.pointClouds)):
fusedNowToLocalNow = vtk.vtkTransform()
fusedNowToLocalNow.PreMultiply()
fusedNowToLocalNow.Concatenate( cameraToLocalNow)
fusedNowToLocalNow.Concatenate( cameraToLocalFusedNow.GetLinearInverse() )
fusedTransform = vtk.vtkTransform()
fusedTransform.PreMultiply()
fusedTransform.Concatenate( fusedNowToLocalNow)
fusedTransform.Concatenate( self.cameraToLocalFusedTransforms[i] )
pd = filterUtils.transformPolyData(self.pointClouds[i], fusedTransform)
vis.showFrame(fusedTransform, ('cloud frame ' + str(i)), visible=True, scale=0.2, parent='stereo')
vis.showPolyData(pd, ('stereo ' + str(i)), parent='stereo', colorByName='rgb_colors')
def getFootstepRelativeTransform(self):
self.footstepsToPlan = []
for n in xrange(1,self.numFootsteps + 1):
stepFrameName = 'step ' + str(n) + ' frame'
fToWorld = transformUtils.copyFrame(om.findObjectByName(stepFrameName).transform)
fToPlan = self.getTransformToPlanningFrame(fToWorld)
self.footstepsToPlan.append(fToPlan)
def getFootstepsFolder():
obj = om.findObjectByName('footstep plan')
if obj is None:
obj = om.getOrCreateContainer('footstep plan', parentObj=om.getOrCreateContainer('planning'))
obj.setIcon(om.Icons.Feet)
om.collapse(obj)
return obj
def getTerrainSlicesFolder():
obj = om.findObjectByName('terrain slices')
if obj is None:
obj = om.getOrCreateContainer('terrain slices', parentObj=getFootstepsFolder())
obj.setProperty('Visible', False)
om.collapse(obj)
return obj
def showHandCloud(hand='left', view=None):
view = view or app.getCurrentRenderView()
if view is None:
return
assert hand in ('left', 'right')
maps = om.findObjectByName('Map Server')
assert maps is not None
viewId = 52 if hand == 'left' else 53
reader = maps.source.reader
def getCurrentViewId():
return reader.GetCurrentMapId(viewId)
p = vtk.vtkPolyData()
obj = showPolyData(p, '%s hand cloud' % hand, view=view, parent='sensors')
obj.currentViewId = -1
def updateCloud():
currentViewId = getCurrentViewId()
#print 'updateCloud: current view id:', currentViewId
if currentViewId != obj.currentViewId:
reader.GetDataForMapId(viewId, currentViewId, p)
#print 'updated poly data. %d points.' % p.GetNumberOfPoints()
obj._renderAllViews()
t = TimerCallback()
t.targetFps = 1
t.callback = updateCloud
t.start()
obj.updater = t
return obj
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 getFootstepsFolder():
obj = om.findObjectByName('footstep plan')
if obj is None:
obj = om.getOrCreateContainer('footstep plan', parentObj=om.getOrCreateContainer('planning'))
obj.setIcon(om.Icons.Feet)
om.collapse(obj)
return obj
def run(self):
polyData = self.getPointCloud()
annotation = self.getAnnotationInput()
annotationPoint = annotation.annotationPoints[0]
planePoints, normal = segmentation.applyLocalPlaneFit(polyData, annotationPoint, searchRadius=0.1, searchRadiusEnd=0.2)
viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection()
if np.dot(normal, viewDirection) < 0:
normal = -normal
xaxis = normal
zaxis = [0, 0, 1]
yaxis = np.cross(zaxis, xaxis)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
yaxis /= np.linalg.norm(yaxis)
t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
t.PostMultiply()
t.Translate(annotationPoint)
polyData = annotation.polyData
polyData = segmentation.transformPolyData(polyData, t.GetLinearInverse())
annotation.setProperty('Visible', False)
om.removeFromObjectModel(om.findObjectByName('wall'))
obj = vis.showPolyData(polyData, 'wall')
obj.actor.SetUserTransform(t)
vis.showFrame(t, 'wall frame', scale=0.2, parent=obj)
def initialize(self):
self.dt = self.options['dt']
self.controllerTypeOrder = ['defaultRandom', 'learnedRandom', 'learned', 'default']
self.setDefaultOptions()
self.Sensor = SensorObj(rayLength=self.options['Sensor']['rayLength'],
numRays=self.options['Sensor']['numRays'])
self.Controller = ControllerObj(self.Sensor)
self.Car = CarPlant( controller=self.Controller,
velocity=self.options['Car']['velocity'])
self.Reward = Reward(self.Sensor, collisionThreshold=self.collisionThreshold,
actionCost=self.options['Reward']['actionCost'],
collisionPenalty=self.options['Reward']['collisionPenalty'],
raycastCost=self.options['Reward']['raycastCost'])
self.setSARSA()
# create the things needed for simulation
om.removeFromObjectModel(om.findObjectByName('world'))
self.world = World.buildCircleWorld(percentObsDensity=self.options['World']['percentObsDensity'],
circleRadius=self.options['World']['circleRadius'],
nonRandom=self.options['World']['nonRandomWorld'],
scale=self.options['World']['scale'],
randomSeed=self.options['World']['randomSeed'],
obstaclesInnerFraction=self.options['World']['obstaclesInnerFraction'])
om.removeFromObjectModel(om.findObjectByName('robot'))
self.robot, self.frame = World.buildRobot()
self.locator = World.buildCellLocator(self.world.visObj.polyData)
self.Sensor.setLocator(self.locator)
self.frame = self.robot.getChildFrame()
self.frame.setProperty('Scale', 3)
self.frame.setProperty('Edit', True)
self.frame.widget.HandleRotationEnabledOff()
rep = self.frame.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
self.supervisedTrainingTime = self.options['runTime']['supervisedTrainingTime']
self.learningRandomTime = self.options['runTime']['learningRandomTime']
self.learningEvalTime = self.options['runTime']['learningEvalTime']
self.defaultControllerTime = self.options['runTime']['defaultControllerTime']
self.Car.setFrame(self.frame)
print "Finished initialization"
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 colorizeMultisense(enabled):
obj = om.findObjectByName('Multisense')
if not obj:
return
setVisProperties(obj, enabled)
colorBy = 'Camera RGB' if enabled else 'Solid Color'
obj.setProperty('Color By', colorBy)
def __init__(self):
om.init()
self.view = PythonQt.dd.ddQVTKWidgetView()
# init grid
self.gridObj = vis.showGrid(self.view, parent='scene')
self.gridObj.setProperty('Surface Mode', 'Surface with edges')
self.gridObj.setProperty('Color', [0,0,0])
self.gridObj.setProperty('Alpha', 0.1)
# init view options
self.viewOptions = vis.ViewOptionsItem(self.view)
om.addToObjectModel(self.viewOptions, parentObj=om.findObjectByName('scene'))
self.viewOptions.setProperty('Background color', [0.3, 0.3, 0.35])
self.viewOptions.setProperty('Background color 2', [0.95,0.95,1])
# setup camera
applogic.setCameraTerrainModeEnabled(self.view, True)
applogic.resetCamera(viewDirection=[-1, 0, -0.3], view=self.view)
# add view behaviors
viewBehaviors = viewbehaviors.ViewBehaviors(self.view)
applogic._defaultRenderView = self.view
self.mainWindow = QtGui.QMainWindow()
self.mainWindow.setCentralWidget(self.view)
self.mainWindow.resize(768 * (16/9.0), 768)
self.mainWindow.setWindowTitle('Drake Visualizer')
self.mainWindow.setWindowIcon(QtGui.QIcon(':/images/drake_logo.png'))
self.mainWindow.show()
self.drakeVisualizer = DrakeVisualizer(self.view)
self.lcmglManager = lcmgl.LCMGLManager(self.view) if lcmgl.LCMGL_AVAILABLE else None
self.screenGrabberPanel = ScreenGrabberPanel(self.view)
self.screenGrabberDock = self.addWidgetToDock(self.screenGrabberPanel.widget, QtCore.Qt.RightDockWidgetArea)
self.screenGrabberDock.setVisible(False)
self.cameraBookmarksPanel = camerabookmarks.CameraBookmarkWidget(self.view)
self.cameraBookmarksDock = self.addWidgetToDock(self.cameraBookmarksPanel.widget, QtCore.Qt.RightDockWidgetArea)
self.cameraBookmarksDock.setVisible(False)
model = om.getDefaultObjectModel()
model.getTreeWidget().setWindowTitle('Scene Browser')
model.getPropertiesPanel().setWindowTitle('Properties Panel')
model.setActiveObject(self.viewOptions)
self.sceneBrowserDock = self.addWidgetToDock(model.getTreeWidget(), QtCore.Qt.LeftDockWidgetArea)
self.propertiesDock = self.addWidgetToDock(self.wrapScrollArea(model.getPropertiesPanel()), QtCore.Qt.LeftDockWidgetArea)
self.sceneBrowserDock.setVisible(False)
self.propertiesDock.setVisible(False)
applogic.addShortcut(self.mainWindow, 'Ctrl+Q', self.applicationInstance().quit)
applogic.addShortcut(self.mainWindow, 'F1', self._toggleObjectModel)
applogic.addShortcut(self.mainWindow, 'F2', self._toggleScreenGrabber)
applogic.addShortcut(self.mainWindow, 'F3', self._toggleCameraBookmarks)
applogic.addShortcut(self.mainWindow, 'F8', applogic.showPythonConsole)
