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