def planLeftFootDown(self):
startPose = self.getPlanningStartPose()
startPoseName = 'q_footdown_start'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
endPoseName = 'q_footdown_end'
utorsoFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('utorso', startPose)
finalLeftFootFrame = self.computeLeftFootOverPlatformFrame(startPose, 0.0)
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.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)
#constraintSet.seedPoseName = 'q_start'
#constraintSet.nominalPoseName = 'q_start'
endPose, _ = constraintSet.runIk()
keyFramePlan = constraintSet.planEndPoseGoal(feetOnGround=False)
poseTimes, poses = planplayback.PlanPlayback.getPlanPoses(keyFramePlan)
ts = [poseTimes[0], poseTimes[-1]]
supportsList = [['r_foot'], ['r_foot','l_foot']]
plan = self.publishPlanWithSupports(keyFramePlan, supportsList, ts, True)
self.addPlan(plan)
return plan, endPose
def planShiftWeightOut(self, startPose=None):
if startPose is None:
startPose = self.getPlanningStartPose()
startPoseName = 'q_egress_start'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
endPoseName = 'q_egress_end'
constraints = []
utorsoFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('utorso', startPose)
constraints.extend(self.createUtorsoGazeConstraints([1.0, 1.0]))
constraints.append(ik.QuasiStaticConstraint(leftFootEnabled=False, rightFootEnabled=True,
pelvisEnabled=False,
shrinkFactor=self.quasiStaticShrinkFactor))
constraints.append(self.robotSystem.ikPlanner.createXYZMovingBasePostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createKneePostureConstraint([0.7, 2.5]))
constraints.append(self.robotSystem.ikPlanner.createFixedLinkConstraints(startPoseName, 'l_foot'))
constraints.append(self.robotSystem.ikPlanner.createFixedLinkConstraints(startPoseName, 'r_foot'))
constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints, endPoseName, startPoseName)
constraintSet.ikParameters = IkParameters(usePointwise=True, maxBaseMetersPerSecond=0.02)
constraintSet.runIk()
keyFramePlan = constraintSet.planEndPoseGoal(feetOnGround=False)
poseTimes, poses = planplayback.PlanPlayback.getPlanPoses(keyFramePlan)
ts = [poseTimes[0]]
supportsList = [['r_foot', 'l_foot']]
plan = self.publishPlanWithSupports(keyFramePlan, supportsList, ts, True)
self.addPlan(plan)
return plan
def planStandUp(self):
startPose = self.getPlanningStartPose()
startPoseName = 'q_egress_start'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
endPoseName = 'q_egress_end'
pelvisFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('pelvis', startPose)
t = transformUtils.frameFromPositionAndRPY([self.pelvisLiftX, 0, self.pelvisLiftZ], [0, 0, 0])
liftFrame = transformUtils.concatenateTransforms([t, pelvisFrame])
constraints = []
utorsoFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('utorso', startPose)
g = self.createUtorsoGazeConstraints([1.0, 1.0])
constraints.append(g[1])
p = ik.PositionConstraint(linkName='pelvis', referenceFrame=liftFrame,
lowerBound=np.array([0.0, -np.inf, 0.0]),
upperBound=np.array([np.inf, np.inf, 0.0]))
constraints.append(p)
constraints.append(ik.QuasiStaticConstraint(leftFootEnabled=True, rightFootEnabled=True, pelvisEnabled=False,
shrinkFactor=self.quasiStaticShrinkFactor))
constraints.append(self.robotSystem.ikPlanner.createXYZMovingBasePostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint(startPoseName))
constraints.extend(self.robotSystem.ikPlanner.createFixedFootConstraints(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createKneePostureConstraint([0.7, 2.5]))
constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints, endPoseName, startPoseName)
constraintSet.ikParameters = IkParameters(usePointwise=True, maxBaseMetersPerSecond=0.02)
constraintSet.runIk()
keyFramePlan = constraintSet.planEndPoseGoal(feetOnGround=True)
poseTimes, poses = planplayback.PlanPlayback.getPlanPoses(keyFramePlan)
ts = [poseTimes[0]]
supportsList = [['r_foot', 'l_foot']]
plan = self.publishPlanWithSupports(keyFramePlan, supportsList, ts, True)
self.addPlan(plan)
return plan
def planGetWeightOverFeet(self):
startPose = self.getPlanningStartPose()
startPoseName = 'q_egress_start'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
endPoseName = 'q_egress_end'
constraints = []
constraints.append(
ikconstraints.QuasiStaticConstraint(leftFootEnabled=True,
rightFootEnabled=True,
pelvisEnabled=False,
shrinkFactor=0.5))
constraints.append(
self.robotSystem.ikPlanner.createLockedBasePostureConstraint(
startPoseName))
constraints.append(
self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint(
startPoseName))
constraints.append(
self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint(
startPoseName))
constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints,
endPoseName, startPoseName)
constraintSet.ikParameters = IkParameters(usePointwise=False,
maxDegreesPerSecond=10)
constraintSet.runIk()
keyFramePlan = constraintSet.planEndPoseGoal(feetOnGround=False)
poseTimes, poses = planplayback.PlanPlayback.getPlanPoses(keyFramePlan)
ts = [poseTimes[0]]
supportsList = [['r_foot', 'l_foot', 'pelvis']]
plan = self.publishPlanWithSupports(keyFramePlan, supportsList, ts,
True)
self.addPlan(plan)
return plan
def planPinchReach(self, maxDegreesPerSecond=None):
if maxDegreesPerSecond is None:
maxDegreesPerSecond = 10
ikPlanner = self.ikPlanner
targetFrame = om.findObjectByName('pinch reach frame').transform
pinchToHand = self.getPinchToHandFrame()
startPose = self.getPlanningStartPose()
constraintSet = self.computeGraspPose(startPose,
targetFrame,
graspToHand=pinchToHand)
constraintSet.ikParameters = IkParameters(
maxDegreesPerSecond=maxDegreesPerSecond)
seedPose = ikPlanner.getMergedPostureFromDatabase(startPose,
'surprise:switch',
'above_switch',
side='right')
seedPoseName = 'q_above_switch'
self.robotSystem.ikPlanner.addPose(seedPose, seedPoseName)
constraintSet.seedPoseName = seedPoseName
constraintSet.nominalPoseName = seedPoseName
endPose, info = constraintSet.runIk()
plan = constraintSet.planEndPoseGoal()
self.addPlan(plan)
def runIK(self,
targetFrame,
startPose=None,
graspToHandLinkFrame=None,
positionTolerance=0.0,
angleToleranceInDegrees=0.0,
seedPoseName='q_nom'):
if graspToHandLinkFrame is None:
graspToHandLinkFrame = vtk.vtkTransform()
if startPose is None:
startPose = self.getPlanningStartPose()
ikPlanner = self.robotSystem.ikPlanner
startPoseName = 'reach_start'
endPoseName = 'reach_end'
ikPlanner.addPose(startPose, startPoseName)
side = ikPlanner.reachingSide
constraints = []
constraints.append(
KukaPlanningUtils.createLockedBasePostureConstraint(
ikPlanner, startPoseName))
# positionConstraint, orientationConstraint = ikPlanner.createPositionOrientationGraspConstraints(side, targetFrame,
# graspToHandLinkFrame,
# positionTolerance=positionTolerance,
# angleToleranceInDegrees=angleToleranceInDegrees)
positionConstraint, orientationConstraint = ikPlanner.createPositionOrientationConstraint(
self.endEffectorLinkName,
targetFrame,
graspToHandLinkFrame,
positionTolerance=positionTolerance,
angleToleranceInDegrees=angleToleranceInDegrees)
positionConstraint.tspan = [1.0, 1.0]
orientationConstraint.tspan = [1.0, 1.0]
constraints.append(positionConstraint)
constraints.append(orientationConstraint)
constraintSet = ConstraintSet(ikPlanner, constraints, 'reach_end',
startPoseName)
constraintSet.ikParameters = IkParameters()
constraintSet.seedPoseName = seedPoseName
print "consraintSet ", constraintSet
print "constraintSet.seedPoseName ", constraintSet.seedPoseName
print "constraintSet.nominalPoseName ", constraintSet.nominalPoseName
endPose, info = constraintSet.runIk()
returnData = dict()
returnData['info'] = info
returnData['endPose'] = endPose
return returnData
def planFootEgress(self):
def saveOriginalTraj(name):
commands = ['%s = qtraj_orig;' % name]
self.robotSystem.ikServer.comm.sendCommands(commands)
def concatenateAndRescaleTrajectories(trajectoryNames, concatenatedTrajectoryName, junctionTimesName, ikParameters):
commands = []
commands.append('joint_v_max = repmat(%s*pi/180, r.getNumVelocities()-6, 1);' % ikParameters.maxDegreesPerSecond)
commands.append('xyz_v_max = repmat(%s, 3, 1);' % ikParameters.maxBaseMetersPerSecond)
commands.append('rpy_v_max = repmat(%s*pi/180, 3, 1);' % ikParameters.maxBaseRPYDegreesPerSecond)
commands.append('v_max = [xyz_v_max; rpy_v_max; joint_v_max];')
commands.append("max_body_translation_speed = %r;" % ikParameters.maxBodyTranslationSpeed)
commands.append("max_body_rotation_speed = %r;" % ikParameters.maxBodyRotationSpeed)
commands.append('rescale_body_ids = [%s];' % (','.join(['links.%s' % linkName for linkName in ikParameters.rescaleBodyNames])))
commands.append('rescale_body_pts = reshape(%s, 3, []);' % ik.ConstraintBase.toColumnVectorString(ikParameters.rescaleBodyPts))
commands.append("body_rescale_options = struct('body_id',rescale_body_ids,'pts',rescale_body_pts,'max_v',max_body_translation_speed,'max_theta',max_body_rotation_speed,'robot',r);")
commands.append('trajectories = {};')
for name in trajectoryNames:
commands.append('trajectories{end+1} = %s;' % name)
commands.append('[%s, %s] = concatAndRescaleTrajectories(trajectories, v_max, %s, %s, body_rescale_options);' % (concatenatedTrajectoryName, junctionTimesName, ikParameters.accelerationParam, ikParameters.accelerationFraction))
commands.append('s.publishTraj(%s, 1);' % concatenatedTrajectoryName)
self.robotSystem.ikServer.comm.sendCommands(commands)
return self.robotSystem.ikServer.comm.getFloatArray(junctionTimesName)
self.planShiftWeightOut()
shiftWeightName = 'qtraj_shift_weight'
saveOriginalTraj(shiftWeightName)
nextStartPose = robotstate.convertStateMessageToDrakePose(self.plans[-1].plan.plan[-1])
self.planFootOut(startPose=nextStartPose, finalFootHeight=0.0)
footOutName = 'qtraj_foot_out'
saveOriginalTraj(footOutName)
nextStartPose = robotstate.convertStateMessageToDrakePose(self.plans[-1].plan.plan[-1])
self.planCenterWeight(startPose=nextStartPose)
centerWeightName = 'qtraj_center_weight'
saveOriginalTraj(centerWeightName)
ikParameters = IkParameters(usePointwise=True, maxBaseRPYDegreesPerSecond=10,
rescaleBodyNames=['l_foot'], rescaleBodyPts=[0.0, 0.0, 0.0],
maxBodyTranslationSpeed=self.maxFootTranslationSpeed)
ikParameters = self.robotSystem.ikPlanner.mergeWithDefaultIkParameters(ikParameters)
listener = self.robotSystem.ikPlanner.getManipPlanListener()
supportTimes = concatenateAndRescaleTrajectories([shiftWeightName, footOutName, centerWeightName],
'qtraj_foot_egress', 'ts', ikParameters)
keyFramePlan = listener.waitForResponse()
listener.finish()
supportsList = []
supportsList.append(['l_foot', 'r_foot'])
supportsList.append(['r_foot'])
supportsList.append(['l_foot', 'r_foot'])
supportsList.append(['l_foot', 'r_foot'])
plan = self.publishPlanWithSupports(keyFramePlan, supportsList, supportTimes, True)
self.addPlan(plan)
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 runIK(self,
targetFrame,
startPose=None,
graspToHandLinkFrame=None,
makePlan=True,
positionTolerance=0.0,
angleToleranceInDegrees=5.0,
maxDegreesPerSecond=60):
"""
Sets the cameraFrame to the targetFrame using IK
:param targetFrame:
:return:
"""
if startPose is None:
startPose = self.getPlanningStartPose()
ikPlanner = self.robotSystem.ikPlanner
startPoseName = 'reach_start'
endPoseName = 'reach_end'
ikPlanner.addPose(startPose, startPoseName)
side = ikPlanner.reachingSide
constraints = []
constraints.append(
KukaPlanningUtils.createLockedBasePostureConstraint(
ikPlanner, startPoseName))
positionConstraint, orientationConstraint = ikPlanner.createPositionOrientationGraspConstraints(
side,
targetFrame,
graspToHandLinkFrame,
positionTolerance=positionTolerance,
angleToleranceInDegrees=angleToleranceInDegrees)
positionConstraint.tspan = [1.0, 1.0]
orientationConstraint.tspan = [1.0, 1.0]
constraints.append(positionConstraint)
constraints.append(orientationConstraint)
constraintSet = ConstraintSet(ikPlanner, constraints, 'reach_end',
startPoseName)
constraintSet.ikParameters = IkParameters(
maxDegreesPerSecond=maxDegreesPerSecond)
endPose, info = constraintSet.runIk()
returnData = dict()
returnData['info'] = info
returnData['endPose'] = endPose
if makePlan:
plan = constraintSet.planEndPoseGoal()
returnData['plan'] = plan
return returnData
def computeSingleCameraPose(self, targetLocationWorld=[1,0,0], cameraFrameLocation=[0.22, 0, 0.89], flip=False):
cameraAxis = [0,0,1]
linkName = self.handFrame
linkName = 'iiwa_link_7'
linkFrame =self.robotSystem.robotStateModel.getLinkFrame(linkName)
cameraFrame = self.robotSystem.robotStateModel.getLinkFrame(self.handFrame)
cameraToLinkFrame = transformUtils.concatenateTransforms([cameraFrame, linkFrame.GetLinearInverse()])
ikPlanner = self.robotSystem.ikPlanner
startPoseName = 'q_nom'
endPoseName = 'reach_end'
seedPoseName = 'q_nom'
if flip:
print "FLIPPING startPoseName"
startPoseName = 'q_nom_invert_7th_joint'
seedPoseName = 'q_nom_invert_7th_joint'
pose = np.asarray([ 0. , 0. , 0. , 0. , 0. , 0. , 0. ,
-0.68 , 1.0 , -1.688 , 1.0 , -0.5635, 1.0 ])
ikPlanner.addPose(pose, startPoseName)
else:
print "NOT flipped"
constraints = []
constraints.append(ikPlanner.createPostureConstraint(startPoseName, robotstate.matchJoints('base_')))
positionConstraint = HandEyeCalibration.createPositionConstraint(targetPosition=cameraFrameLocation, linkName=linkName, linkOffsetFrame=cameraToLinkFrame, positionTolerance=0.05)
cameraGazeConstraint = HandEyeCalibration.createCameraGazeTargetConstraint(linkName=linkName, cameraToLinkFrame=cameraToLinkFrame, cameraAxis=cameraAxis, worldPoint=targetLocationWorld, coneThresholdDegrees=5.0)
constraints.append(positionConstraint)
constraints.append(cameraGazeConstraint)
constraintSet = ConstraintSet(ikPlanner, constraints, 'reach_end',
startPoseName)
if flip:
constraintSet.ikPlanner.addPose(pose, startPoseName)
constraintSet.ikParameters = IkParameters()
constraintSet.seedPoseName = seedPoseName
endPose, info = constraintSet.runIk()
returnData = dict()
returnData['info'] = info
returnData['endPose'] = endPose
return returnData
def computeSingleCameraPose(self,
targetLocationWorld=[1, 0, 0],
cameraFrameLocation=[0.22, 0, 0.89]):
cameraAxis = [0, -1,
0] # assuming we are using 'palm' as the link frame
linkName = self.handFrame
linkName = 'iiwa_link_7'
linkFrame = self.robotSystem.robotStateModel.getLinkFrame(linkName)
cameraFrame = self.robotSystem.robotStateModel.getLinkFrame(
self.handFrame)
cameraToLinkFrame = transformUtils.concatenateTransforms(
[cameraFrame, linkFrame.GetLinearInverse()])
ikPlanner = self.robotSystem.ikPlanner
startPoseName = 'q_nom'
endPoseName = 'reach_end'
seedPoseName = 'q_nom'
constraints = []
constraints.append(
ikPlanner.createPostureConstraint(startPoseName,
robotstate.matchJoints('base_')))
positionConstraint = HandEyeCalibration.createPositionConstraint(
targetPosition=cameraFrameLocation,
linkName=linkName,
linkOffsetFrame=cameraToLinkFrame,
positionTolerance=0.05)
cameraGazeConstraint = HandEyeCalibration.createCameraGazeTargetConstraint(
linkName=linkName,
cameraToLinkFrame=cameraToLinkFrame,
cameraAxis=cameraAxis,
worldPoint=targetLocationWorld,
coneThresholdDegrees=5.0)
constraints.append(positionConstraint)
constraints.append(cameraGazeConstraint)
constraintSet = ConstraintSet(ikPlanner, constraints, 'reach_end',
startPoseName)
constraintSet.ikParameters = IkParameters()
constraintSet.seedPoseName = seedPoseName
endPose, info = constraintSet.runIk()
returnData = dict()
returnData['info'] = info
returnData['endPose'] = endPose
return returnData
def planArmsForward(self):
q0 = self.getPlanningStartPose()
q1 = self.robotSystem.ikPlanner.getMergedPostureFromDatabase(q0, 'General', 'hands-forward', side='left')
q2 = self.robotSystem.ikPlanner.getMergedPostureFromDatabase(q1, 'General', 'hands-forward', side='right')
a = 0.25
q1 = (1-a)*q1 + a*np.array(q2)
ikParameters = IkParameters(usePointwise=True, maxBaseRPYDegreesPerSecond=10,
rescaleBodyNames=['l_hand', 'r_hand'],
rescaleBodyPts=list(self.robotSystem.ikPlanner.getPalmPoint(side='left')) +
list(self.robotSystem.ikPlanner.getPalmPoint(side='right')),
maxBodyTranslationSpeed=3*self.maxHandTranslationSpeed)
plan = self.robotSystem.ikPlanner.computeMultiPostureGoal([q0, q1, q2], ikParameters=ikParameters)
self.addPlan(plan)
return plan
def movePose(self, joint_positions, maxDegreesPerSecond=30):
assert isinstance(joint_positions, dict)
self.manipPlanner.lastPlan = None
startPose = self.robotSystem.robotStateJointController.q
endPose = self.ikPlanner.mergePostures(startPose, joint_positions)
ikParameters = IkParameters(maxDegreesPerSecond=maxDegreesPerSecond)
plan = self.ikPlanner.computePostureGoal(startPose, endPose, ikParameters=ikParameters)
self.manipPlanner.commitManipPlan(plan)
# block until this plan is over
planDuration = plan.plan[-1].utime/1e6
print "running plan of duration %s seconds " %(planDuration)
time.sleep(1.1*planDuration)
def planReach(self):
ikPlanner = self.robotSystem.ikPlanner
startPose = self.getPlanningStartPose()
startPoseName = 'q_reach_start'
endPoseName = 'q_reach_end'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
side = 'right'
movingReachConstraint = ikPlanner.createMovingReachConstraints(
startPoseName,
lockBase=True,
lockBack=True,
lockArm=True,
side=side)
palmToHand = ikPlanner.getPalmToHandLink(side=side)
targetFrame = om.findObjectByName('reach frame').transform
poseConstraints = ikPlanner.createPositionOrientationGraspConstraints(
side,
targetFrame,
graspToHandLinkFrame=palmToHand,
angleToleranceInDegrees=5.0)
constraints = []
constraints.extend(movingReachConstraint)
constraints.extend(poseConstraints)
constraintSet = ikplanner.ConstraintSet(ikPlanner, constraints,
endPoseName, startPoseName)
constraintSet.ikParameters = IkParameters(maxDegreesPerSecond=30)
seedPose = ikPlanner.getMergedPostureFromDatabase(startPose,
'surprise:switch',
'above_switch',
side='right')
seedPoseName = 'q_above_switch'
self.robotSystem.ikPlanner.addPose(seedPose, seedPoseName)
constraintSet.seedPoseName = seedPoseName
constraintSet.nominalPoseName = seedPoseName
endPose, info = constraintSet.runIk()
plan = constraintSet.planEndPoseGoal()
self.addPlan(plan)
def planArmsPrep2(self, startPose=None):
ikPlanner = self.robotSystem.ikPlanner
if startPose is None:
startPose = self.getPlanningStartPose()
startPoseName = 'q_arms_prep2_start'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
endPose = ikPlanner.getMergedPostureFromDatabase(startPose,
'surprise:switch',
'reach_up_1',
side='right')
ikParameters = IkParameters(maxDegreesPerSecond=30)
plan = ikPlanner.computePostureGoal(startPose,
endPose,
feetOnGround=False,
ikParameters=ikParameters)
self.addPlan(plan)
