def processSnippet():
obj = om.getOrCreateContainer('continuous')
om.getOrCreateContainer('cont debug', obj)
if (continuouswalkingDemo.processContinuousStereo):
polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/block_snippet_stereo.vtp'))
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
else:
polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/block_snippet.vtp'))
vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='continuous')
standingFootName = 'l_foot'
standingFootFrame = robotStateModel.getLinkFrame(standingFootName)
vis.updateFrame(standingFootFrame, standingFootName, parent='continuous', visible=False)
# Step 2: find all the surfaces in front of the robot (about 0.75sec)
clusters = segmentation.findHorizontalSurfaces(polyData)
if (clusters is None):
print "No cluster found, stop walking now!"
return
# Step 3: find the corners of the minimum bounding rectangles
blocks,groundPlane = cwdemo.extractBlocksFromSurfaces(clusters, standingFootFrame)
footsteps = cwdemo.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame)
cwdemo.drawFittedSteps(footsteps)
def 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 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 getDebugFolder():
obj = om.findObjectByName('debug')
if obj is None:
obj = om.getOrCreateContainer('debug',
om.getOrCreateContainer('segmentation'))
om.collapse(obj)
return obj
def onHideBDIButton(self):
self.driver.showBDIPlan = False
self.driver.bdiRobotModel.setProperty('Visible', False)
folder = om.getOrCreateContainer("BDI footstep plan")
om.removeFromObjectModel(folder)
folder = om.getOrCreateContainer("BDI adj footstep plan")
om.removeFromObjectModel(folder)
def onHideBDIButton(self):
self.driver.showBDIPlan = False
self.driver.bdiRobotModel.setProperty('Visible', False)
folder = om.getOrCreateContainer("BDI footstep plan")
om.removeFromObjectModel(folder)
folder = om.getOrCreateContainer("BDI adj footstep plan")
om.removeFromObjectModel(folder)
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 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 drawBDIFootstepPlanAdjusted(self):
if (self.bdi_plan_adjusted is None):
return
folder = om.getOrCreateContainer('BDI adj footstep plan')
om.removeFromObjectModel(folder)
folder = om.getOrCreateContainer('BDI adj footstep plan')
folder.setIcon(om.Icons.Feet)
om.collapse(folder)
self.drawFootstepPlan(self.bdi_plan_adjusted, folder, [1.0, 1.0, 0.0] , [0.0, 1.0, 1.0])
def drawBDIFootstepPlanAdjusted(self):
if (self.bdi_plan_adjusted is None):
return
folder = om.getOrCreateContainer('BDI adj footstep plan')
om.removeFromObjectModel(folder)
folder = om.getOrCreateContainer('BDI adj footstep plan')
folder.setIcon(om.Icons.Feet)
om.collapse(folder)
self.drawFootstepPlan(self.bdi_plan_adjusted, folder, [1.0, 1.0, 0.0] , [0.0, 1.0, 1.0])
def showHandSamples(self, numberOfSamples=15):
om.removeFromObjectModel(om.findObjectByName('sampled hands'))
handFolder = om.getOrCreateContainer('sampled hands', parentObj=om.getOrCreateContainer('debug'))
for i in xrange(numberOfSamples):
t = self.splineInterp(i/float(numberOfSamples-1))
handObj, f = self.handFactory.placeHandModelWithTransform(t, self.view, side=self.side, name='sample %d' % i, parent=handFolder)
handObj.setProperty('Alpha', 0.3)
handFolder.setProperty('Visible', False)
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 _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 _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 showPolyData(polyData, name, color=None, colorByName=None, colorByRange=None, alpha=1.0, visible=True, view=None, parent='segmentation', cls=None):
view = view or app.getCurrentRenderView()
assert view
cls = cls or PolyDataItem
item = cls(name, polyData, view)
if isinstance(parent, str):
parentObj = om.getOrCreateContainer(parent)
else:
parentObj = parent
om.addToObjectModel(item, parentObj)
item.setProperty('Visible', visible)
item.setProperty('Alpha', alpha)
if colorByName and colorByName not in item.getArrayNames():
print 'showPolyData(colorByName=%s): array not found' % colorByName
colorByName = None
if colorByName:
item.setProperty('Color By', colorByName)
item.colorBy(colorByName, colorByRange)
else:
color = [1.0, 1.0, 1.0] if color is None else color
item.setProperty('Color', [float(c) for c in color])
item.colorBy(None)
return item
def newPolyData(self, name, view, parent=None):
polyData = self.getNewHandPolyData()
if isinstance(parent, str):
parent = om.getOrCreateContainer(parent)
color = [1.0, 1.0, 0.0]
if self.side == 'right':
color = [0.33, 1.0, 0.0]
obj = vis.showPolyData(polyData,
name,
view=view,
color=color,
visible=False,
parent=parent)
obj.side = self.side
frame = vtk.vtkTransform()
frame.PostMultiply()
obj.actor.SetUserTransform(frame)
frameObj = vis.showFrame(frame,
'%s frame' % name,
view=view,
scale=0.2,
visible=False,
parent=obj)
return obj
def run(self):
radius = self.properties.getProperty('Radius')
thickness = 0.03
folder = om.getOrCreateContainer('affordances')
frame = self.computeValveFrame()
d = DebugData()
d.addLine(np.array([0, 0, -thickness / 2.0]),
np.array([0, 0, thickness / 2.0]),
radius=radius)
mesh = d.getPolyData()
params = dict(radius=radius,
length=thickness,
xwidth=radius,
ywidth=radius,
zwidth=thickness,
otdf_type='steering_cyl',
friendly_name='valve')
affordance = vis.showPolyData(mesh,
'valve',
color=[0.0, 1.0, 0.0],
cls=affordanceitems.FrameAffordanceItem,
parent=folder,
alpha=1.0)
frame = vis.showFrame(frame,
'valve frame',
parent=affordance,
visible=False,
scale=radius)
affordance.actor.SetUserTransform(frame.transform)
affordance.setAffordanceParams(params)
affordance.updateParamsFromActorTransform()
def 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 planSequenceRoomMap(self):
self.graspingHand = 'left'
self.targetSweepType = 'orientation'
self.graspToHandLinkFrame = self.ikPlanner.newGraspToHandFrame(
self.graspingHand)
self.planFromCurrentRobotState = False
self.plans = []
self.currentYawDegrees = 60
self.ikPlanner.ikServer.maxDegreesPerSecond = 10
self.nextPosition = [0, 0, 0]
self.targetPath = []
self.resetTargetPath()
self.fromTop = True
self.mapFolder = om.getOrCreateContainer('room mapping')
om.collapse(self.mapFolder)
# taskQueue doesnt support a while loop:
#while (self.currentYawDegrees >= -90):
# self.getRoomSweepFrames()
# self.planRoomReach()# move to next start point
# self.planRoomSweep() # reach down/up
# self.currentYawDegrees = self.currentYawDegrees - 30
# self.fromTop = not self.fromTop
self.getRoomSweepFrames()
self.planRoomReach() # move to next start point
self.planRoomSweep() # reach down/up
self.moveRoomSweepOnwards()
self.getRoomSweepFrames()
self.planRoomReach() # move to next start point
self.planRoomSweep() # reach down/up
self.moveRoomSweepOnwards()
self.getRoomSweepFrames()
self.planRoomReach() # move to next start point
self.planRoomSweep() # reach down/up
self.moveRoomSweepOnwards()
self.getRoomSweepFrames()
self.planRoomReach() # move to next start point
self.planRoomSweep() # reach down/up
self.moveRoomSweepOnwards()
self.getRoomSweepFrames()
self.planRoomReach() # move to next start point
self.planRoomSweep() # reach down/up
self.moveRoomSweepOnwards()
self.getRoomSweepFrames()
self.planRoomReach() # move to next start point
self.planRoomSweep() # reach down/up
self.moveRoomSweepOnwards()
self.getRoomSweepFrames()
self.planRoomReach() # move to next start point
self.planRoomSweep() # reach down/up
self.moveRoomSweepOnwards()
def loadRobotModel(name, view=None, parent='planning', urdfFile=None, color=None, visible=True):
if not urdfFile:
urdfFile = urdfConfig['default']
if isinstance(parent, str):
parent = om.getOrCreateContainer(parent)
model = loadRobotModelFromFile(urdfFile)
if not model:
raise Exception('Error loading robot model from file: %s' % urdfFile)
obj = RobotModelItem(model)
om.addToObjectModel(obj, parent)
obj.setProperty('Visible', visible)
obj.setProperty('Name', name)
obj.setProperty('Color', color or getRobotGrayColor())
if view is not None:
obj.addToView(view)
jointController = jointcontrol.JointController([obj], fixedPointFile)
jointController.setNominalPose()
return obj, jointController
def loadRobotModel(name, view=None, parent='planning', urdfFile=None, color=None, visible=True):
if not urdfFile:
urdfFile = urdfConfig['default']
if isinstance(parent, str):
parent = om.getOrCreateContainer(parent)
model = loadRobotModelFromFile(urdfFile)
if not model:
raise Exception('Error loading robot model from file: %s' % urdfFile)
obj = RobotModelItem(model)
om.addToObjectModel(obj, parent)
obj.setProperty('Visible', visible)
obj.setProperty('Name', name)
obj.setProperty('Color', color or getRobotGrayColor())
if view is not None:
obj.addToView(view)
jointController = jointcontrol.JointController([obj], fixedPointFile)
jointController.setNominalPose()
return obj, jointController
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 newPolyData(self, name, view, parent=None):
self.handModel.model.setJointPositions(
np.zeros(self.handModel.model.numberOfJoints()))
polyData = vtk.vtkPolyData()
self.handModel.model.getModelMesh(polyData)
polyData = filterUtils.transformPolyData(polyData, self.modelToPalm)
if isinstance(parent, str):
parent = om.getOrCreateContainer(parent)
color = [1.0, 1.0, 0.0]
if self.side == 'right':
color = [0.33, 1.0, 0.0]
obj = vis.showPolyData(polyData,
name,
view=view,
color=color,
visible=False,
parent=parent)
obj.side = self.side
frame = vtk.vtkTransform()
frame.PostMultiply()
obj.actor.SetUserTransform(frame)
frameObj = vis.showFrame(frame,
'%s frame' % name,
view=view,
scale=0.2,
visible=False,
parent=obj)
return obj
def init(view):
global _multisenseItem
global multisenseDriver
m = MultiSenseSource(view)
m.start()
multisenseDriver = m
sensorsFolder = om.getOrCreateContainer('sensors')
_multisenseItem = MultisenseItem(m)
om.addToObjectModel(_multisenseItem, sensorsFolder)
useMapServer = hasattr(drc, 'vtkMapServerSource')
if useMapServer:
mapServerSource = MapServerSource(view, callbackFunc=view.render)
mapsServerContainer = om.ObjectModelItem('Map Server', icon=om.Icons.Robot)
mapsServerContainer.source = mapServerSource
om.addToObjectModel(mapsServerContainer, parentObj=sensorsFolder)
mapServerSource.start()
else:
mapServerSource = None
spindleDebug = SpindleAxisDebug(multisenseDriver)
spindleDebug.addToView(view)
om.addToObjectModel(spindleDebug, sensorsFolder)
return multisenseDriver, mapServerSource
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 makeSplineGraspConstraints(self, ikPlanner, positionTolerance=0.03, angleToleranceInDegrees=15):
params = self.computeHandleParameterization()
segments = zip(params, params[1:])
#times = [np.linspace(segment[0], segment[1], 6) for segment in segments]
#times = [[0.0, 0.3, 0.5], np.linspace(params[-2], params[-1], 6)]
times = np.linspace(params[-2], params[-1], 6)
times = np.hstack(times)
times = np.unique(times)
frames = []
for t in times:
frames.append(self.splineInterp(t))
folder = om.getOrCreateContainer('constraint spline samples', parentObj=om.getOrCreateContainer('debug'))
for f in frames:
vis.showFrame(f, 'frame', scale=0.1)
side = self.side
graspToPalm = vtk.vtkTransform()
graspToHand = ikPlanner.newGraspToHandFrame(side, graspToPalm)
constraints = []
for f, t in zip(frames[:-1], times[:-1]):
graspToWorld = f
p, q = ikPlanner.createPositionOrientationGraspConstraints(side, graspToWorld, graspToHand)
p.lowerBound = np.tile(-positionTolerance, 3)
p.upperBound = np.tile(positionTolerance, 3)
q.angleToleranceInDegrees = angleToleranceInDegrees
if t >= params[-2]:
q.angleToleranceInDegrees = 0
p.lowerBound = np.tile(-0.0, 3)
p.upperBound = np.tile(0.0, 3)
constraints.append(q)
p.tspan = [t, t]
q.tspan = [t, t]
constraints.append(p)
return constraints
def planSequenceRoomMap(self):
self.graspingHand = 'left'
self.targetSweepType = 'orientation'
self.graspToHandLinkFrame = self.ikPlanner.newGraspToHandFrame(self.graspingHand)
self.planFromCurrentRobotState = False
self.plans = []
self.currentYawDegrees = 60
self.ikPlanner.ikServer.maxDegreesPerSecond = 10
self.nextPosition =[0,0,0]
self.targetPath = []
self.resetTargetPath()
self.fromTop = True
self.mapFolder=om.getOrCreateContainer('room mapping')
om.collapse(self.mapFolder)
# taskQueue doesnt support a while loop:
#while (self.currentYawDegrees >= -90):
# self.getRoomSweepFrames()
# self.planRoomReach()# move to next start point
# self.planRoomSweep() # reach down/up
# self.currentYawDegrees = self.currentYawDegrees - 30
# self.fromTop = not self.fromTop
self.getRoomSweepFrames()
self.planRoomReach()# move to next start point
self.planRoomSweep() # reach down/up
self.moveRoomSweepOnwards()
self.getRoomSweepFrames()
self.planRoomReach()# move to next start point
self.planRoomSweep() # reach down/up
self.moveRoomSweepOnwards()
self.getRoomSweepFrames()
self.planRoomReach()# move to next start point
self.planRoomSweep() # reach down/up
self.moveRoomSweepOnwards()
self.getRoomSweepFrames()
self.planRoomReach()# move to next start point
self.planRoomSweep() # reach down/up
self.moveRoomSweepOnwards()
self.getRoomSweepFrames()
self.planRoomReach()# move to next start point
self.planRoomSweep() # reach down/up
self.moveRoomSweepOnwards()
self.getRoomSweepFrames()
self.planRoomReach()# move to next start point
self.planRoomSweep() # reach down/up
self.moveRoomSweepOnwards()
self.getRoomSweepFrames()
self.planRoomReach()# move to next start point
self.planRoomSweep() # reach down/up
self.moveRoomSweepOnwards()
def findEndEffectors(self):
planning = om.getOrCreateContainer('planning')
if planning is not None:
teleopFolder = planning.findChild('teleop plan')
if teleopFolder is not None:
framesFolder = teleopFolder.findChild('constraint frames')
if framesFolder is not None:
self.endEffectorFrames = framesFolder.children()
def _loadAffordanceFromDescription(self, desc):
className = desc['classname']
cls = getattr(affordanceitems, className)
aff = cls(desc['Name'], self.view)
om.addToObjectModel(aff, parentObj=om.getOrCreateContainer('affordances'))
vis.addChildFrame(aff)
aff.loadDescription(desc, copyMode=aff.COPY_MODE_ALL)
self.registerAffordance(aff, notify=False)
def findEndEffectors(self):
planning = om.getOrCreateContainer('planning')
if planning is not None:
teleopFolder = planning.findChild('teleop plan')
if teleopFolder is not None:
framesFolder = teleopFolder.findChild('constraint frames')
if framesFolder is not None:
self.endEffectorFrames = framesFolder.children()
def computeFootstepPlanSafeRegions(self, blocks, robotPose, standingFootName):
print 'planning with safe regions. %d blocks.' % len(blocks)
folder = om.getOrCreateContainer('Safe terrain regions')
om.removeFromObjectModel(folder)
footsteps = []
for i, block in enumerate(blocks):
corners = block.getCorners()
rpy = np.radians(block.cornerTransform.GetOrientation())
#rpy = [0.0, 0.0, 0.0]
self.convertStepToSafeRegion(corners, rpy)
lastBlock = blocks[-1]
goalFrame = transformUtils.copyFrame(lastBlock.cornerTransform)
goalOffset = vtk.vtkTransform()
goalOffset.Translate(0.3, lastBlock.rectWidth/2.0, 0.0)
goalFrame.PreMultiply()
goalFrame.Concatenate(goalOffset)
goalPosition = np.array(goalFrame.GetPosition())
if len(blocks) > 1:
goalFrame = transformUtils.copyFrame(blocks[-2].cornerTransform)
goalFrame.Translate(goalPosition - np.array(goalFrame.GetPosition()))
vis.updateFrame(goalFrame, 'footstep plan goal', scale=0.2)
request = self.footstepsPanel.driver.constructFootstepPlanRequest(robotPose, goalFrame)
assert standingFootName in (self.ikPlanner.leftFootLink, self.ikPlanner.rightFootLink)
if standingFootName == self.ikPlanner.rightFootLink:
leadingFoot = lcmdrc.footstep_plan_params_t.LEAD_RIGHT
else:
leadingFoot = lcmdrc.footstep_plan_params_t.LEAD_LEFT
request.params.leading_foot = leadingFoot
request.params.max_forward_step = 0.5
request.params.nom_forward_step = 0.12
request.params.nom_step_width = 0.22
request.params.max_num_steps = 8 #2*len(blocks)
plan = self.footstepsPanel.driver.sendFootstepPlanRequest(request, waitForResponse=True)
if not plan:
return []
#print 'received footstep plan with %d steps.' % len(plan.footsteps)
footsteps = []
for i, footstep in enumerate(plan.footsteps):
footstepTransform = self.transformFromFootstep(footstep)
footsteps.append(Footstep(footstepTransform, footstep.is_right_foot))
return footsteps[2:]
def _loadAffordanceFromDescription(self, desc):
className = desc['classname']
cls = getattr(affordanceitems, className)
aff = cls(desc['Name'], self.view)
om.addToObjectModel(aff,
parentObj=om.getOrCreateContainer('affordances'))
vis.addChildFrame(aff)
aff.loadDescription(desc, copyMode=aff.COPY_MODE_ALL)
self.registerAffordance(aff, notify=False)
def computeFootstepPlanSafeRegions(self, blocks, robotPose, standingFootName):
print 'planning with safe regions. %d blocks.' % len(blocks)
folder = om.getOrCreateContainer('Safe terrain regions')
om.removeFromObjectModel(folder)
footsteps = []
for i, block in enumerate(blocks):
corners = block.getCorners()
rpy = np.radians(block.cornerTransform.GetOrientation())
#rpy = [0.0, 0.0, 0.0]
self.convertStepToSafeRegion(corners, rpy)
lastBlock = blocks[-1]
goalFrame = transformUtils.copyFrame(lastBlock.cornerTransform)
goalOffset = vtk.vtkTransform()
goalOffset.Translate(0.3, lastBlock.rectWidth/2.0, 0.0)
goalFrame.PreMultiply()
goalFrame.Concatenate(goalOffset)
goalPosition = np.array(goalFrame.GetPosition())
if len(blocks) > 1:
goalFrame = transformUtils.copyFrame(blocks[-2].cornerTransform)
goalFrame.Translate(goalPosition - np.array(goalFrame.GetPosition()))
vis.updateFrame(goalFrame, 'footstep plan goal', scale=0.2)
request = self.footstepsPanel.driver.constructFootstepPlanRequest(robotPose, goalFrame)
assert standingFootName in (self.ikPlanner.leftFootLink, self.ikPlanner.rightFootLink)
if standingFootName == self.ikPlanner.rightFootLink:
leadingFoot = lcmdrc.footstep_plan_params_t.LEAD_RIGHT
else:
leadingFoot = lcmdrc.footstep_plan_params_t.LEAD_LEFT
request.params.leading_foot = leadingFoot
request.params.max_forward_step = 0.5
request.params.nom_forward_step = 0.12
request.params.nom_step_width = 0.22
request.params.max_num_steps = 8 #2*len(blocks)
plan = self.footstepsPanel.driver.sendFootstepPlanRequest(request, waitForResponse=True)
if not plan:
return []
#print 'received footstep plan with %d steps.' % len(plan.footsteps)
footsteps = []
for i, footstep in enumerate(plan.footsteps):
footstepTransform = self.transformFromFootstep(footstep)
footsteps.append(Footstep(footstepTransform, footstep.is_right_foot))
return footsteps[2:]
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 planTargetReach(self):
# A single one shot gaze-constrained reach: place xyz at goal and align y-axis of hand with x-axis of goal
worldToTargetFrame = vis.updateFrame(
self.targetFrame,
'gaze goal',
visible=False,
scale=0.2,
parent=om.getOrCreateContainer('affordances'))
self.initConstraintSet()
self.addConstraintForTargetFrame(worldToTargetFrame, 1)
self.planTrajectory()
def init(view):
global PointCloudQueue, _pointcloudItem, _pointcloudSource
PointCloudQueue = PythonQt.dd.ddPointCloudLCM(lcmUtils.getGlobalLCMThread())
PointCloudQueue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
_pointcloudSource = PointCloudSource(view, PointCloudQueue)
_pointcloudSource.start()
sensorsFolder = om.getOrCreateContainer('sensors')
_pointcloudItem = PointCloudItem(_pointcloudSource)
om.addToObjectModel(_pointcloudItem, sensorsFolder)
def init(view):
global KinectQueue, _kinectItem, _kinectSource
KinectQueue = PythonQt.dd.ddKinectLCM(lcmUtils.getGlobalLCMThread())
KinectQueue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
_kinectSource = KinectSource(view, KinectQueue)
_kinectSource.start()
sensorsFolder = om.getOrCreateContainer('sensors')
_kinectItem = KinectItem(_kinectSource)
om.addToObjectModel(_kinectItem, sensorsFolder)
def run(self):
folder = om.getOrCreateContainer('affordances')
frame = self.computeAffordanceFrame()
mesh = segmentation.getDrillMesh()
params = segmentation.getDrillAffordanceParams(np.array(frame.GetPosition()), [1,0,0], [0,1,0], [0,0,1])
affordance = vis.showPolyData(mesh, 'drill', color=[0.0, 1.0, 0.0], cls=affordanceitems.FrameAffordanceItem, parent=folder)
frame = vis.showFrame(frame, 'drill frame', parent=affordance, visible=False, scale=0.2)
affordance.actor.SetUserTransform(frame.transform)
affordance.setAffordanceParams(params)
affordance.updateParamsFromActorTransform()
def init(view):
global KinectQueue, _kinectItem, _kinectSource
KinectQueue = PythonQt.dd.ddKinectLCM(lcmUtils.getGlobalLCMThread())
KinectQueue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
_kinectSource = KinectSource(view, KinectQueue)
_kinectSource.start()
sensorsFolder = om.getOrCreateContainer('sensors')
_kinectItem = KinectItem(_kinectSource)
om.addToObjectModel(_kinectItem, sensorsFolder)
def newTerrainItem(self, tform, uid=None, region=None):
if uid is None:
uid = self.getNewUID()
elif uid in self.regions:
return self.regions[uid]
view = app.getCurrentRenderView()
item = TerrainRegionItem(uid, view, tform, self, region)
parentObj = om.getOrCreateContainer('Safe terrain regions')
om.addToObjectModel(item, parentObj)
self.regions[uid] = item
return item
def newTerrainItem(self, tform, uid=None, region=None):
if uid is None:
uid = self.getNewUID()
elif uid in self.regions:
return self.regions[uid]
view = app.getCurrentRenderView()
item = TerrainRegionItem(uid, view, tform, self, region)
parentObj = om.getOrCreateContainer('Safe terrain regions')
om.addToObjectModel(item, parentObj)
self.regions[uid] = item
return item
def init(view):
global PointCloudQueue, _pointcloudItem, _pointcloudSource
PointCloudQueue = PythonQt.dd.ddPointCloudLCM(
lcmUtils.getGlobalLCMThread())
PointCloudQueue.init(lcmUtils.getGlobalLCMThread(),
drcargs.args().config_file)
_pointcloudSource = PointCloudSource(view, PointCloudQueue)
_pointcloudSource.start()
sensorsFolder = om.getOrCreateContainer('sensors')
_pointcloudItem = PointCloudItem(_pointcloudSource)
om.addToObjectModel(_pointcloudItem, sensorsFolder)
def showFrame(frame, name, view=None, parent='segmentation', scale=0.35, visible=True):
view = view or app.getCurrentRenderView()
assert view
if isinstance(parent, str):
parentObj = om.getOrCreateContainer(parent)
else:
parentObj = parent
item = FrameItem(name, frame, view)
om.addToObjectModel(item, parentObj)
item.setProperty('Visible', visible)
item.setProperty('Scale', scale)
return item
def showText(text, name, fontSize=18, position=(10, 10), parent=None, view=None):
view = view or app.getCurrentRenderView()
assert view
item = TextItem(name, text, view=view)
item.setProperty('Font Size', fontSize)
item.setProperty('Position', list(position))
if isinstance(parent, str):
parentObj = om.getOrCreateContainer(parent)
else:
parentObj = parent
om.addToObjectModel(item, parentObj)
return item
def newPolyData(self, name, view, parent=None):
polyData = self.getNewHandPolyData()
if isinstance(parent, str):
parent = om.getOrCreateContainer(parent)
color = [1.0, 1.0, 0.0]
if self.side == 'right':
color = [0.33, 1.0, 0.0]
obj = vis.showPolyData(polyData, name, view=view, color=color, visible=False, parent=parent)
obj.side = self.side
frame = vtk.vtkTransform()
frame.PostMultiply()
obj.actor.SetUserTransform(frame)
frameObj = vis.showFrame(frame, '%s frame' % name, view=view, scale=0.2, visible=False, parent=obj)
return obj
def run(self):
radius = self.properties.getProperty('Radius')
thickness = 0.03
folder = om.getOrCreateContainer('affordances')
frame = self.computeValveFrame()
d = DebugData()
d.addLine(np.array([0, 0, -thickness/2.0]), np.array([0, 0, thickness/2.0]), radius=radius)
mesh = d.getPolyData()
params = dict(radius=radius, length=thickness, xwidth=radius, ywidth=radius, zwidth=thickness, otdf_type='steering_cyl', friendly_name='valve')
affordance = vis.showPolyData(mesh, 'valve', color=[0.0, 1.0, 0.0], cls=affordanceitems.FrameAffordanceItem, parent=folder, alpha=1.0)
frame = vis.showFrame(frame, 'valve frame', parent=affordance, visible=False, scale=radius)
affordance.actor.SetUserTransform(frame.transform)
affordance.setAffordanceParams(params)
affordance.updateParamsFromActorTransform()
def spawnTargetAffordance(self):
for obj in om.getObjects():
if obj.getProperty('Name') == 'target':
om.removeFromObjectModel(obj)
targetFrame = transformUtils.frameFromPositionAndRPY([0.6, 0.2, 0.6],
[180, 0, 90])
folder = om.getOrCreateContainer('affordances')
z = DebugData()
z.addLine(np.array([0, 0, 0]),
np.array([-self.boxLength, 0, 0]),
radius=0.02) # main bar
z.addLine(np.array([-self.boxLength, 0, 0]),
np.array([-self.boxLength, 0, self.boxLength]),
radius=0.02) # main bar
z.addLine(np.array([-self.boxLength, 0, self.boxLength]),
np.array([0, 0, self.boxLength]),
radius=0.02) # main bar
z.addLine(np.array([0, 0, self.boxLength]),
np.array([0, 0, 0]),
radius=0.02) # main bar
targetMesh = z.getPolyData()
self.targetAffordance = vis.showPolyData(
targetMesh,
'target',
color=[0.0, 1.0, 0.0],
cls=affordanceitems.FrameAffordanceItem,
parent=folder,
alpha=0.3)
self.targetAffordance.actor.SetUserTransform(targetFrame)
self.targetFrame = vis.showFrame(targetFrame,
'target frame',
parent=self.targetAffordance,
visible=False,
scale=0.2)
self.targetFrame = self.targetFrame.transform
params = dict(length=self.boxLength,
otdf_type='target',
friendly_name='target')
self.targetAffordance.setAffordanceParams(params)
self.targetAffordance.updateParamsFromActorTransform()
def newPolyData(self, name, view, parent=None):
self.handModel.model.setJointPositions(np.zeros(self.handModel.model.numberOfJoints()))
polyData = vtk.vtkPolyData()
self.handModel.model.getModelMesh(polyData)
polyData = filterUtils.transformPolyData(polyData, self.modelToPalm)
if isinstance(parent, str):
parent = om.getOrCreateContainer(parent)
color = [1.0, 1.0, 0.0]
if self.side == 'right':
color = [0.33, 1.0, 0.0]
obj = vis.showPolyData(polyData, name, view=view, color=color, visible=False, parent=parent)
obj.side = self.side
frame = vtk.vtkTransform()
frame.PostMultiply()
obj.actor.SetUserTransform(frame)
frameObj = vis.showFrame(frame, '%s frame' % name, view=view, scale=0.2, visible=False, parent=obj)
return obj
def onShowMapButton(self):
# reloads the map each time - in case its changed on disk:
#if (self.octomap_cloud is None):
filename = ddapp.getDRCBaseDir() + "/software/build/data/octomap.pcd"
self.octomap_cloud = io.readPolyData(filename) # c++ object called vtkPolyData
assert (self.octomap_cloud.GetNumberOfPoints() !=0 )
# clip point cloud to height - doesnt work very well yet. need to know robot's height
#self.octomap_cloud = segmentation.cropToLineSegment(self.octomap_cloud, np.array([0,0,-10]), np.array([0,0,3]) )
# access to z values
#points= vnp.getNumpyFromVtk(self.octomap_cloud, 'Points')
#zvalues = points[:,2]
# remove previous map:
folder = om.getOrCreateContainer("segmentation")
om.removeFromObjectModel(folder)
vis.showPolyData(self.octomap_cloud, 'prior map', alpha=1.0, color=[0,0,0.4])
def onShowMapButton(self):
# reloads the map each time - in case its changed on disk:
#if (self.octomap_cloud is None):
filename = ddapp.getDRCBaseDir() + "/software/build/data/octomap.pcd"
self.octomap_cloud = io.readPolyData(filename) # c++ object called vtkPolyData
assert (self.octomap_cloud.GetNumberOfPoints() !=0 )
# clip point cloud to height - doesnt work very well yet. need to know robot's height
#self.octomap_cloud = segmentation.cropToLineSegment(self.octomap_cloud, np.array([0,0,-10]), np.array([0,0,3]) )
# access to z values
#points= vnp.getNumpyFromVtk(self.octomap_cloud, 'Points')
#zvalues = points[:,2]
# remove previous map:
folder = om.getOrCreateContainer("segmentation")
om.removeFromObjectModel(folder)
vis.showPolyData(self.octomap_cloud, 'prior map', alpha=1.0, color=[0,0,0.4])
def addCollisionObject(self, obj):
if om.getOrCreateContainer('affordances').findChild(obj.getProperty('Name') + ' affordance'):
return # Affordance has been created previously
frame = obj.findChild(obj.getProperty('Name') + ' frame')
(origin, quat) = transformUtils.poseFromTransform(frame.transform)
(xaxis, yaxis, zaxis) = transformUtils.getAxesFromTransform(frame.transform)
# TODO: move this into transformUtils as getAxisDimensions or so
box = obj.findChild(obj.getProperty('Name') + ' box')
box_np = vtkNumpy.getNumpyFromVtk(box.polyData, 'Points')
box_min = np.amin(box_np, 0)
box_max = np.amax(box_np, 0)
xwidth = np.linalg.norm(box_max[0]-box_min[0])
ywidth = np.linalg.norm(box_max[1]-box_min[1])
zwidth = np.linalg.norm(box_max[2]-box_min[2])
name = obj.getProperty('Name') + ' affordance'
boxAffordance = segmentation.createBlockAffordance(origin, xaxis, yaxis, zaxis, xwidth, ywidth, zwidth, name, parent='affordances')
boxAffordance.setSolidColor(obj.getProperty('Color'))
boxAffordance.setProperty('Alpha', 0.3)
def run(self):
folder = om.getOrCreateContainer('affordances')
frame = self.computeAffordanceFrame()
mesh = segmentation.getDrillMesh()
params = segmentation.getDrillAffordanceParams(
np.array(frame.GetPosition()), [1, 0, 0], [0, 1, 0], [0, 0, 1])
affordance = vis.showPolyData(mesh,
'drill',
color=[0.0, 1.0, 0.0],
cls=affordanceitems.FrameAffordanceItem,
parent=folder)
frame = vis.showFrame(frame,
'drill frame',
parent=affordance,
visible=False,
scale=0.2)
affordance.actor.SetUserTransform(frame.transform)
affordance.setAffordanceParams(params)
affordance.updateParamsFromActorTransform()
def autonomousExecuteRoomMap(self):
self.graspingHand = 'left'
self.targetSweepType = 'orientation'
self.graspToHandLinkFrame = self.ikPlanner.newGraspToHandFrame(self.graspingHand)
self.planFromCurrentRobotState = True
self.visOnly = False
self.ikPlanner.ikServer.maxDegreesPerSecond = 3#5
self.currentYawDegrees = 60
self.fromTop = True
self.mapFolder=om.getOrCreateContainer('room mapping')
taskQueue = AsyncTaskQueue()
self.addTasksToQueueSweep(taskQueue)
self.addTasksToQueueSweep(taskQueue)
self.addTasksToQueueSweep(taskQueue)
self.addTasksToQueueSweep(taskQueue)
self.addTasksToQueueSweep(taskQueue)
self.addTasksToQueueSweep(taskQueue)
self.addTasksToQueueSweep(taskQueue)
taskQueue.addTask(self.printAsync('done!'))
taskQueue.addTask(self.doneIndicator)
return taskQueue