Exemplo n.º 1
0
    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
Exemplo n.º 2
0
 def createAllButLeftLegPostureConstraint(self, poseName):
     joints = robotstate.matchJoints('^(?!l_leg)')
     return self.robotSystem.ikPlanner.createPostureConstraint(poseName, joints)
Exemplo n.º 3
0
    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