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 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 = ikconstraints.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(
ikconstraints.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 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(
ikconstraints.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 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(
ikconstraints.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 planCenterWeight(self, startPose=None):
ikPlanner = self.robotSystem.ikPlanner
if startPose is None:
startPose = self.getPlanningStartPose()
startPoseName = 'q_lean_right'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
endPoseName = 'q_egress_end'
footFixedConstraints = ikPlanner.createFixedFootConstraints(
startPoseName)
backConstraint = ikPlanner.createMovingBackLimitedPostureConstraint()
armsLocked = ikPlanner.createLockedArmsPostureConstraints(
startPoseName)
constraints = [backConstraint]
constraints.extend(footFixedConstraints)
constraints.extend(armsLocked)
constraints.append(
ikconstraints.QuasiStaticConstraint(
leftFootEnabled=True,
rightFootEnabled=True,
pelvisEnabled=False,
shrinkFactor=self.quasiStaticShrinkFactor))
constraints.append(
self.robotSystem.ikPlanner.createKneePostureConstraint([0.7, 2.5]))
constraintSet = ConstraintSet(ikPlanner, constraints, endPoseName,
startPoseName)
constraintSet.seedPoseName = 'q_start'
constraintSet.nominalPoseName = 'q_nom'
endPose = constraintSet.runIk()
keyFramePlan = constraintSet.planEndPoseGoal()
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 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(
ikconstraints.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 = ikconstraints.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
