Meta(Oculus)Quest のコントローラーの動きでロボットの手の動きを制御:ソースコード:axis_angle.py

AxisAngle__init__初期化
set_quaternionクォータニオンを設定します。
get_quaternionクォータニオンを取得します。
set_axis_angle回転軸と角度で設定します。
set_axis_angle_deg回転角と角度(deg)で設定します。
get_rotate_quaternion指定のクォータニオンで開店後のクォータニオンを得ます
rotate_axis_angle指定の軸角度で回転させます
get_rotate_vector指定のベクトルを自身のデータで回転させます。
inverse_axis軸の向きを反転させます。
get_angleクォータニオンの角度を取得します。
get_axis回転軸を取得します。
get_axis_list回転軸をpythonのlistで取得します。
nomalizeクォータニオンをノーマライズします
get_nomalized_quaternionノーマライズされたクォータニオンを取得します。
get_add_quaternion指定の軸角度で回転させた軸角度を得ます。
__add__指定の軸角度で回転させた軸角度を得ます。
__iadd__指定の軸角度で回転させます。
get_sub_quaternion指定の軸角度で逆回転させた軸角度を得ます。
__sub__指定の軸角度で逆回転させた軸角度を得ます。
__isub__指定の軸角度で逆回転させます。
__str__軸角度をテキスト出力します。printで指定した時様です。

# axis_angle.py
# copyright 2022- https://robot-creation-diary.com/

import numpy as np
import quaternion
import array


class AxisAngle():

    def __init__(self, arg1=None, arg2=None):
        self.__deg_to_rad_param = np.pi / 180.0

        if isinstance(arg1, type(None)):
            self.__quaternion = np.quaternion(1.0, 0, 0, 0)

        elif isinstance(arg1, list):
            if isinstance(arg2, type(None)):
                self.__quaternion = quaternion.as_quat_array(
                    np.array(arg1, np.float64))
            else:
                self.set_axis_angle(arg1, arg2)

        elif isinstance(arg1, np.ndarray):
            if isinstance(arg2, type(None)):
                self.__quaternion = quaternion.as_quat_array(
                    np.array(arg1, np.float64))
            else:
                self.set_axis_angle(arg1, arg2)

        elif isinstance(arg1, quaternion.quaternion):
            self.__quaternion = arg1
        elif isinstance(arg1, array.array):
            self.__quaternion = quaternion.as_quat_array(
                    np.array(arg1, np.float64))
        else:
            self.__quaternion = arg1.__quaternion

    def set_quaternion(self, quat):
        """
        クォータニオンを設定します。
        """
        self.__quaternion = quat

    def get_quaternion(self):
        """
        クォータニオンを取得します。
        """
        return self.__quaternion

    def set_axis_angle(self, axis, rad):
        """
        回転軸と角度で設定します。
        """
        axis = axis / np.sqrt(np.dot(axis, axis))
        self.__quaternion = np.quaternion(
            np.cos(rad * 0.5),
            np.float64(axis[0]) * np.sin(rad * 0.5),
            np.float64(axis[1]) * np.sin(rad * 0.5),
            np.float64(axis[2]) * np.sin(rad * 0.5))
        self.__quaternion = self.__quaternion.normalized()

    def set_axis_angle_deg(self, axis, deg):
        """
        回転角と角度(deg)で設定します。
        """
        self.set_axis_angle(axis, self.__deg_to_rad_param * deg)

    def get_rotate_quaternion(self, quat):
        """
        指定のクォータニオンで開店後のクォータニオンを得ます
        """
        return (self.__quaternion * quat)

    def rotate_axis_angle(self, axis_angle):
        """
        指定の軸角度で回転させます。
        """
        return self.__quaternion * axis_angle.__quaternion

    def get_rotate_vector(self, vector):
        """
        指定のベクトルを自身のデータで回転させます。
        """
        return quaternion.rotate_vectors(self.__quaternion, vector)

    def inverse_axis(self):
        """
        軸の向きを反転させます。
        """
        self.__quaternion[1] *= -1
        self.__quaternion[2] *= -1
        self.__quaternion[3] *= -1

    def get_angle(self):
        return self.__quaternion.angle()

    def get_axis(self):
        """
        回転軸を取得します。
        """

        return quaternion.as_rotation_vector(self.__quaternion)

    def get_axis_list(self):
        """
        回転軸をpythonのlistで取得します。
        """
        return self.get_axis().tolist()

    def nomalize(self):
        """
        クォータニオンをノーマライズします
        """
        self.__quaternion = self.__quaternion.nomalized()

    def get_nomalized_quaternion(self):
        """
        ノーマライズされたクォータニオンを取得します。
        """
        return self.__quaternion.nomalized()

    def get_add_quaternion(self, axis_angle):
        """
        指定の軸角度で回転させた軸角度を得ます。
        """
        return axis_angle.get_rotate_quaternion(self.__quaternion)

    def __add__(self, axis_angle):
        """
        指定の軸角度で回転させた軸角度を得ます。
        """
        self.result = AxisAngle(
                        axis_angle.get_rotate_quaternion(self.__quaternion))
        return self.result

    def __iadd__(self, axis_angle):
        """
        指定の軸角度で回転させます。
        """
        # axis_angleによって自分が回転させられる
        self.__quaternion = axis_angle.get_rotate_quaternion(self.__quaternion)
        return self

    def get_sub_quaternion(self, axis_angle):
        """
        指定の軸角度で逆回転させた軸角度を得ます。
        """
        return self.__quaternion * axis_angle.__quaternion.inverse()

    def __sub__(self, axis_angle):
        """
        指定の軸角度で逆回転させた軸角度を得ます。
        """
        self.result = AxisAngle(self.get_sub_quaternion(axis_angle))
        return self.result

    def __isub__(self, axis_angle):
        """
        指定の軸角度で逆回転させます。

        """
        self.__quaternion = self.__quaternion \
            * axis_angle.__quaternion.inverse()
        return self

    def __str__(self):
        """
        軸角度をテキスト出力します。printで指定した時様です。
        """
        return (str(self.get_axis()) + "," +
                str(self.get_angle() / self.__deg_to_rad_param))


if __name__ == '__main__':
    aa = AxisAngle()