座標変換

https://programming-surgeon.com/script/euler-angle/


https://www.google.com/search?q=%E3%83%AD%E3%83%BC%E3%82%AB%E3%83%AB%E5%BA%A7%E6%A8%99%E5%9E%8B%E3%80%80%E5%9B%9E%E8%BB%A2%E8%A1%8C%E5%88%97%E3%80%80%E3%82%B0%E3%83%AD%E3%83%BC%E3%83%90%E3%83%AB%E5%BA%A7%E6%A8%99%E7%B3%BB%E3%80%80&sca_esv=b2e9466f8d6921f5&rlz=1C5CHFA_enJP1083JP1085&sxsrf=ADLYWIJffH-VuYd4v0TWU0fgAgK_-TaKsA%3A1715696082395&ei=0nFDZqLiF_Kzvr0PkbuB4A4&ved=0ahUKEwiik6L8qY2GAxXyma8BHZFdAOwQ4dUDCBA&uact=5&oq=%E3%83%AD%E3%83%BC%E3%82%AB%E3%83%AB%E5%BA%A7%E6%A8%99%E5%9E%8B%E3%80%80%E5%9B%9E%E8%BB%A2%E8%A1%8C%E5%88%97%E3%80%80%E3%82%B0%E3%83%AD%E3%83%BC%E3%83%90%E3%83%AB%E5%BA%A7%E6%A8%99%E7%B3%BB%E3%80%80&gs_lp=Egxnd3Mtd2l6LXNlcnAiQuODreODvOOCq-ODq-W6p-aomeWei-OAgOWbnui7ouihjOWIl-OAgOOCsOODreODvOODkOODq-W6p-aomeezu-OAgDIJECEYoAEYChgqSLKLAVCKHVigiQFwC3gBkAEBmAGfAqAB4kSqAQYyLjY1LjK4AQPIAQD4AQGYAkCgAsU0wgIKEAAYsAMY1gQYR8ICChAjGIAEGCcYigXCAgQQIxgnwgILEAAYgAQYkQIYigXCAgsQLhiABBjRAxjHAcICBRAAGIAEwgIFEC4YgATCAgoQABiABBixAxgEwgINEAAYgAQYsQMYgwEYBMICBxAAGIAEGATCAgoQABiABBhDGIoFwgIHECMYsQIYJ8ICCBAAGIAEGKIEwgIFECEYoAHCAggQABiiBBiJBcICBxAhGKABGAqYAwCIBgGQBgqSBwUxMi41MqAHn5UC&sclient=gws-wiz-serp#vhid=8WJxYFzTlvVLJM&vssid=l&ip=1



https://programming-surgeon.com/script/coordinate-system/

https://qiita.com/q_tarou/items/46e5045068742dfb2fa6



```python

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)

```


```python
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")


```


```python

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

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




```