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 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(ik.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 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 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 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
