def initialize(self):
self.initializedFlag = True
self.updateFramesAndAffordance()
self.setFoostepData()
self.setFootstepDataForwards()
self.footstepRequestGenerator = FootstepRequestGenerator(
self.robotSystem.footstepsDriver)
#define the frames we need relative to the box frame etc
self.numFootsteps = 2
self.minHoldTime = 2
def initialize(self):
self.initializedFlag = True
self.updateFramesAndAffordance()
self.setFoostepData()
self.setFootstepDataForwards()
self.footstepRequestGenerator = FootstepRequestGenerator(self.robotSystem.footstepsDriver)
#define the frames we need relative to the box frame etc
self.numFootsteps = 2
self.minHoldTime = 2
class PolarisPlatformPlanner(object):
def __init__(self, ikServer, robotSystem):
self.ikServer = ikServer
self.robotSystem = robotSystem
self.terrainTask = terraintask.TerrainTask(robotSystem)
self.initializedFlag = False
self.plans = []
def initialize(self):
self.initializedFlag = True
self.updateFramesAndAffordance()
self.setFoostepData()
self.setFootstepDataForwards()
self.footstepRequestGenerator = FootstepRequestGenerator(self.robotSystem.footstepsDriver)
#define the frames we need relative to the box frame etc
self.numFootsteps = 2
self.minHoldTime = 2
def updateAffordance(self):
self.platform = om.findObjectByName('running board')
self.dimensions = np.array(self.platform.getProperty('Dimensions'))
def updateFramesAndAffordance(self):
self.updateAffordance()
self.getPlanningFrame()
self.requestRaycastTerrain()
# this method should set the planning frame
def getPlanningFrame(self):
platformToWorld = self.platform.getChildFrame().transform
worldToPlatform = platformToWorld.GetLinearInverse()
f = self.robotSystem.footstepsDriver.getFeetMidPoint(self.robotSystem.robotStateModel)
footPosition = f.GetPosition()
footPosInPlatformFrame = worldToPlatform.TransformPoint(footPosition)
planFramePosInPlatformFrame = self.dimensions/2.0
planFramePosInPlatformFrame[1] = footPosInPlatformFrame[1]
planFramePosInWorld = platformToWorld.TransformPoint(planFramePosInPlatformFrame)
# now we want to find the homogeneous transform for the planning Frame
_,quat = transformUtils.poseFromTransform(platformToWorld)
self.planToWorld = transformUtils.transformFromPose(planFramePosInWorld,quat)
# returns the f to plan transform, given fToWorld transform
def getTransformToPlanningFrame(self,fToWorld):
fToPlan = fToWorld
fToPlan.PostMultiply()
fToPlan.Concatenate(self.planToWorld.GetLinearInverse())
return fToPlan
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 visFootstepFrames(self):
for n in xrange(1,self.numFootsteps + 1):
fToPlan = self.footstepsToPlan[n-1]
fToWorld = fToPlan
fToWorld.PostMultiply()
fToWorld.Concatenate(self.planToWorld)
frameName = 'step_'+str(n)+'ToWorld'
vis.updateFrame(fToWorld,frameName)
def setFoostepData(self):
self.footstepPosition = []
self.footstepPosition.append(np.array([-0.19052019522393965, -0.16752527574088918, 0.07678844136281959 ]))
self.footstepPosition.append(np.array([-0.2111150611750166, 0.1621390575248655, 0.07571540514427666]))
self.footstepPosition.append(np.array([ 0.19315482042625953, -0.2866541182385602, 0.016873465171285976]))
self.footstepPosition.append(np.array([ 0.13708399594888881, 0.1522408848113495, 0.008706862136780541 ]))
# note that this is in radians, transform to degrees
self.footstepYaw = np.array([-0.77413819, -0.57931552, -0.75042088, -0.68140433])
self.footstepYaw = np.rad2deg(self.footstepYaw)
def setFootstepDataForwards(self):
self.footstepPositionForwards = []
self.footstepPositionForwards.append(np.array([-0.08774644, 0.0635555 , 0.07771066])) # narrow first step
# self.footstepPositionForwards.append(np.array([-0.06954156, 0.14726368, 0.07522517])) # normal first step
self.footstepPositionForwards.append(np.array([ 0.18256867, -0.11692981, 0.01602283]))
self.footstepPositionForwards.append(np.array([ 0.31539397, 0.15317327, 0.04011487]))
self.footstepYawForwards = np.array([0, 0, 0])
self.footstepYawForwards = np.rad2deg(self.footstepYawForwards)
def planTurn(self):
# request footsteps 1 and 2
footstepsToWorldList = self.getFootstepToWorldTransforms([0,1])
q = self.robotSystem.robotStateJointController.q
request = self.footstepRequestGenerator.makeFootstepRequest(q, footstepsToWorldList, 'right', snapToTerrain=True)
request.params.map_mode = lcmdrc.footstep_plan_params_t.TERRAIN_HEIGHTS_AND_NORMALS
request = self.setMapModeToTerrainAndNormals(request)
self.robotSystem.footstepsDriver.sendFootstepPlanRequest(request)
def planStepDown(self):
footstepsToWorldList = self.getFootstepToWorldTransforms([3])
q = self.robotSystem.robotStateJointController.q
request = self.footstepRequestGenerator.makeFootstepRequest(q, footstepsToWorldList, 'left', snapToTerrain=True)
self.robotSystem.footstepsDriver.sendFootstepPlanRequest(request)
def planStepOff(self):
footstepsToWorldList = self.getFootstepToWorldTransforms([2])
q = self.robotSystem.robotStateJointController.q
request = self.footstepRequestGenerator.makeFootstepRequest(q, footstepsToWorldList, 'right', snapToTerrain=True)
self.robotSystem.footstepsDriver.sendFootstepPlanRequest(request)
def planWeightShift(self):
ikPlanner = self.robotSystem.ikPlanner
startPoseName = 'plan_start'
endPoseName = 'plan_end'
startPose = self.robotSystem.robotStateJointController.q
ikPlanner.addPose(startPose, startPoseName)
constraints = ikPlanner.createMovingBodyConstraints(startPoseName, lockBack=True, lockLeftArm=True, lockRightArm=True)
constraints[0].rightFootEnabled = False
constraints[0].shrinkFactor=0.1
constraints.append(ikPlanner.createKneePostureConstraint([1, 2.5]))
cs = ikplanner.ConstraintSet(ikPlanner, constraints, endPoseName, startPoseName)
endPose, info = cs.runIk()
ikPlanner.computeMultiPostureGoal([startPose, endPose])
# maybe should update the frame and affordance everytime we call a method?
def planStepDownForwards(self):
# need to make us step left foot forwards
# set the walking defaults to be what we want
q = self.getPlanningStartPose()
footstepsToWorldList = self.getFootstepToWorldTransforms([0,1], stepOffDirection='forwards')
request = self.footstepRequestGenerator.makeFootstepRequest(q, footstepsToWorldList, 'left', snapToTerrain=True)
request.goal_steps[0].params.support_contact_groups = lcmdrc.footstep_params_t.SUPPORT_GROUPS_HEEL_MIDFOOT
self.robotSystem.footstepsDriver.sendFootstepPlanRequest(request)
def planStepOffForwards(self):
q = self.getPlanningStartPose()
footstepsToWorldList = self.getFootstepToWorldTransforms([2], stepOffDirection='forwards')
request = self.footstepRequestGenerator.makeFootstepRequest(q, footstepsToWorldList, 'left', snapToTerrain=True)
self.robotSystem.footstepsDriver.sendFootstepPlanRequest(request)
def planWeightShiftForwards(self):
pass
def setMinHoldTime(self, request, minHoldTime):
for stepMessages in request.goal_steps:
stepMessages.params.drake_min_hold_time = minHoldTime
return request
def switchToPolarisPlatformParameters(self):
self.robotSystem.footstepsDriver.params.setProperty('Defaults', 'Polaris Platform')
#get the footsteps to world transform from footstepsToPlan transform
def getFootstepToWorldTransforms(self,footstepIdx, stepOffDirection='sideways'):
self.updateFramesAndAffordance()
footstepsToWorldList = []
for j in footstepIdx:
if stepOffDirection == 'sideways':
rpy = np.array([0,0,self.footstepYaw[j]])
position = self.footstepPosition[j]
else:
rpy = np.array([0,0,0], self.footstepYawForwards[j])
position = self.footstepPositionForwards[j]
footstepToPlan = transformUtils.frameFromPositionAndRPY(position,rpy)
footstepToWorld = footstepToPlan
footstepToWorld.PostMultiply();
footstepToWorld.Concatenate(self.planToWorld)
footstepsToWorldList.append(footstepToWorld)
return footstepsToWorldList
def setMapModeToTerrainAndNormals(self,request):
request.params.map_mode = lcmdrc.footstep_plan_params_t.TERRAIN_HEIGHTS_AND_NORMALS
return request
def spawnRunningBoardAffordance(self):
boxDimensions = [0.4, 1.0, 0.05]
stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(self.robotSystem.robotStateModel, useWorldZ=False)
boxFrame = transformUtils.copyFrame(stanceFrame)
boxFrame.PreMultiply()
boxFrame.Translate(0.0, 0.0, -boxDimensions[2]/2.0)
box = om.findObjectByName('running board')
if not box:
pose = transformUtils.poseFromTransform(boxFrame)
desc = dict(classname='BoxAffordanceItem', Name='running board', Dimensions=boxDimensions, pose=pose)
box = self.robotSystem.affordanceManager.newAffordanceFromDescription(desc)
return box
def fitRunningBoardAtFeet(self):
# get stance frame
startPose = self.getPlanningStartPose()
stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(self.robotSystem.robotStateModel, useWorldZ=False)
stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame)
# get pointcloud and extract search region covering the running board
polyData = segmentation.getCurrentRevolutionData()
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
_, polyData = segmentation.removeGround(polyData)
polyData = segmentation.cropToBox(polyData, stanceFrame, [1.0, 1.0, 0.1])
if not polyData.GetNumberOfPoints():
print 'empty search region point cloud'
return
vis.updatePolyData(polyData, 'running board search points', parent=segmentation.getDebugFolder(), color=[0,1,0], visible=False)
# extract maximal points along the stance x axis
perpAxis = stanceFrameAxes[0]
edgeAxis = stanceFrameAxes[1]
edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis)
edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints)
vis.updatePolyData(edgePoints, 'edge points', parent=segmentation.getDebugFolder(), visible=True)
# ransac fit a line to the edge points
linePoint, lineDirection, fitPoints = segmentation.applyLineFit(edgePoints)
if np.dot(lineDirection, stanceFrameAxes[1]) < 0:
lineDirection = -lineDirection
linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels', [1.0, 1.0])
dists = np.dot(vnp.getNumpyFromVtk(linePoints, 'Points')-linePoint, lineDirection)
p1 = linePoint + lineDirection*np.min(dists)
p2 = linePoint + lineDirection*np.max(dists)
vis.updatePolyData(fitPoints, 'line fit points', parent=segmentation.getDebugFolder(), colorByName='ransac_labels', visible=False)
# compute a new frame that is in plane with the stance frame
# and matches the orientation and position of the detected edge
origin = np.array(stanceFrame.GetPosition())
normal = np.array(stanceFrameAxes[2])
# project stance origin to edge, then back to foot frame
originProjectedToEdge = linePoint + lineDirection*np.dot(origin - linePoint, lineDirection)
originProjectedToPlane = segmentation.projectPointToPlane(originProjectedToEdge, origin, normal)
zaxis = np.array(stanceFrameAxes[2])
yaxis = np.array(lineDirection)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
d = DebugData()
d.addSphere(p1, radius=0.005)
d.addSphere(p2, radius=0.005)
d.addLine(p1, p2)
d.addSphere(originProjectedToEdge, radius=0.001, color=[1,0,0])
d.addSphere(originProjectedToPlane, radius=0.001, color=[0,1,0])
d.addLine(originProjectedToPlane, origin, color=[0,1,0])
d.addLine(originProjectedToEdge, origin, color=[1,0,0])
vis.updatePolyData(d.getPolyData(), 'running board edge', parent=segmentation.getDebugFolder(), colorByName='RGB255', visible=False)
# update the running board box affordance position and orientation to
# fit the detected edge
box = self.spawnRunningBoardAffordance()
boxDimensions = box.getProperty('Dimensions')
t = transformUtils.getTransformFromAxesAndOrigin(xaxis, yaxis, zaxis, originProjectedToPlane)
t.PreMultiply()
t.Translate(-boxDimensions[0]/2.0, 0.0, -boxDimensions[2]/2.0)
box.getChildFrame().copyFrame(t)
self.initialize()
#passthrough methods to the terrain task
# should force updating the affordance before doing this
def requestRaycastTerrain(self):
self.terrainTask.requestRaycastTerrain()
def spawnGroundAffordance(self):
self.terrainTask.spawnGroundAffordance()
def spawnFootplaneGroundAffordance(self):
self.terrainTask.spawnFootplaneGroundAffordance('right')
def planArmsUp(self, stepOffDirection):
ikPlanner = self.robotSystem.ikPlanner
startPose = self.getPlanningStartPose()
if stepOffDirection == 'forwards':
endPose = ikPlanner.getMergedPostureFromDatabase(startPose, 'General', 'hands-forward', side='left')
endPose = ikPlanner.getMergedPostureFromDatabase(endPose, 'General', 'hands-forward', side='right')
else:
endPose = ikPlanner.getMergedPostureFromDatabase(startPose, 'General', 'polaris_step_arm_safe', side='left')
endPose = ikPlanner.getMergedPostureFromDatabase(endPose, 'General', 'polaris_step_arm_safe', side='right')
plan = ikPlanner.computeMultiPostureGoal([startPose, endPose])
self.addPlan(plan)
def addPlan(self, plan):
self.plans.append(plan)
def commitManipPlan(self):
self.robotSystem.manipPlanner.commitManipPlan(self.plans[-1])
def getPlanningStartPose(self):
return self.robotSystem.robotStateJointController.q.copy()
def planNominal(self):
ikPlanner = self.robotSystem.ikPlanner
startPose = self.getPlanningStartPose()
endPose = ikPlanner.getMergedPostureFromDatabase(startPose, 'General', 'safe nominal')
endPose, info = ikPlanner.computeStandPose(endPose)
newPlan = ikPlanner.computePostureGoal(startPose, endPose)
self.addPlan(newPlan)
def addPlan(self, plan):
self.plans.append(plan)
class PolarisPlatformPlanner(object):
def __init__(self, ikServer, robotSystem):
self.ikServer = ikServer
self.robotSystem = robotSystem
self.terrainTask = terraintask.TerrainTask(robotSystem)
self.initializedFlag = False
self.plans = []
def initialize(self):
self.initializedFlag = True
self.updateFramesAndAffordance()
self.setFoostepData()
self.setFootstepDataForwards()
self.footstepRequestGenerator = FootstepRequestGenerator(
self.robotSystem.footstepsDriver)
#define the frames we need relative to the box frame etc
self.numFootsteps = 2
self.minHoldTime = 2
def updateAffordance(self):
self.platform = om.findObjectByName('running board')
self.dimensions = np.array(self.platform.getProperty('Dimensions'))
def updateFramesAndAffordance(self):
self.updateAffordance()
self.getPlanningFrame()
self.requestRaycastTerrain()
# this method should set the planning frame
def getPlanningFrame(self):
platformToWorld = self.platform.getChildFrame().transform
worldToPlatform = platformToWorld.GetLinearInverse()
f = self.robotSystem.footstepsDriver.getFeetMidPoint(
self.robotSystem.robotStateModel)
footPosition = f.GetPosition()
footPosInPlatformFrame = worldToPlatform.TransformPoint(footPosition)
planFramePosInPlatformFrame = self.dimensions / 2.0
planFramePosInPlatformFrame[1] = footPosInPlatformFrame[1]
planFramePosInWorld = platformToWorld.TransformPoint(
planFramePosInPlatformFrame)
# now we want to find the homogeneous transform for the planning Frame
_, quat = transformUtils.poseFromTransform(platformToWorld)
self.planToWorld = transformUtils.transformFromPose(
planFramePosInWorld, quat)
# returns the f to plan transform, given fToWorld transform
def getTransformToPlanningFrame(self, fToWorld):
fToPlan = fToWorld
fToPlan.PostMultiply()
fToPlan.Concatenate(self.planToWorld.GetLinearInverse())
return fToPlan
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 visFootstepFrames(self):
for n in xrange(1, self.numFootsteps + 1):
fToPlan = self.footstepsToPlan[n - 1]
fToWorld = fToPlan
fToWorld.PostMultiply()
fToWorld.Concatenate(self.planToWorld)
frameName = 'step_' + str(n) + 'ToWorld'
vis.updateFrame(fToWorld, frameName)
def setFoostepData(self):
self.footstepPosition = []
self.footstepPosition.append(
np.array([
-0.19052019522393965, -0.16752527574088918, 0.07678844136281959
]))
self.footstepPosition.append(
np.array(
[-0.2111150611750166, 0.1621390575248655,
0.07571540514427666]))
self.footstepPosition.append(
np.array([
0.19315482042625953, -0.2866541182385602, 0.016873465171285976
]))
self.footstepPosition.append(
np.array([
0.13708399594888881, 0.1522408848113495, 0.008706862136780541
]))
# note that this is in radians, transform to degrees
self.footstepYaw = np.array(
[-0.77413819, -0.57931552, -0.75042088, -0.68140433])
self.footstepYaw = np.rad2deg(self.footstepYaw)
def setFootstepDataForwards(self):
self.footstepPositionForwards = []
self.footstepPositionForwards.append(
np.array([-0.08774644, 0.0635555,
0.07771066])) # narrow first step
# self.footstepPositionForwards.append(np.array([-0.06954156, 0.14726368, 0.07522517])) # normal first step
self.footstepPositionForwards.append(
np.array([0.18256867, -0.11692981, 0.01602283]))
self.footstepPositionForwards.append(
np.array([0.31539397, 0.15317327, 0.04011487]))
self.footstepYawForwards = np.array([0, 0, 0])
self.footstepYawForwards = np.rad2deg(self.footstepYawForwards)
def planTurn(self):
# request footsteps 1 and 2
footstepsToWorldList = self.getFootstepToWorldTransforms([0, 1])
q = self.robotSystem.robotStateJointController.q
request = self.footstepRequestGenerator.makeFootstepRequest(
q, footstepsToWorldList, 'right', snapToTerrain=True)
request.params.map_mode = lcmdrc.footstep_plan_params_t.TERRAIN_HEIGHTS_AND_NORMALS
request = self.setMapModeToTerrainAndNormals(request)
self.robotSystem.footstepsDriver.sendFootstepPlanRequest(request)
def planStepDown(self):
footstepsToWorldList = self.getFootstepToWorldTransforms([3])
q = self.robotSystem.robotStateJointController.q
request = self.footstepRequestGenerator.makeFootstepRequest(
q, footstepsToWorldList, 'left', snapToTerrain=True)
self.robotSystem.footstepsDriver.sendFootstepPlanRequest(request)
def planStepOff(self):
footstepsToWorldList = self.getFootstepToWorldTransforms([2])
q = self.robotSystem.robotStateJointController.q
request = self.footstepRequestGenerator.makeFootstepRequest(
q, footstepsToWorldList, 'right', snapToTerrain=True)
self.robotSystem.footstepsDriver.sendFootstepPlanRequest(request)
def planWeightShift(self):
ikPlanner = self.robotSystem.ikPlanner
startPoseName = 'plan_start'
endPoseName = 'plan_end'
startPose = self.robotSystem.robotStateJointController.q
ikPlanner.addPose(startPose, startPoseName)
constraints = ikPlanner.createMovingBodyConstraints(startPoseName,
lockBack=True,
lockLeftArm=True,
lockRightArm=True)
constraints[0].rightFootEnabled = False
constraints[0].shrinkFactor = 0.1
constraints.append(ikPlanner.createKneePostureConstraint([1, 2.5]))
cs = ikplanner.ConstraintSet(ikPlanner, constraints, endPoseName,
startPoseName)
endPose, info = cs.runIk()
ikPlanner.computeMultiPostureGoal([startPose, endPose])
# maybe should update the frame and affordance everytime we call a method?
def planStepDownForwards(self):
# need to make us step left foot forwards
# set the walking defaults to be what we want
q = self.getPlanningStartPose()
footstepsToWorldList = self.getFootstepToWorldTransforms(
[0, 1], stepOffDirection='forwards')
request = self.footstepRequestGenerator.makeFootstepRequest(
q, footstepsToWorldList, 'left', snapToTerrain=True)
request.goal_steps[
0].params.support_contact_groups = lcmdrc.footstep_params_t.SUPPORT_GROUPS_HEEL_MIDFOOT
self.robotSystem.footstepsDriver.sendFootstepPlanRequest(request)
def planStepOffForwards(self):
q = self.getPlanningStartPose()
footstepsToWorldList = self.getFootstepToWorldTransforms(
[2], stepOffDirection='forwards')
request = self.footstepRequestGenerator.makeFootstepRequest(
q, footstepsToWorldList, 'left', snapToTerrain=True)
self.robotSystem.footstepsDriver.sendFootstepPlanRequest(request)
def planWeightShiftForwards(self):
pass
def setMinHoldTime(self, request, minHoldTime):
for stepMessages in request.goal_steps:
stepMessages.params.drake_min_hold_time = minHoldTime
return request
def switchToPolarisPlatformParameters(self):
self.robotSystem.footstepsDriver.params.setProperty(
'Defaults', 'Polaris Platform')
#get the footsteps to world transform from footstepsToPlan transform
def getFootstepToWorldTransforms(self,
footstepIdx,
stepOffDirection='sideways'):
self.updateFramesAndAffordance()
footstepsToWorldList = []
for j in footstepIdx:
if stepOffDirection == 'sideways':
rpy = np.array([0, 0, self.footstepYaw[j]])
position = self.footstepPosition[j]
else:
rpy = np.array([0, 0, 0], self.footstepYawForwards[j])
position = self.footstepPositionForwards[j]
footstepToPlan = transformUtils.frameFromPositionAndRPY(
position, rpy)
footstepToWorld = footstepToPlan
footstepToWorld.PostMultiply()
footstepToWorld.Concatenate(self.planToWorld)
footstepsToWorldList.append(footstepToWorld)
return footstepsToWorldList
def setMapModeToTerrainAndNormals(self, request):
request.params.map_mode = lcmdrc.footstep_plan_params_t.TERRAIN_HEIGHTS_AND_NORMALS
return request
def spawnRunningBoardAffordance(self):
boxDimensions = [0.4, 1.0, 0.05]
stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(
self.robotSystem.robotStateModel, useWorldZ=False)
boxFrame = transformUtils.copyFrame(stanceFrame)
boxFrame.PreMultiply()
boxFrame.Translate(0.0, 0.0, -boxDimensions[2] / 2.0)
box = om.findObjectByName('running board')
if not box:
pose = transformUtils.poseFromTransform(boxFrame)
desc = dict(classname='BoxAffordanceItem',
Name='running board',
Dimensions=boxDimensions,
pose=pose)
box = self.robotSystem.affordanceManager.newAffordanceFromDescription(
desc)
return box
def fitRunningBoardAtFeet(self):
# get stance frame
startPose = self.getPlanningStartPose()
stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(
self.robotSystem.robotStateModel, useWorldZ=False)
stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame)
# get pointcloud and extract search region covering the running board
polyData = segmentation.getCurrentRevolutionData()
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
_, polyData = segmentation.removeGround(polyData)
polyData = segmentation.cropToBox(polyData, stanceFrame,
[1.0, 1.0, 0.1])
if not polyData.GetNumberOfPoints():
print 'empty search region point cloud'
return
vis.updatePolyData(polyData,
'running board search points',
parent=segmentation.getDebugFolder(),
color=[0, 1, 0],
visible=False)
# extract maximal points along the stance x axis
perpAxis = stanceFrameAxes[0]
edgeAxis = stanceFrameAxes[1]
edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis)
edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints)
vis.updatePolyData(edgePoints,
'edge points',
parent=segmentation.getDebugFolder(),
visible=True)
# ransac fit a line to the edge points
linePoint, lineDirection, fitPoints = segmentation.applyLineFit(
edgePoints)
if np.dot(lineDirection, stanceFrameAxes[1]) < 0:
lineDirection = -lineDirection
linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels',
[1.0, 1.0])
dists = np.dot(
vnp.getNumpyFromVtk(linePoints, 'Points') - linePoint,
lineDirection)
p1 = linePoint + lineDirection * np.min(dists)
p2 = linePoint + lineDirection * np.max(dists)
vis.updatePolyData(fitPoints,
'line fit points',
parent=segmentation.getDebugFolder(),
colorByName='ransac_labels',
visible=False)
# compute a new frame that is in plane with the stance frame
# and matches the orientation and position of the detected edge
origin = np.array(stanceFrame.GetPosition())
normal = np.array(stanceFrameAxes[2])
# project stance origin to edge, then back to foot frame
originProjectedToEdge = linePoint + lineDirection * np.dot(
origin - linePoint, lineDirection)
originProjectedToPlane = segmentation.projectPointToPlane(
originProjectedToEdge, origin, normal)
zaxis = np.array(stanceFrameAxes[2])
yaxis = np.array(lineDirection)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
d = DebugData()
d.addSphere(p1, radius=0.005)
d.addSphere(p2, radius=0.005)
d.addLine(p1, p2)
d.addSphere(originProjectedToEdge, radius=0.001, color=[1, 0, 0])
d.addSphere(originProjectedToPlane, radius=0.001, color=[0, 1, 0])
d.addLine(originProjectedToPlane, origin, color=[0, 1, 0])
d.addLine(originProjectedToEdge, origin, color=[1, 0, 0])
vis.updatePolyData(d.getPolyData(),
'running board edge',
parent=segmentation.getDebugFolder(),
colorByName='RGB255',
visible=False)
# update the running board box affordance position and orientation to
# fit the detected edge
box = self.spawnRunningBoardAffordance()
boxDimensions = box.getProperty('Dimensions')
t = transformUtils.getTransformFromAxesAndOrigin(
xaxis, yaxis, zaxis, originProjectedToPlane)
t.PreMultiply()
t.Translate(-boxDimensions[0] / 2.0, 0.0, -boxDimensions[2] / 2.0)
box.getChildFrame().copyFrame(t)
self.initialize()
#passthrough methods to the terrain task
# should force updating the affordance before doing this
def requestRaycastTerrain(self):
self.terrainTask.requestRaycastTerrain()
def spawnGroundAffordance(self):
self.terrainTask.spawnGroundAffordance()
def spawnFootplaneGroundAffordance(self):
self.terrainTask.spawnFootplaneGroundAffordance('right')
def planArmsUp(self, stepOffDirection):
ikPlanner = self.robotSystem.ikPlanner
startPose = self.getPlanningStartPose()
if stepOffDirection == 'forwards':
endPose = ikPlanner.getMergedPostureFromDatabase(startPose,
'General',
'hands-forward',
side='left')
endPose = ikPlanner.getMergedPostureFromDatabase(endPose,
'General',
'hands-forward',
side='right')
else:
endPose = ikPlanner.getMergedPostureFromDatabase(
startPose, 'General', 'polaris_step_arm_safe', side='left')
endPose = ikPlanner.getMergedPostureFromDatabase(
endPose, 'General', 'polaris_step_arm_safe', side='right')
plan = ikPlanner.computeMultiPostureGoal([startPose, endPose])
self.addPlan(plan)
def addPlan(self, plan):
self.plans.append(plan)
def commitManipPlan(self):
self.robotSystem.manipPlanner.commitManipPlan(self.plans[-1])
def getPlanningStartPose(self):
return self.robotSystem.robotStateJointController.q.copy()
def planNominal(self):
ikPlanner = self.robotSystem.ikPlanner
startPose = self.getPlanningStartPose()
endPose = ikPlanner.getMergedPostureFromDatabase(
startPose, 'General', 'safe nominal')
endPose, info = ikPlanner.computeStandPose(endPose)
newPlan = ikPlanner.computePostureGoal(startPose, endPose)
self.addPlan(newPlan)
def addPlan(self, plan):
self.plans.append(plan)
