예제 #1
0
    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
예제 #2
0
    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
예제 #3
0
    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
예제 #4
0
    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
예제 #5
0
    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
예제 #6
0
    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
예제 #7
0
    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