手首の位置合わせプログラム解説

1, 2リンク問題を解けるように、肩の位置を軸にしてyz平面上に手首の目標位置を移動
2, 2リンク問題を解く。この際に肘が下に来るようにする。
3, 手首、肘の位置をもとの場所に戻す。
4, 肘の位置から、肩の2つの関節の回転角を求める。
5, 肩の2つの回転角と2で求めた肘の回転角を使って順運動学で手首の位置を求める。
6, 最終目標の手首の位置と、5の手首の位置のずれをなくすような、上腕軸周りの回転を求める。

robot_support.pyの以下の部分

    def four_link_ik(self, goal_vec):
        """
        4リンク問題を解く
        肩から手首までの四リンクの逆運動学を解く
        """
        # 肩、ひじ、手首が上面図で一直線上になるように位置決めする。
        # 手首の位置で、y軸周りの回転を取得

        vec_buf = copy.deepcopy(goal_vec)
        vec_buf[1] = 0
        # print("goal_vec",goal_vec)
        yangle = self.get_axis_points_angle([0, 1, 0], [1, 0, 0], vec_buf)
        yaa = AxisAngle([0, 1, 0], -yangle)
        goal_buf = yaa.get_rotate_vector(goal_vec)
        # 2リンク問題を解く
        angle1, angle2 = self.tow_link_ik([goal_buf[0], goal_buf[1]])
        if angle1 is None and angle2 is None:
            return None, None, None, None, None
        # 2リンク問題を解く終了
        # 2リンクの結果で肘と手首の位置を計算最終的な肘と手首の位置を求める。
        elbow_position, wrist_position, wristaa =\
            self.__get_xy_elbow_wrist_position(angle1, angle2)
        # zy平面にしたときのyの分だけ回転する。
        iyaa = AxisAngle([0, 1, 0], yangle)
        elbow_position = iyaa.get_rotate_vector(elbow_position)
        wrist_position = iyaa.get_rotate_vector(wrist_position)
        # 肩の回転角を取得
        shoulder_normal = self.get_normal([0, 0, 0],
                                          [0, 0, -1],
                                          elbow_position)
        shoulder_angle = self.get_angle([0, 0, 0], [0, 0, -1], elbow_position)
        # 肩の回転角から肘の位置を計算
        shoulder_aa1 = AxisAngle(shoulder_normal, shoulder_angle)
        posi1 = shoulder_aa1.get_rotate_vector([0, 0, -self.arm_length1])
        shoulder_angle = self.get_angle_sine(shoulder_normal,
                                             shoulder_angle,
                                             posi1,
                                             elbow_position)
        shoulder_aa = AxisAngle(shoulder_normal, shoulder_angle)
        # 各ロボットのモータ間接に合わせて回転角を変換
        # 肩を2軸の回転角として取得

        shoulder_xangle, shoulder_yangle =\
            self.get_shoulder_angles(elbow_position, shoulder_aa)
        # 肩2つの角度と肘関節の角度を用いで肘と手首の位置を求める。手首の位置がずれている場合腕回りの回転を求める。

        elbow_position2, wrist_position2, wrist_aa2 =\
            self.__get_check_wrist_position(shoulder_xangle,
                                            shoulder_yangle,
                                            angle2)
        arm_angle = self.get_axis_points_angle(elbow_position2,
                                               wrist_position2,
                                               wrist_position)
        arm_angle = self.get_angle_sine(elbow_position2,
                                        arm_angle,
                                        wrist_position2,
                                        wrist_position)

        arm_aa = AxisAngle(elbow_position2, arm_angle)
        wrist_aa2 += arm_aa
        # 肩上下回転, 肩片脇開閉、 腕回転 肘関節 手首回転角
        return shoulder_xangle, shoulder_yangle, arm_angle, angle2, wrist_aa2