controller.py 文件源码

def moveGripperTo(self, position, rollpitchyaw=[-np.pi, 0.0, 0.0], timeout=1, useInitGuess=False, wait=False, rightArm=True, ret=False):
        # Move using IK and joint trajectory controller
        # Attach new pose to a frame
        poseData = list(position) + list(rollpitchyaw)
        frameData = PyKDL.Frame()
        poseFrame = dh.array2KDLframe(poseData)
        poseFrame = dh.frameConversion(poseFrame, frameData)
        pose = dh.KDLframe2Pose(poseFrame)

        # Create a PoseStamped message and perform transformation to given frame
        ps = PoseStamped()
        ps.header.frame_id = self.frame
        ps.pose.position = pose.position
        ps.pose.orientation = pose.orientation
        ps = self.tf.transformPose(self.frame, ps)

        # Perform IK
        if rightArm:
            ikGoal = self.rightArmKdl.inverse(ps.pose, q_guess=self.initRightJointGuess, min_joints=self.rightJointLimitsMin, max_joints=self.rightJointLimitsMax)
            # ikGoal = self.rightArmKdl.inverse(ps.pose, q_guess=(self.initRightJointGuess if useInitGuess or self.currentRightJointPositions is None else self.currentRightJointPositions), min_joints=self.rightJointLimitsMin, max_joints=self.rightJointLimitsMax)
        else:
            ikGoal = self.leftArmKdl.inverse(ps.pose, q_guess=self.initLeftJointGuess, min_joints=self.leftJointLimitsMin, max_joints=self.leftJointLimitsMax)
            # ikGoal = self.leftArmKdl.inverse(ps.pose, q_guess=(self.initLeftJointGuess if useInitGuess or self.currentLeftJointPositions is None else self.currentLeftJointPositions), min_joints=self.leftJointLimitsMin, max_joints=self.leftJointLimitsMax)
        if ikGoal is not None:
            if not ret:
                self.moveToJointAngles(ikGoal, timeout=timeout, wait=wait, rightArm=rightArm)
            else:
                return ikGoal
        else:
            print 'IK failed'