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 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 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 planGetWeightOverFeet(self):
startPose = self.getPlanningStartPose()
startPoseName = 'q_egress_start'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
endPoseName = 'q_egress_end'
constraints = []
constraints.append(
ik.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 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 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 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)
def planTurn(self, wristAngleCW=np.radians(320)):
ikParameters = IkParameters(maxDegreesPerSecond=self.speedTurn)
startPose = self.getPlanningStartPose()
wristAngleCW = min(
np.radians(320) - 0.01, max(-np.radians(160) + 0.01, wristAngleCW))
if self.graspingHand == 'left':
postureJoints = {'l_arm_lwy': -np.radians(160) + wristAngleCW}
else:
postureJoints = {'r_arm_lwy': np.radians(160) - wristAngleCW}
endPose = self.ikPlanner.mergePostures(startPose, postureJoints)
plan = self.ikPlanner.computePostureGoal(startPose,
endPose,
ikParameters=ikParameters)
app.displaySnoptInfo(1)
self.addPlan(plan)
def planReach(self, verticalOffset=None, **kwargs):
startPose = self.getPlanningStartPose()
insert_plan = self.planInsertTraj(self.speedHigh,
lockFeet=True,
usePoses=True,
resetPoses=True,
**kwargs)
info = max(insert_plan.plan_info)
reachPose = robotstate.convertStateMessageToDrakePose(
insert_plan.plan[0])
ikParameters = IkParameters(
maxDegreesPerSecond=2 * self.speedTurn,
rescaleBodyNames=[
self.ikPlanner.getHandLink(side=self.graspingHand)
],
rescaleBodyPts=list(
self.ikPlanner.getPalmPoint(side=self.graspingHand)),
maxBodyTranslationSpeed=self.maxHandTranslationSpeed)
plan = self.ikPlanner.computePostureGoal(startPose,
reachPose,
ikParameters=ikParameters)
plan.plan_info = [info] * len(plan.plan_info)
lcmUtils.publish('CANDIDATE_MANIP_PLAN', plan)
self.addPlan(plan)
def planInsertTraj(self,
speed,
lockFeet=True,
lockBase=None,
resetBase=False,
wristAngleCW=0,
startPose=None,
verticalOffset=0.01,
usePoses=False,
resetPoses=True,
planFromCurrentRobotState=False,
retract=False):
ikParameters = IkParameters(
usePointwise=False,
maxDegreesPerSecond=speed,
numberOfAddedKnots=1,
quasiStaticShrinkFactor=self.quasiStaticShrinkFactor,
fixInitialState=planFromCurrentRobotState)
_, yaxis, _ = transformUtils.getAxesFromTransform(
self.computeGraspFrame().transform)
yawDesired = np.arctan2(yaxis[1], yaxis[0])
if startPose is None:
startPose = self.getPlanningStartPose()
nominalPose = self.getNominalPose()
self.ikPlanner.addPose(nominalPose, self.nominalPoseName)
self.ikPlanner.addPose(startPose, self.startPoseName)
self.ikPlanner.reachingSide = self.graspingHand
constraints = []
constraints.extend(
self.createBaseConstraints(resetBase, lockBase, lockFeet,
yawDesired))
constraints.append(self.createBackPostureConstraint())
constraints.append(self.ikPlanner.createQuasiStaticConstraint())
constraints.extend(self.createFootConstraints(lockFeet))
constraints.append(
self.ikPlanner.createLockedArmPostureConstraint(
self.startPoseName))
constraints.append(
self.ikPlanner.createKneePostureConstraint([0.7, 2.5]))
constraints.append(self.createElbowPostureConstraint())
constraints.append(self.createStaticTorqueConstraint())
constraints.append(self.createHandGazeConstraint())
constraints.append(self.createHandFixedOrientConstraint())
constraints.append(
self.createWristAngleConstraint(wristAngleCW,
planFromCurrentRobotState))
constraints.extend(
self.createAllHandPositionConstraints(self.coaxialTol, retract))
if retract:
startPoseName = self.getStartPoseName(planFromCurrentRobotState,
True, usePoses)
endPoseName = self.getEndPoseName(True, usePoses)
endPose = self.ikPlanner.jointController.getPose(endPoseName)
endPose = self.ikPlanner.mergePostures(
endPose, robotstate.matchJoints('lwy'), startPose)
endPoseName = 'q_retract'
self.ikPlanner.addPose(endPose, endPoseName)
else:
startPoseName = self.getStartPoseName(planFromCurrentRobotState,
retract, usePoses)
endPoseName = self.getEndPoseName(retract, usePoses)
plan = self.ikPlanner.runIkTraj(constraints,
startPoseName,
endPoseName,
self.nominalPoseName,
ikParameters=ikParameters)
if resetPoses and not retract and max(plan.plan_info) <= 10:
self.setReachAndTouchPoses(plan)
return plan
