# 運動学的方程式の導出 （XYZオイラー角）

吉田勝俊（宇都宮大学）

## 参考情報
- [SymPyで代数演算してみる - Qiita](https://qiita.com/zawawahoge/items/1be137a8147902a5e6cb)
- [Matrices (linear algebra) &#8212; SymPy 1.6.2 documentation](https://docs.sympy.org/latest/modules/matrices/matrices.html)


```python
import sympy as sym #数式処理ライブラリ
sym.init_printing() #数式を綺麗に表示する設定
```

## Sympy 変数・関数


```python
om1, om2, om3 = sym.symbols('ω_1 ω_2 ω_3')
th1, th2, th3 = sym.symbols('θ_1 θ_2 θ_3', Real=True, positive=True)
dth1, dth2, dth3 = sym.symbols('\dot{θ}_1 \dot{θ}_2 \dot{θ}_3', Real=True, positive=True)
```


```python
th = sym.Matrix([th1,th2,th3]) #オイラー角
dth = sym.Matrix([dth1,dth2,dth3]) #その時間微分

display(th)
display(dth)
```

#### テスト


```python
th.dot(dth)
```

## ZYZオイラー角による回転行列


```python
def RotX(th): #X軸回転の行列
    return sym.Matrix([
        [1, 0, 0],
        [0, sym.cos(th), -sym.sin(th)],
        [0, sym.sin(th),  sym.cos(th)],
    ])

def RotY(th): #Y軸回転の行列
    return sym.Matrix([
        [sym.cos(th), 0, sym.sin(th)],
        [0, 1, 0],
        [-sym.sin(th), 0, sym.cos(th)],
    ])

def RotZ(th): #Z軸回転の行列
    return sym.Matrix([
        [sym.cos(th), -sym.sin(th), 0],
        [sym.sin(th),  sym.cos(th), 0],
        [0, 0, 1],
    ])

Rot = RotZ(th3)*RotY(th2)*RotX(th1)
Rot
```

### LaTeXソースの出力


```python
print(sym.latex(Rot))
```

### 回転行列の時間微分


```python
def sympy_dt_param(expr, param_vec, dparam_vec):
    """
    パラメータを時間の関数として時間微分する関数
        param_vec: パラメータベクトル sympy.Matrix([sympyのシンボル達]) 
        dparam_vec: パラメータベクトルの微分 sympy.Matrix([sympyのシンボル達])
    """
    dim = len(param_vec)
    result = expr
   
    # 仮の時間変数
    t = sym.symbols('t')

    # パラメータを時間関数で置換
    func_vec = []
    for i in range(dim):
        lab = 'param' + str(i) #仮の変数表示名
        fun = sym.symbols(lab, cls=sym.Function, Real=True)
        func_vec.append( fun(t) )

    func_vec = sym.Matrix(func_vec) #sympy型へ変換
    
    for i in range(dim):
        result = result.replace(param_vec[i],func_vec[i])

    # 時間微分を実行
    result = result.diff()
    dfunc_vec = func_vec.diff()
    
    # 時間関数を元のパラメータに戻す
    for i in range(dim):
        result = result.replace(dfunc_vec[i], dparam_vec[i])
        result = result.replace(func_vec[i], param_vec[i])

    return result
```

回転行列を実際に時間微分する


```python
dRot = sympy_dt_param(Rot, th, dth)
dRot
```

### 回転行列の逆行列


```python
invRot = Rot.transpose() #回転行列なので転置で求まる
invRot
```

### 角速度ベクトルの外積行列 $[\boldsymbol{\omega}\times]:=R^{-1}\dot{R}$


```python
OMcross = sym.simplify(invRot*dRot)
```

### 外積行列 $[\boldsymbol{\omega}\times]:=R^{-1}\dot{R}$ から角速度ベクトル $\boldsymbol{\omega}$ を抽出


```python
OMvec = sym.Matrix([OMcross[2,1],OMcross[0,2],OMcross[1,0]])
OMvec
```

### 行列表示 $\boldsymbol{\omega}=\Omega \dot{\boldsymbol{\theta}}$ の表現行列 $\Omega$


```python
tmp = OMvec
OMmat = tmp.jacobian(dth)
OMmat
```

### LaTeXソースの出力


```python
print(sym.latex(OMmat))
```

### $\Omega(\boldsymbol{\theta},\dot{\boldsymbol{\theta}})$ を $\dot{\boldsymbol{\theta}}$ について解き，運動学的方程式 $\dot{\boldsymbol{\theta}}=K(\boldsymbol{\theta},\boldsymbol{\omega})$ を導く

#### $\dot{\boldsymbol{\theta}}$ について解く


```python
var = dth
oms = sym.Matrix([om1, om2, om3])
equations = OMmat*var - oms
result = sym.solve(equations, var)
result
```

#### 解から$K(\boldsymbol{\theta},\boldsymbol{\omega})$を作る


```python
Kfunc = sym.Matrix([result[dth[i]] for i in range(3)])
Kfunc = sym.simplify(Kfunc)
Kfunc
```

## 運動学的方程式 $\dot{\boldsymbol{\theta}}=K(\boldsymbol{\theta},\boldsymbol{\omega})$ の行列表示

- 線形変換 $K(\boldsymbol{q},\boldsymbol{\omega})$ のヤコビ行列＝表現行列なので，ヤコビ行列を求める関数 [jacobian(X)](https://docs.sympy.org/latest/modules/matrices/matrices.html#sympy.matrices.matrices.MatrixCalculus.jacobian) を流用しました．

### $\dot{\boldsymbol{\theta}} = K_{\omega}\boldsymbol{\theta}$ 型の表現行列

オイラー角は，$\theta_1$, $\theta_2$, $\theta_3$ に関して非線形（三角関数の中身）なので，この型の表現行列は取れません． 

### $\dot{\boldsymbol{\theta}} = K_{\boldsymbol{\theta}}\boldsymbol{\omega}$ 型の表現行列


```python
Kth = Kfunc.jacobian(oms)
Kth
```

### LaTeXソースの出力


```python
print(sym.latex(Kth))
```

3e8366359cb78e7158d9bb9e4fa78c2f0b72294a
ipynb
Jupyter Notebook
m3d/Colab/Python_9.2.ipynb
ktysd/_colab_test
0ffc4a63dce926e21647f4497269ac90f4eaa941
MIT
m3d/Colab/Python_9.2.ipynb
ktysd/_colab_test
0ffc4a63dce926e21647f4497269ac90f4eaa941
MIT
m3d/Colab/Python_9.2.ipynb
ktysd/_colab_test
0ffc4a63dce926e21647f4497269ac90f4eaa941
MIT
Qwen/Qwen-72B
1. YES
2. YES