座標変換

import numpy as np

def rodrigues_rotation_matrix(theta, rot_axis):
    """
    ロドリゲスの回転公式を使用して、指定された軸と角度での回転行列を返します。
    
    Parameters:
    rot_axis : array_like
        回転軸を表すベクトル
    theta : float
        回転角（ラジアン）
    
    Returns:
    R : ndarray
        回転行列
    """
    # 回転軸を正規化する
    axis = np.asarray(rot_axis)
    axis = axis / np.linalg.norm(axis)

    # 各要素を計算する
    cos = np.cos(theta)
    sin = np.sin(theta)
    n1 = axis[0]
    n2 = axis[1]
    n3 = axis[2]

    # ロドリゲスの回転公式を使用して回転行列 R を計算する
    R = np.array([[n1**2 * (1 - cos) + cos       ,   n1 * n2 * (1 - cos) - n3 * sin,    n1 * n3 * (1 - cos) + n2 * sin],
                  [n1 * n2 * (1 - cos) + n3 * sin,   n2**2 * (1 - cos) + cos       ,    n2 * n3 * (1 - cos) - n1 * sin],
                  [n1 * n3 * (1 - cos) - n2 * sin,   n2 * n3 * (1 - cos) + n1 * sin,    n3**2 * (1 - cos) + cos       ]])
    return R

# 使用例
axis = [8.2, 2.9, 2.1]  # 回転軸
theta = 0.5  # 回転角

np.set_printoptions(precision=2, suppress=True)
R = rodrigues_rotation_matrix(theta, axis)
print(R)

import numpy as np

def rotation_matrix_to_euler_xyz(R):
    """
    XYZオイラー角を使用して、回転行列からオイラー角を計算します。
    
    Parameters:
    R : ndarray
        3x3の回転行列
        
    Returns:
    roll, pitch, yaw : float
        XYZオイラー角（ラジアン）
    """
    # Roll (X軸まわりの回転)
    roll = np.arctan2(R[2, 1], R[2, 2])
    
    # Pitch (Y軸まわりの回転)
    pitch = -np.arcsin(R[2, 0])
    
    # Yaw (Z軸まわりの回転)
    yaw = np.arctan2(R[1, 0], R[0, 0])
    
    return roll, pitch, yaw

# 回転行列の定義
R = np.array([[0.98, -0.08,  0.18],
              [ 0.15,  0.89, -0.43],
              [-0.13,  0.45,  0.88]])


# XYZオイラー角を計算
roll, pitch, yaw = rotation_matrix_to_euler_xyz(R)

print("XYZオイラー角:")
print(f"Roll: {roll} rad")
print(f"Pitch: {pitch} rad")
print(f"Yaw: {yaw} rad")

# 使用例
axis = [1, 1, 1]  # 回転軸
theta = 0.5  # 回転角

np.set_printoptions(precision=2, suppress=True)
R = rodrigues_rotation_matrix(theta, axis)
print(R)