def onVisualizeButton(self):
print "viz", self.ui.comboBox.currentText
frame = self.getFrameFromCombo()
#vis.showFrame(frame, self.getSelectedGoalName(), parent="navigation", scale=0.35, visible=True)
#vis.updateFrame(frame, self.getSelectedGoalName(), parent="navigation")
vis.updateFrame(frame, "Current Goal", parent="navigation")
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 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 computeBoardReachFrames(self):
''' Reach ~10cm short of the grasp frame '''
reachLeftXYZ = copy.deepcopy(self.graspLeftHandFrameXYZ)
reachLeftXYZ[0] = reachLeftXYZ[0] - self.reachDepth
reachLeftRPY = copy.deepcopy(self.graspLeftHandFrameRPY)
tl = transformUtils.frameFromPositionAndRPY(reachLeftXYZ, reachLeftRPY)
tl.Concatenate(self.frame.transform)
self.reachLeftFrame = vis.updateFrame(tl,
'reach left frame',
parent=self.affordance,
visible=False,
scale=0.2)
self.reachLeftFrame.addToView(app.getDRCView())
reachRightXYZ = copy.deepcopy(self.graspRightHandFrameXYZ)
reachRightXYZ[0] = reachRightXYZ[0] - self.reachDepth
reachRightRPY = copy.deepcopy(self.graspRightHandFrameRPY)
tr = transformUtils.frameFromPositionAndRPY(reachRightXYZ,
reachRightRPY)
tr.Concatenate(self.frame.transform)
self.reachRightFrame = vis.updateFrame(tr,
'reach right frame',
parent=self.affordance,
visible=False,
scale=0.2)
self.reachRightFrame.addToView(app.getDRCView())
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 computeBoardStanceFrame(self):
objectTransform = transformUtils.copyFrame(
self.graspLeftFrame.transform)
self.relativeStanceTransform = transformUtils.copyFrame(
transformUtils.frameFromPositionAndRPY(self.relativeStanceXYZ,
self.relativeStanceRPY))
robotStance = self.b.computeRobotStanceFrame(
objectTransform, self.relativeStanceTransform)
self.stanceFrame = vis.updateFrame(robotStance,
'board stance',
parent=self.affordance,
visible=False,
scale=0.2)
self.stanceFrame.addToView(app.getDRCView())
objectTransform = transformUtils.copyFrame(
self.graspLeftFrame.transform)
self.relativeAsymmetricStanceTransform = transformUtils.copyFrame(
transformUtils.frameFromPositionAndRPY(
self.relativeAsymmetricStanceXYZ, self.relativeStanceRPY))
robotAsymmetricStance = self.b.computeRobotStanceFrame(
objectTransform, self.relativeAsymmetricStanceTransform)
self.asymmetricStanceFrame = vis.updateFrame(robotAsymmetricStance,
'board Asymmetric stance',
parent=self.affordance,
visible=False,
scale=0.2)
self.asymmetricStanceFrame.addToView(app.getDRCView())
def removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[-np.inf, -0.03], whichAxis=1, whichAxisLetter='y', percentile = 95):
yvalues = vnp.getNumpyFromVtk(polyData, 'Points')[:, whichAxis]
backY = np.percentile(yvalues, percentile)
if ( percentile > 50):
searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [backY - 0.1, np.inf])
else:
searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [-np.inf, backY + 0.1])
vis.updatePolyData(searchRegion, 'search region', parent="segmentation", colorByName=whichAxisLetter, visible=False)
# find the plane of the back wall, remove it and the points behind it:
_, origin, normal = segmentation.applyPlaneFit(searchRegion, distanceThreshold=0.02, expectedNormal=expectedNormal, perpendicularAxis=expectedNormal, returnOrigin=True)
points = vnp.getNumpyFromVtk(polyData, 'Points')
dist = np.dot(points - origin, normal)
vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')
backFrame = transformUtils.getTransformFromOriginAndNormal(origin, normal, normalAxis=2)
vis.updateFrame(backFrame, 'back frame', parent='segmentation', scale=0.15 , visible=False)
vis.updatePolyData(polyData, 'dist to back', parent='segmentation', visible=False)
polyData = segmentation.thresholdPoints(polyData, 'dist_to_plane', filterRange)
vis.updatePolyData(polyData, 'back off and all', parent='segmentation', visible=False)
return polyData
def onVisualizeButton(self):
print "viz",self.ui.comboBox.currentText
frame = self.getFrameFromCombo()
#vis.showFrame(frame, self.getSelectedGoalName(), parent="navigation", scale=0.35, visible=True)
#vis.updateFrame(frame, self.getSelectedGoalName(), parent="navigation")
vis.updateFrame(frame, "Current Goal", parent="navigation")
def findFarRightCorner(self, polyData, linkFrame):
'''
Within a point cloud find the point to the far right from the link
The input is typically the 4 corners of a minimum bounding box
'''
diagonalTransform = transformUtils.frameFromPositionAndRPY([0,0,0], [0,0,45])
diagonalTransform.Concatenate(linkFrame)
vis.updateFrame(diagonalTransform, 'diagonal frame', parent='cont debug', visible=False)
#polyData = shallowCopy(polyData)
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
#vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x')
#vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y')
#vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z')
viewOrigin = diagonalTransform.TransformPoint([0.0, 0.0, 0.0])
viewX = diagonalTransform.TransformVector([1.0, 0.0, 0.0])
viewY = diagonalTransform.TransformVector([0.0, 1.0, 0.0])
viewZ = diagonalTransform.TransformVector([0.0, 0.0, 1.0])
#polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x')
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y')
#polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z')
vis.updatePolyData( polyData, 'cornerPoints', parent='cont debug', visible=False)
farRightIndex = vtkNumpy.getNumpyFromVtk(polyData, 'distance_along_foot_y').argmin()
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
return points[farRightIndex,:]
def getNextDoubleSupportPose(self, lfootTransform, rfootTransform):
vis.updateFrame(lfootTransform, 'lfootTransform', visible=True, scale=0.2)
vis.updateFrame(rfootTransform, 'rfootTransform', visible=True, scale=0.2)
startPose = self.robotStateJointController.getPose('EST_ROBOT_STATE')
startPoseName = 'stride_start'
self.ikPlanner.addPose(startPose, startPoseName)
constraints = []
# lock everything except the feet, constrain the feet
constraints.append(self.ikPlanner.createQuasiStaticConstraint())
constraints.append(self.ikPlanner.createMovingBackPostureConstraint())
constraints.append(self.ikPlanner.createMovingBasePostureConstraint(startPoseName))
constraints.append(self.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName))
constraints.append(self.ikPlanner.createLockedRightArmPostureConstraint(startPoseName))
nullFrame = vtk.vtkTransform()
positionConstraint, orientationConstraint = self.ikPlanner.createPositionOrientationConstraint(self.ikPlanner.rightFootLink, rfootTransform, nullFrame)
positionConstraint.tspan = [1.0, 1.0]
orientationConstraint.tspan = [1.0, 1.0]
constraints.append(positionConstraint)
constraints.append(orientationConstraint)
positionConstraint, orientationConstraint = self.ikPlanner.createPositionOrientationConstraint(self.ikPlanner.leftFootLink, lfootTransform, nullFrame)
positionConstraint.tspan = [1.0, 1.0]
orientationConstraint.tspan = [1.0, 1.0]
constraints.append(positionConstraint)
constraints.append(orientationConstraint)
constraintSet = ikplanner.ConstraintSet(self.ikPlanner, constraints, 'stride_end', startPoseName)
nextDoubleSupportPose, info = constraintSet.runIk()
return nextDoubleSupportPose
def getNextDoubleSupportPose(self, lfootTransform, rfootTransform):
vis.updateFrame(lfootTransform, 'lfootTransform', visible=True, scale=0.2)
vis.updateFrame(rfootTransform, 'rfootTransform', visible=True, scale=0.2)
startPose = self.robotStateJointController.getPose('EST_ROBOT_STATE')
startPoseName = 'stride_start'
self.ikPlanner.addPose(startPose, startPoseName)
constraints = []
# lock everything except the feet, constrain the feet
constraints.append(self.ikPlanner.createQuasiStaticConstraint())
constraints.append(self.ikPlanner.createMovingBackPostureConstraint())
constraints.append(self.ikPlanner.createMovingBasePostureConstraint(startPoseName))
constraints.append(self.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName))
constraints.append(self.ikPlanner.createLockedRightArmPostureConstraint(startPoseName))
nullFrame = vtk.vtkTransform()
positionConstraint, orientationConstraint = self.ikPlanner.createPositionOrientationConstraint(self.ikPlanner.rightFootLink, rfootTransform, nullFrame)
positionConstraint.tspan = [1.0, 1.0]
orientationConstraint.tspan = [1.0, 1.0]
constraints.append(positionConstraint)
constraints.append(orientationConstraint)
positionConstraint, orientationConstraint = self.ikPlanner.createPositionOrientationConstraint(self.ikPlanner.leftFootLink, lfootTransform, nullFrame)
positionConstraint.tspan = [1.0, 1.0]
orientationConstraint.tspan = [1.0, 1.0]
constraints.append(positionConstraint)
constraints.append(orientationConstraint)
constraintSet = ikplanner.ConstraintSet(self.ikPlanner, constraints, 'stride_end', startPoseName)
nextDoubleSupportPose, info = constraintSet.runIk()
return nextDoubleSupportPose
def findFarRightCorner(self, polyData, linkFrame):
'''
Within a point cloud find the point to the far right from the link
The input is typically the 4 corners of a minimum bounding box
'''
diagonalTransform = transformUtils.frameFromPositionAndRPY([0,0,0], [0,0,45])
diagonalTransform.Concatenate(linkFrame)
vis.updateFrame(diagonalTransform, 'diagonal frame', parent='cont debug', visible=False)
#polyData = shallowCopy(polyData)
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
#vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x')
#vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y')
#vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z')
viewOrigin = diagonalTransform.TransformPoint([0.0, 0.0, 0.0])
viewX = diagonalTransform.TransformVector([1.0, 0.0, 0.0])
viewY = diagonalTransform.TransformVector([0.0, 1.0, 0.0])
viewZ = diagonalTransform.TransformVector([0.0, 0.0, 1.0])
#polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x')
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y')
#polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z')
vis.updatePolyData( polyData, 'cornerPoints', parent='cont debug', visible=False)
farRightIndex = vtkNumpy.getNumpyFromVtk(polyData, 'distance_along_foot_y').argmin()
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
return points[farRightIndex,:]
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 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 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 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 removePlaneAndBeyond(polyData,
expectedNormal=[1, 0, 0],
filterRange=[-np.inf, -0.03],
whichAxis=1,
whichAxisLetter='y',
percentile=95):
yvalues = vnp.getNumpyFromVtk(polyData, 'Points')[:, whichAxis]
backY = np.percentile(yvalues, percentile)
if (percentile > 50):
searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter,
[backY - 0.1, np.inf])
else:
searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter,
[-np.inf, backY + 0.1])
vis.updatePolyData(searchRegion,
'search region',
parent="segmentation",
colorByName=whichAxisLetter,
visible=False)
# find the plane of the back wall, remove it and the points behind it:
_, origin, normal = segmentation.applyPlaneFit(
searchRegion,
distanceThreshold=0.02,
expectedNormal=expectedNormal,
perpendicularAxis=expectedNormal,
returnOrigin=True)
points = vnp.getNumpyFromVtk(polyData, 'Points')
dist = np.dot(points - origin, normal)
vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')
backFrame = transformUtils.getTransformFromOriginAndNormal(origin,
normal,
normalAxis=2)
vis.updateFrame(backFrame,
'back frame',
parent='segmentation',
scale=0.15,
visible=False)
vis.updatePolyData(polyData,
'dist to back',
parent='segmentation',
visible=False)
polyData = segmentation.thresholdPoints(polyData, 'dist_to_plane',
filterRange)
vis.updatePolyData(polyData,
'back off and all',
parent='segmentation',
visible=False)
return polyData
def planFootOut(self, startPose=None, finalFootHeight=0.05):
if startPose is None:
startPose = self.getPlanningStartPose()
startPoseName = 'q_egress_start'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
endPoseName = 'q_egress_end'
utorsoFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('utorso', startPose)
finalLeftFootFrame = self.computeLeftFootOverPlatformFrame(startPose, finalFootHeight)
constraints = []
constraints.extend(self.createUtorsoGazeConstraints([0.0, 1.0]))
constraints.append(ik.QuasiStaticConstraint(leftFootEnabled=False, rightFootEnabled=True,
pelvisEnabled=False, shrinkFactor=0.01))
constraints.append(self.robotSystem.ikPlanner.createMovingBaseSafeLimitsConstraint())
constraints.append(self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint(startPoseName))
#constraints.append(self.robotSystem.ikPlanner.createLockedBackPostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createKneePostureConstraint([0.7, 2.5]))
constraints.append(self.robotSystem.ikPlanner.createFixedLinkConstraints(startPoseName, 'r_foot'))
constraints.extend(self.createLeftFootPoseConstraint(finalLeftFootFrame, tspan=[1,1]))
constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints, endPoseName, startPoseName)
constraintSet.ikParameters = IkParameters(usePointwise=True, maxBaseRPYDegreesPerSecond=10,
rescaleBodyNames=['l_foot'],
rescaleBodyPts=[0.0, 0.0, 0.0],
maxBodyTranslationSpeed=self.maxFootTranslationSpeed)
#constraintSet.seedPoseName = 'q_start'
#constraintSet.nominalPoseName = 'q_start'
constraintSet.runIk()
footFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('l_foot', startPose)
t = transformUtils.frameFromPositionAndRPY([0, 0, self.polaris.leftFootEgressOutsideFrame.transform.GetPosition()[2]-footFrame.GetPosition()[2]], [0, 0, 0])
liftFrame = transformUtils.concatenateTransforms([footFrame, t])
vis.updateFrame(liftFrame, 'lift frame')
c = ik.WorldFixedOrientConstraint()
c.linkName = 'l_foot'
c.tspan = [0.0, 0.1, 0.2]
constraints.append(c)
constraints.extend(self.createLeftFootPoseConstraint(liftFrame, tspan=[0.2, 0.2]))
constraints.extend(self.createLeftFootPoseConstraint(self.polaris.leftFootEgressMidFrame, tspan=[0.5, 0.5]))
constraints.extend(self.createLeftFootPoseConstraint(self.polaris.leftFootEgressOutsideFrame, tspan=[0.8, 0.8]))
#plan = constraintSet.planEndPoseGoal(feetOnGround=False)
keyFramePlan = constraintSet.runIkTraj()
poseTimes, poses = planplayback.PlanPlayback.getPlanPoses(keyFramePlan)
ts = [poseTimes[0]]
supportsList = [['r_foot']]
plan = self.publishPlanWithSupports(keyFramePlan, supportsList, ts, False)
self.addPlan(plan)
return plan
def run(self):
poseName = self.properties.getProperty('Pose name')
if poseName == 'EST_ROBOT_STATE':
pose = robotSystem.robotStateJointController.q.copy()
else:
pose = robotSystem.ikPlanner.jointController.getPose(poseName)
robotModel = robotSystem.ikPlanner.getRobotModelAtPose(pose)
footFrame = robotSystem.footstepsDriver.getFeetMidPoint(robotModel)
vis.updateFrame(footFrame, self.properties.getProperty('Frame output name'), scale=0.2)
def __init__(self):
pose = transformUtils.poseFromTransform(vtk.vtkTransform())
self.pointcloud = ioUtils.readPolyData(ddapp.getDRCBaseDir() + '/software/models/rehearsal_pointcloud.vtp')
self.pointcloudPD = vis.showPolyData(self.pointcloud, 'coursemodel', parent=None)
segmentation.makeMovable(self.pointcloudPD, transformUtils.transformFromPose(array([0, 0, 0]), array([ 1.0, 0. , 0. , 0.0])))
self.originFrame = self.pointcloudPD.getChildFrame()
t = transformUtils.transformFromPose(array([-4.39364111, -0.51507392, -0.73125563]), array([ 0.93821625, 0. , 0. , -0.34604951]))
self.valveWalkFrame = vis.updateFrame(t, 'ValveWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-3.31840048, 0.36408685, -0.67413123]), array([ 0.93449475, 0. , 0. , -0.35597691]))
self.drillPreWalkFrame = vis.updateFrame(t, 'DrillPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.24553758, -0.52990939, -0.73255338]), array([ 0.93697004, 0. , 0. , -0.34940972]))
self.drillWalkFrame = vis.updateFrame(t, 'DrillWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.51306835, -0.92994004, -0.74173541 ]), array([-0.40456572, 0. , 0. , 0.91450893]))
self.drillWallWalkFarthestSafeFrame = vis.updateFrame(t, 'DrillWallWalkFarthestSafe', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.5314524 , -0.27401861, -0.71302976]), array([ 0.98691519, 0. , 0. , -0.16124022]))
self.drillWallWalkBackFrame = vis.updateFrame(t, 'DrillWallWalkBack', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-1.16122318, 0.04723203, -0.67493468]), array([ 0.93163145, 0. , 0. , -0.36340451]))
self.surprisePreWalkFrame = vis.updateFrame(t, 'SurprisePreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-0.5176186 , -1.00151554, -0.70650799]), array([ 0.84226497, 0. , 0. , -0.53906374]))
self.surpriseWalkFrame = vis.updateFrame(t, 'SurpriseWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-0.69100097, -0.43713269, -0.68495922]), array([ 0.98625075, 0. , 0. , -0.16525575]))
self.surpriseWalkBackFrame = vis.updateFrame(t, 'SurpriseWalkBack', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([ 0.65827322, -0.08028796, -0.77370834]), array([ 0.94399977, 0. , 0. , -0.3299461 ]))
self.terrainPreWalkFrame = vis.updateFrame(t, 'TerrainPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([ 5.47126425, -0.09790393, -0.70504679]), array([ 1., 0., 0., 0.]))
self.stairsPreWalkFrame = vis.updateFrame(t, 'StairsPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
self.frameSync = vis.FrameSync()
self.frameSync.addFrame(self.originFrame)
self.frameSync.addFrame(self.pointcloudPD.getChildFrame(), ignoreIncoming=True)
self.frameSync.addFrame(self.valveWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillPreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWallWalkFarthestSafeFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWallWalkBackFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surprisePreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surpriseWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surpriseWalkBackFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.terrainPreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.stairsPreWalkFrame, ignoreIncoming=True)
def __init__(self):
pose = transformUtils.poseFromTransform(vtk.vtkTransform())
self.pointcloud = ioUtils.readPolyData(ddapp.getDRCBaseDir() + '/software/models/rehearsal_pointcloud.vtp')
self.pointcloudPD = vis.showPolyData(self.pointcloud, 'coursemodel', parent=None)
segmentation.makeMovable(self.pointcloudPD, transformUtils.transformFromPose(array([0, 0, 0]), array([ 1.0, 0. , 0. , 0.0])))
self.originFrame = self.pointcloudPD.getChildFrame()
t = transformUtils.transformFromPose(array([-4.39364111, -0.51507392, -0.73125563]), array([ 0.93821625, 0. , 0. , -0.34604951]))
self.valveWalkFrame = vis.updateFrame(t, 'ValveWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-3.31840048, 0.36408685, -0.67413123]), array([ 0.93449475, 0. , 0. , -0.35597691]))
self.drillPreWalkFrame = vis.updateFrame(t, 'DrillPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.24553758, -0.52990939, -0.73255338]), array([ 0.93697004, 0. , 0. , -0.34940972]))
self.drillWalkFrame = vis.updateFrame(t, 'DrillWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.51306835, -0.92994004, -0.74173541 ]), array([-0.40456572, 0. , 0. , 0.91450893]))
self.drillWallWalkFarthestSafeFrame = vis.updateFrame(t, 'DrillWallWalkFarthestSafe', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.5314524 , -0.27401861, -0.71302976]), array([ 0.98691519, 0. , 0. , -0.16124022]))
self.drillWallWalkBackFrame = vis.updateFrame(t, 'DrillWallWalkBack', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-1.16122318, 0.04723203, -0.67493468]), array([ 0.93163145, 0. , 0. , -0.36340451]))
self.surprisePreWalkFrame = vis.updateFrame(t, 'SurprisePreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-0.5176186 , -1.00151554, -0.70650799]), array([ 0.84226497, 0. , 0. , -0.53906374]))
self.surpriseWalkFrame = vis.updateFrame(t, 'SurpriseWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-0.69100097, -0.43713269, -0.68495922]), array([ 0.98625075, 0. , 0. , -0.16525575]))
self.surpriseWalkBackFrame = vis.updateFrame(t, 'SurpriseWalkBack', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([ 0.65827322, -0.08028796, -0.77370834]), array([ 0.94399977, 0. , 0. , -0.3299461 ]))
self.terrainPreWalkFrame = vis.updateFrame(t, 'TerrainPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([ 5.47126425, -0.09790393, -0.70504679]), array([ 1., 0., 0., 0.]))
self.stairsPreWalkFrame = vis.updateFrame(t, 'StairsPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
self.frameSync = vis.FrameSync()
self.frameSync.addFrame(self.originFrame)
self.frameSync.addFrame(self.pointcloudPD.getChildFrame(), ignoreIncoming=True)
self.frameSync.addFrame(self.valveWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillPreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWallWalkFarthestSafeFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWallWalkBackFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surprisePreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surpriseWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surpriseWalkBackFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.terrainPreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.stairsPreWalkFrame, ignoreIncoming=True)
def moveToRobot(self, robotModel):
handLinkToWorld = robotModel.getLinkFrame(self.handLinkName)
t = vtk.vtkTransform()
t.PostMultiply()
t.Concatenate(self.modelToPalm)
t.Concatenate(self.palmToHandLink)
t.Concatenate(handLinkToWorld)
self.moveHandModelToFrame(self.handModel, t)
vis.updateFrame(self.getPalmToWorldTransform(), '%s palm' % self.side)
def computeBoardGraspFrames(self):
t = transformUtils.frameFromPositionAndRPY( self.graspLeftHandFrameXYZ , self.graspLeftHandFrameRPY )
t_copy = transformUtils.copyFrame(t)
t_copy.Concatenate(self.frame.transform)
self.graspLeftFrame = vis.updateFrame(t_copy, 'grasp left frame', parent=self.affordance, visible=False, scale=0.2)
self.graspLeftFrame.addToView(app.getDRCView())
t = transformUtils.frameFromPositionAndRPY( self.graspRightHandFrameXYZ , self.graspRightHandFrameRPY )
t_copy = transformUtils.copyFrame(t)
t_copy.Concatenate(self.frame.transform)
self.graspRightFrame = vis.updateFrame(t_copy, 'grasp right frame', parent=self.affordance, visible=False, scale=0.2)
self.graspRightFrame.addToView(app.getDRCView())
def run(self):
poseName = self.properties.getProperty('Pose name')
if poseName == 'EST_ROBOT_STATE':
pose = robotSystem.robotStateJointController.q.copy()
else:
pose = robotSystem.ikPlanner.jointController.getPose(poseName)
robotModel = robotSystem.ikPlanner.getRobotModelAtPose(pose)
footFrame = robotSystem.footstepsDriver.getFeetMidPoint(robotModel)
vis.updateFrame(footFrame,
self.properties.getProperty('Frame output name'),
scale=0.2)
def computeBoardStanceFrame(self):
objectTransform = transformUtils.copyFrame( self.graspLeftFrame.transform )
self.relativeStanceTransform = transformUtils.copyFrame( transformUtils.frameFromPositionAndRPY( self.relativeStanceXYZ , self.relativeStanceRPY ) )
robotStance = self.b.computeRobotStanceFrame( objectTransform, self.relativeStanceTransform )
self.stanceFrame = vis.updateFrame(robotStance, 'board stance', parent=self.affordance, visible=False, scale=0.2)
self.stanceFrame.addToView(app.getDRCView())
objectTransform = transformUtils.copyFrame( self.graspLeftFrame.transform )
self.relativeAsymmetricStanceTransform = transformUtils.copyFrame( transformUtils.frameFromPositionAndRPY( self.relativeAsymmetricStanceXYZ , self.relativeStanceRPY ) )
robotAsymmetricStance = self.b.computeRobotStanceFrame( objectTransform, self.relativeAsymmetricStanceTransform )
self.asymmetricStanceFrame = vis.updateFrame(robotAsymmetricStance, 'board Asymmetric stance', parent=self.affordance, visible=False, scale=0.2)
self.asymmetricStanceFrame.addToView(app.getDRCView())
def onShowMapButton(self):
vis.updateFrame(self.cameraToLocalInit, 'initial cam' )
filename = os.path.expanduser('~/Kinect_Logs/Kintinuous.pcd')
print filename
pd = io.readPolyData(filename)
pd = filterUtils.transformPolyData(pd, self.cameraToLocalInit )
#t = transformUtils.frameFromPositionAndRPY([0,0,0],[-90,0,-90])
#pd = filterUtils.transformPolyData(pd , t)
pdi = vis.updatePolyData(pd,'map')
pdi.setProperty('Color By', 'rgb_colors')
def moveToRobot(self, robotModel):
handLinkToWorld = robotModel.getLinkFrame(self.handLinkName)
t = vtk.vtkTransform()
t.PostMultiply()
t.Concatenate(self.modelToPalm)
t.Concatenate(self.palmToHandLink)
t.Concatenate(handLinkToWorld)
self.moveHandModelToFrame(self.handModel, t)
vis.updateFrame(self.getPalmToWorldTransform(), '%s palm' % self.side)
def onStartMappingButton(self):
msg = map_command_t()
msg.timestamp = getUtime()
msg.command = 0
lcmUtils.publish('KINECT_MAP_COMMAND', msg)
utime = self.queue.getCurrentImageTime('KINECT_RGB')
self.cameraToLocalInit = vtk.vtkTransform()
self.queue.getTransform('KINECT_RGB', 'local', utime, self.cameraToLocalInit)
vis.updateFrame(self.cameraToLocalInit, 'initial cam' )
print "starting mapping", utime
print self.cameraToLocalInit.GetPosition()
print self.cameraToLocalInit.GetOrientation()
def drawFittedSteps(self, footsteps):
''' Draw the footsteps fitted to the blocks
These are NOT the steps placed by the planner
'''
left_color=None
right_color=None
for i, footstep in enumerate(footsteps):
mesh,color = self.getMeshAndColor(footstep.is_right_foot)
vis.updateFrame(footstep.transform, 'foot placement %d' % i , parent='foot placements', scale=0.2, visible=False)
obj = vis.showPolyData(mesh, 'step %d' % i, color=color, alpha=1.0, parent='steps')
#frameObj = vis.showFrame(footstepTransform, stepName + ' frame', parent=obj, scale=0.3, visible=False)
obj.actor.SetUserTransform(footstep.transform)
def drawFittedSteps(self, footsteps):
''' Draw the footsteps fitted to the blocks
These are NOT the steps placed by the planner
'''
left_color=None
right_color=None
for i, footstep in enumerate(footsteps):
mesh,color = self.getMeshAndColor(footstep.is_right_foot)
vis.updateFrame(footstep.transform, 'foot placement %d' % i , parent='foot placements', scale=0.2, visible=False)
obj = vis.showPolyData(mesh, 'step %d' % i, color=color, alpha=1.0, parent='steps')
#frameObj = vis.showFrame(footstepTransform, stepName + ' frame', parent=obj, scale=0.3, visible=False)
obj.actor.SetUserTransform(footstep.transform)
def run(self):
aff = self.getSelectedAffordance()
affFrame = aff.getChildFrame().transform
groundFrame = self.getGroundFrame().transform
projectedXYZ = np.hstack([affFrame.GetPosition()[0:2], groundFrame.GetPosition()[2]])
result = transformUtils.copyFrame(affFrame)
result.Translate(projectedXYZ - np.array(result.GetPosition()))
outputName = self.properties.getProperty('Frame output name')
outputName = outputName or '%s ground frame' % aff.getProperty('Name')
vis.updateFrame(result, outputName, scale=0.2)
def doFTDraw(self, unusedrobotstate):
frames = []
fts = []
vis_names = []
if (hasattr(self.robotStateJointController, 'lastRobotStateMessage') and
self.robotStateJointController.lastRobotStateMessage):
if self.draw_right:
rft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_force +
self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_torque)
rft[0] = -1.*rft[0] # right FT looks to be rotated 180deg around the z axis
rft[1] = -1.*rft[1]
rft[3] = -1.*rft[3]
rft[4] = -1.*rft[4]
rft -= self.ft_right_bias
fts.append(rft)
frames.append(self.robotStateModel.getLinkFrame('r_hand_force_torque'))
vis_names.append('ft_right')
if self.draw_left:
lft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_force +
self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_torque)
lft -= self.ft_left_bias
fts.append(lft)
frames.append(self.robotStateModel.getLinkFrame('l_hand_force_torque'))
vis_names.append('ft_left')
for i in range(0, len(frames)):
frame = frames[i]
ft = fts[i]
offset = [0., 0., 0.]
p1 = frame.TransformPoint(offset)
scale = 1.0/25.0 # todo add slider for this?
scalet = 1.0 / 2.5
p2f = frame.TransformPoint(offset + ft[0:3]*scale)
p2t = frame.TransformPoint(offset + ft[3:6])
normalt = (np.array(p2t) - np.array(p1))
normalt = normalt / np.linalg.norm(normalt)
d = DebugData()
# force
if np.linalg.norm(np.array(p2f) - np.array(p1)) < 0.1:
d.addLine(p1, p2f, color=[1.0, 0.0, 0.0])
else:
d.addArrow(p1, p2f, color=[1.,0.,0.])
# torque
if self.draw_torque:
d.addCircle(p1, normalt, scalet*np.linalg.norm(ft[3:6]))
# frame (largely for debug)
vis.updateFrame(frame, vis_names[i]+'frame', view=self.view, parent='wristft', visible=False, scale=0.2)
vis.updatePolyData(d.getPolyData(), vis_names[i], view=self.view, parent='wristft')
def selectLink(self, displayPoint, view):
if not self.enabled():
return False
robotModel, _ = vis.findPickedObject(displayPoint, view)
try:
robotModel.model.getLinkNameForMesh
except AttributeError:
return False
model = robotModel.model
pickedPoint, _, polyData = vis.pickProp(displayPoint, view)
linkName = model.getLinkNameForMesh(polyData)
if not linkName:
return False
fadeValue = 1.0 if linkName == self.selectedLink else 0.05
for name in model.getLinkNames():
linkColor = model.getLinkColor(name)
linkColor.setAlphaF(fadeValue)
model.setLinkColor(name, linkColor)
if linkName == self.selectedLink:
self.selectedLink = None
vis.hideCaptionWidget()
om.removeFromObjectModel(
om.findObjectByName('selected link frame'))
else:
self.selectedLink = linkName
linkColor = model.getLinkColor(self.selectedLink)
linkColor.setAlphaF(1.0)
model.setLinkColor(self.selectedLink, linkColor)
vis.showCaptionWidget(robotModel.getLinkFrame(
self.selectedLink).GetPosition(),
self.selectedLink,
view=view)
vis.updateFrame(robotModel.getLinkFrame(self.selectedLink),
'selected link frame',
scale=0.2,
parent=robotModel)
return True
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 spawnFootstepFrame(self):
# should snap this to
boxFrame = om.findObjectByName('Switch Box').getChildFrame().transform
goalFrame = transformUtils.copyFrame(self.footToBox)
goalFrame.PostMultiply()
goalFrame.Concatenate(boxFrame)
# translate goal frame to match current robot height
stanceFrame = self.getStanceFrame()
stanceHeight = stanceFrame.GetPosition()[2]
goalHeight = goalFrame.GetPosition()[2]
goalFrame.PreMultiply()
goalFrame.Translate(0.0, 0.0, stanceHeight - goalHeight)
vis.updateFrame(goalFrame, 'switch box stance frame', scale=0.2)
def computeStanceFrame(self, useIkTraj=False):
objectTransform = transformUtils.copyFrame(
self.computeGraspFrame().transform)
if useIkTraj:
startPose = self.getNominalPose()
plan = self.planInsertTraj(self.speedLow,
lockFeet=False,
lockBase=False,
resetPoses=True,
startPose=startPose)
stancePose = robotstate.convertStateMessageToDrakePose(
plan.plan[0])
stanceRobotModel = self.ikPlanner.getRobotModelAtPose(stancePose)
self.nominalPelvisXYZ = stancePose[:3]
robotStance = self.footstepPlanner.getFeetMidPoint(
stanceRobotModel)
else:
robotStance = self.computeRobotStanceFrame(
objectTransform, self.computeRelativeStanceTransform())
stanceFrame = vis.updateFrame(robotStance,
'valve grasp stance',
parent=self.getValveAffordance(),
visible=False,
scale=0.2)
stanceFrame.addToView(app.getDRCView())
return stanceFrame
def computeGraspFrame(self):
t = self.computeRelativeGraspTransform()
t.Concatenate(self.getValveAffordance().getChildFrame().transform)
graspFrame = vis.updateFrame(t, 'valve grasp frame',
parent=self.getValveAffordance(),
visible=False, scale=0.2)
graspFrame.addToView(app.getDRCView())
return graspFrame
def run(self):
aff = self.getSelectedAffordance()
affFrame = aff.getChildFrame().transform
groundFrame = self.getGroundFrame().transform
projectedXYZ = np.hstack(
[affFrame.GetPosition()[0:2],
groundFrame.GetPosition()[2]])
result = transformUtils.copyFrame(affFrame)
result.Translate(projectedXYZ - np.array(result.GetPosition()))
outputName = self.properties.getProperty('Frame output name')
outputName = outputName or '%s ground frame' % aff.getProperty('Name')
vis.updateFrame(result, outputName, scale=0.2)
def TargetReceived(self, data):
self.log( 'Target received (%.3f,%.3f,%.3f), (%.3f,%.3f,%.3f,%.3f)' % \
(data.trans[0], data.trans[1], data.trans[2], \
data.quat[0], data.quat[1], data.quat[2], data.quat[3]) )
if math.isnan(data.trans[0]):
self.log('Getting NaN target, stop')
return
self.TargetMsg = deepcopy(data)
targetToWorld = transformUtils.frameFromPositionAndRPY(self.TargetMsg.trans,
np.degrees(botpy.quat_to_roll_pitch_yaw(self.TargetMsg.quat)))
startPose = self.getPlanningStartPose()
handToWorld= self.ikPlanner.getLinkFrameAtPose( 'l_hand_face', startPose)
goalFrame = vis.updateFrame(handToWorld, 'OriginalFrame', parent='Pfgrasp', visible=True, scale=0.25)
goalFrame2 = vis.updateFrame(targetToWorld, 'PeterFrame', parent='Pfgrasp', visible=True, scale=0.25)
handToWorld_XYZ = handToWorld.GetPosition()
targetToWorld_XYZ = targetToWorld.GetPosition()
dist = sqrt( (handToWorld_XYZ[0]-targetToWorld_XYZ[0])**2 + (handToWorld_XYZ[1]-targetToWorld_XYZ[1])**2 + (handToWorld_XYZ[2]-targetToWorld_XYZ[2])**2 )
self.log( "dist %.3f" % dist )
threshold = float(self.ui.criterionEdit.text)
if(dist < threshold):
#easygui.msgbox("The correction movement is less than 0.015, you can go grasp it", title="Done")
self.log("The correction movement is %.3f less than %.3f, you can go grasp it" % (dist, threshold))
if self.autoMode: self.guardedMoveForwardAndGraspHoldRetreat()
else:
#print "startPose", startPose
#print "targetToWorld", targetToWorld
#print "graspingHand", self.graspingHand
constraintSet = self.ikPlanner.planEndEffectorGoal(startPose, self.graspingHand, targetToWorld, lockBase=False, lockBack=True)
endPose, info = constraintSet.runIk()
if info > 10:
self.log("in Target received: Bad movement")
return
graspPlan = constraintSet.runIkTraj()
if self.autoMode:
self.manipPlanner.commitManipPlan(graspPlan)
self.waitForPlanExecution(graspPlan)
self.runoneiter()
def spawnFootstepFrame(self):
# should snap this to
boxFrame = om.findObjectByName('Switch Box').getChildFrame().transform
goalFrame = transformUtils.copyFrame(self.footToBox)
goalFrame.PostMultiply()
goalFrame.Concatenate(boxFrame)
# translate goal frame to match current robot height
stanceFrame = self.getStanceFrame()
stanceHeight = stanceFrame.GetPosition()[2]
goalHeight = goalFrame.GetPosition()[2]
goalFrame.PreMultiply()
goalFrame.Translate(0.0, 0.0, stanceHeight - goalHeight)
vis.updateFrame(goalFrame, 'switch box stance frame', scale=0.2)
def onNewWalkingGoal(self, walkingGoal=None):
walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel)
frameObj = vis.updateFrame(walkingGoal,
'walking goal',
parent='planning',
scale=0.25)
frameObj.setProperty('Edit', True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
if self.placer:
self.placer.stop()
terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
if terrain:
pos = np.array(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])
frameObj.transform.Translate(
pos - np.array(frameObj.transform.GetPosition()))
d = DebugData()
d.addSphere((0, 0, 0), radius=0.03)
handle = vis.showPolyData(d.getPolyData(),
'walking goal terrain handle',
parent=frameObj,
visible=True,
color=[1, 1, 0])
handle.actor.SetUserTransform(frameObj.transform)
self.placer = PlacerWidget(app.getCurrentRenderView(), handle,
terrain)
def onFramePropertyModified(propertySet, propertyName):
if propertyName == 'Edit':
if propertySet.getProperty(propertyName):
self.placer.start()
else:
self.placer.stop()
frameObj.properties.connectPropertyChanged(onFramePropertyModified)
onFramePropertyModified(frameObj, 'Edit')
frameObj.connectFrameModified(self.onWalkingGoalModified)
self.onWalkingGoalModified(frameObj)
def onAtlasStepParams(self,msg):
if (msg.desired_step_spec.foot_index ==1):
is_right_foot = True
else:
is_right_foot = False
#print msg.desired_step_spec.foot_index , " and " , is_right_foot
foot = msg.desired_step_spec.foot
footTransform = transformUtils.frameFromPositionAndRPY( foot.position , [0, 0, foot.yaw*180/math.pi])
mesh,color = self.getMeshAndColor(is_right_foot)
#color[1] = 0.75 ; color[2] = 0.25
vis.updateFrame(footTransform, 'bdi foot', parent='foot placements', scale=0.2, visible=False)
#bdi_step_mesh = om.findObjectByName('bdi step')
#om.removeFromObjectModel(bdi_step_mesh)
obj = vis.updatePolyData(mesh, 'bdi step', color=color, alpha=1.0, parent='foot placements', visible=False)
obj.setProperty('Color',QtGui.QColor(color[0]*255.0,color[1]*255.0,color[2]*255.0))
obj.actor.SetUserTransform(footTransform)
def updateLinkFrame(self, robotModel, linkName, create=True):
linkFrameName = '%s frame' % linkName
if not create and not om.findObjectByName(linkFrameName):
return
t = robotModel.getLinkFrame(linkName)
return vis.updateFrame(t, linkFrameName, scale=0.2, visible=False, parent=self.robotModel)
def onAtlasStepParams(self,msg):
if (msg.desired_step_spec.foot_index ==1):
is_right_foot = True
else:
is_right_foot = False
#print msg.desired_step_spec.foot_index , " and " , is_right_foot
foot = msg.desired_step_spec.foot
footTransform = transformUtils.frameFromPositionAndRPY( foot.position , [0, 0, foot.yaw*180/math.pi])
mesh,color = self.getMeshAndColor(is_right_foot)
#color[1] = 0.75 ; color[2] = 0.25
vis.updateFrame(footTransform, 'bdi foot', parent='foot placements', scale=0.2, visible=False)
#bdi_step_mesh = om.findObjectByName('bdi step')
#om.removeFromObjectModel(bdi_step_mesh)
obj = vis.updatePolyData(mesh, 'bdi step', color=color, alpha=1.0, parent='foot placements', visible=False)
obj.setProperty('Color',QtGui.QColor(color[0]*255.0,color[1]*255.0,color[2]*255.0))
obj.actor.SetUserTransform(footTransform)
def computeBoardReachFrames(self):
''' Reach ~10cm short of the grasp frame '''
reachLeftXYZ = copy.deepcopy( self.graspLeftHandFrameXYZ )
reachLeftXYZ[0] = reachLeftXYZ[0] - self.reachDepth
reachLeftRPY = copy.deepcopy ( self.graspLeftHandFrameRPY )
tl = transformUtils.frameFromPositionAndRPY( reachLeftXYZ , reachLeftRPY )
tl.Concatenate(self.frame.transform)
self.reachLeftFrame = vis.updateFrame(tl, 'reach left frame', parent=self.affordance, visible=False, scale=0.2)
self.reachLeftFrame.addToView(app.getDRCView())
reachRightXYZ = copy.deepcopy( self.graspRightHandFrameXYZ )
reachRightXYZ[0] = reachRightXYZ[0] - self.reachDepth
reachRightRPY = copy.deepcopy ( self.graspRightHandFrameRPY )
tr = transformUtils.frameFromPositionAndRPY( reachRightXYZ , reachRightRPY )
tr.Concatenate(self.frame.transform)
self.reachRightFrame = vis.updateFrame(tr, 'reach right frame', parent=self.affordance, visible=False, scale=0.2)
self.reachRightFrame.addToView(app.getDRCView())
def computeGraspFrame(self):
t = self.computeRelativeGraspTransform()
t.Concatenate(self.getValveAffordance().getChildFrame().transform)
graspFrame = vis.updateFrame(t,
'valve grasp frame',
parent=self.getValveAffordance(),
visible=False,
scale=0.2)
graspFrame.addToView(app.getDRCView())
return graspFrame
def planGraspLift(self):
t3 = transformUtils.frameFromPositionAndRPY( [0.00,0.0, 0.04] , [0,0,0] )
liftTransform = transformUtils.copyFrame(self.board.graspFrame.transform)
liftTransform.Concatenate(t3)
self.board.liftFrame = vis.updateFrame(liftTransform, 'lift frame', parent="affordances", visible=False, scale=0.2)
startPose = self.getPlanningStartPose()
constraintSet = self.ikPlanner.planEndEffectorGoal(startPose, self.graspingHand, self.board.liftFrame, lockBase=self.lockBase, lockBack=self.lockBack)
endPose, info = constraintSet.runIk()
liftPlan = constraintSet.runIkTraj()
self.addPlan(liftPlan)
def selectLink(self, displayPoint, view):
if not self.enabled():
return False
robotModel, _ = vis.findPickedObject(displayPoint, view)
try:
robotModel.model.getLinkNameForMesh
except AttributeError:
return False
model = robotModel.model
pickedPoint, _, polyData = vis.pickProp(displayPoint, view)
linkName = model.getLinkNameForMesh(polyData)
if not linkName:
return False
fadeValue = 1.0 if linkName == self.selectedLink else 0.05
for name in model.getLinkNames():
linkColor = model.getLinkColor(name)
linkColor.setAlphaF(fadeValue)
model.setLinkColor(name, linkColor)
if linkName == self.selectedLink:
self.selectedLink = None
vis.hideCaptionWidget()
om.removeFromObjectModel(om.findObjectByName('selected link frame'))
else:
self.selectedLink = linkName
linkColor = model.getLinkColor(self.selectedLink)
linkColor.setAlphaF(1.0)
model.setLinkColor(self.selectedLink, linkColor)
vis.showCaptionWidget(robotModel.getLinkFrame(self.selectedLink).GetPosition(), self.selectedLink, view=view)
vis.updateFrame(robotModel.getLinkFrame(self.selectedLink), 'selected link frame', scale=0.2, parent=robotModel)
return True
def updateLinkFrame(self, robotModel, linkName, create=True):
linkFrameName = '%s frame' % linkName
if not create and not om.findObjectByName(linkFrameName):
return
t = robotModel.getLinkFrame(linkName)
return vis.updateFrame(t,
linkFrameName,
scale=0.2,
visible=False,
parent=self.robotModel)
def computeBoardGraspFrames(self):
t = transformUtils.frameFromPositionAndRPY(self.graspLeftHandFrameXYZ,
self.graspLeftHandFrameRPY)
t_copy = transformUtils.copyFrame(t)
t_copy.Concatenate(self.frame.transform)
self.graspLeftFrame = vis.updateFrame(t_copy,
'grasp left frame',
parent=self.affordance,
visible=False,
scale=0.2)
self.graspLeftFrame.addToView(app.getDRCView())
t = transformUtils.frameFromPositionAndRPY(self.graspRightHandFrameXYZ,
self.graspRightHandFrameRPY)
t_copy = transformUtils.copyFrame(t)
t_copy.Concatenate(self.frame.transform)
self.graspRightFrame = vis.updateFrame(t_copy,
'grasp right frame',
parent=self.affordance,
visible=False,
scale=0.2)
self.graspRightFrame.addToView(app.getDRCView())
def run(self):
inputFrame = self.getInputFrame()
translation = self.properties.getProperty('Translation')
rpy = self.properties.getProperty('Rotation')
offset = transformUtils.frameFromPositionAndRPY(translation, rpy)
offset.PostMultiply()
offset.Concatenate(transformUtils.copyFrame(inputFrame.transform))
outputFrame = vis.updateFrame(offset, self.properties.getProperty('Frame output name'), scale=0.2, parent=inputFrame.parent())
if not hasattr(inputFrame, 'frameSync'):
inputFrame.frameSync = vis.FrameSync()
inputFrame.frameSync.addFrame(inputFrame)
inputFrame.frameSync.addFrame(outputFrame, ignoreIncoming=True)
def onNewWalkingGoal(self, walkingGoal=None):
walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel)
frameObj = vis.updateFrame(walkingGoal, 'walking goal', parent='planning', scale=0.25)
frameObj.setProperty('Edit', True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
if self.placer:
self.placer.stop()
terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
if terrain:
pos = np.array(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])
frameObj.transform.Translate(pos - np.array(frameObj.transform.GetPosition()))
d = DebugData()
d.addSphere((0,0,0), radius=0.03)
handle = vis.showPolyData(d.getPolyData(), 'walking goal terrain handle', parent=frameObj, visible=True, color=[1,1,0])
handle.actor.SetUserTransform(frameObj.transform)
self.placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain)
def onFramePropertyModified(propertySet, propertyName):
if propertyName == 'Edit':
if propertySet.getProperty(propertyName):
self.placer.start()
else:
self.placer.stop()
frameObj.properties.connectPropertyChanged(onFramePropertyModified)
onFramePropertyModified(frameObj, 'Edit')
frameObj.connectFrameModified(self.onWalkingGoalModified)
self.onWalkingGoalModified(frameObj)
def makeStepFrames(stepFrames, relativeFrame=None, showFrames=False):
frames = []
for i, stepFrame in enumerate(stepFrames):
stepFrame = transformUtils.frameFromPositionAndRPY(stepFrame, [0,0,0])
stepFrame.PostMultiply()
if relativeFrame:
stepFrame.Concatenate(relativeFrame)
if showFrames:
obj = vis.updateFrame(stepFrame, 'step frame %d' % i, parent='step frames', scale=0.2)
stepFrame = obj.transform
frames.append(stepFrame)
return frames
