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 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 getDebugFolder():
obj = om.findObjectByName("debug")
if obj is None:
obj = om.getOrCreateContainer("debug",
om.getOrCreateContainer("segmentation"))
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 getDebugFolder():
obj = om.findObjectByName('debug')
if obj is None:
obj = om.getOrCreateContainer('debug',
om.getOrCreateContainer('segmentation'))
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 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 getDebugFolder():
obj = om.findObjectByName('debug')
if obj is None:
obj = om.getOrCreateContainer('debug', om.getOrCreateContainer('segmentation'))
om.collapse(obj)
return obj
def getWalkingVolumesFolder():
obj = om.findObjectByName('walking volumes')
if obj is None:
obj = om.getOrCreateContainer('walking volumes', parentObj=getFootstepsFolder())
om.collapse(obj)
return obj
