<jupyter_start><jupyter_text>運動学的方程式の導出 （オイラーパラメータ） 吉田勝俊（宇都宮大学）  参考情報- [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)<jupyter_code>import sympy as sym #数式処理ライブラリ
sym.init_printing() #数式を綺麗に表示する設定<jupyter_output><empty_output><jupyter_text>Sympy 変数・関数<jupyter_code>om1, om2, om3 = sym.symbols('\omega_1 \omega_2 \omega_3')
q0, q1, q2, q3 = sym.symbols('q0 q1 q2 q3', Real=True, positive=True)
dq0, dq1, dq2, dq3 = sym.symbols('\dot{q}_0 \dot{q}_1 \dot{q}_2 \dot{q}_3', Real=True, positive=True)
q = sym.Matrix([q0,q1,q2,q3]) #オイラーパラメータ
dq = sym.Matrix([dq0,dq1,dq2,dq3]) #その時間微分

display(q)
display(dq)<jupyter_output><empty_output><jupyter_text>テスト<jupyter_code>q.dot(dq)<jupyter_output><empty_output><jupyter_text>クォータニオンによる回転行列<jupyter_code>Rot = sym.Matrix([
    [ 1 -2*q2**2 -2*q3**2,    2*q1*q2 -2*q0*q3, 2*q0*q2 +2*q1*q3],
    [ 2*q1*q2 +2*q0*q3, 1 -2*q1**2 -2*q3**2,   -2*q0*q1 +2*q2*q3],
    [-2*q0*q2 +2*q1*q3, 2*q0*q1 +2*q2*q3, 1 -2*q1**2 -2*q2**2]
])
Rot<jupyter_output><empty_output><jupyter_text>回転行列の時間微分<jupyter_code>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<jupyter_output><empty_output><jupyter_text>回転行列を実際に時間微分する<jupyter_code>dRot = sympy_dt_param(Rot, q, dq)
dRot<jupyter_output><empty_output><jupyter_text>回転行列の逆行列<jupyter_code>invRot = Rot.transpose() #回転行列なので転置で求まる
invRot<jupyter_output><empty_output><jupyter_text>角速度ベクトルの外積行列 $[\boldsymbol{\omega}\times]:=R^{-1}\dot{R}$<jupyter_code>OMcross = sym.simplify(invRot*dRot)<jupyter_output><empty_output><jupyter_text>外積行列 $[\boldsymbol{\omega}\times]:=R^{-1}\dot{R}$ から角速度ベクトル $\boldsymbol{\omega}$ を抽出<jupyter_code>OMvec = sym.Matrix([OMcross[2,1],OMcross[0,2],OMcross[1,0]])
OMvec<jupyter_output><empty_output><jupyter_text>行列表示 $\boldsymbol{\omega}=\Omega \dot{\boldsymbol{q}}$ の表現行列 $\Omega$ - 線形変換のヤコビ行列＝表現行列なので，ヤコビ行列を求める関数 [jacobian(X)](https://docs.sympy.org/latest/modules/matrices/matrices.htmlsympy.matrices.matrices.MatrixCalculus.jacobian) を流用しました．- $q_0$, $q_1$, $q_2$, $q_3$ が拘束条件下 $q_0^2+q_1^2+q_2^2+q_3^2=1$ で独立でなく，sympy の `simplify` のクセと相まって，以下の簡約化は危うい均衡状態にあります．ちょっと変えると，式形が変わります（数学的には等価だが）．<jupyter_code>tmp = OMvec
tmp = sym.Matrix([
    tmp[0].replace(q1, sym.sqrt(1-(q0**2 +q2**2 +q3**2))),
    tmp[1].replace(q2, sym.sqrt(1-(q0**2 +q1**2 +q3**2))),
    tmp[2].replace(q3, sym.sqrt(1-(q0**2 +q1**2 +q2**2))),
])
tmp = tmp.jacobian(dq)
tmp = sym.simplify(tmp)
tmp = tmp.replace(1, (q0**2 +q1**2 +q2**2 +q3**2))
tmp = sym.MutableMatrix(tmp)
# 綺麗にならない成分を微調整
tmp2 = tmp[0,1]
tmp2 = tmp2.replace(q0, sym.sqrt(1-(q1**2 +q2**2 +q3**2)))
tmp2 = sym.simplify(tmp2)
tmp2 = tmp2.replace(1, (q0**2 +q1**2 +q2**2 +q3**2))
tmp[0,1] = tmp2
# 綺麗にならない成分を微調整
tmp2 = tmp[1,2]
tmp2 = tmp2.replace(q0, sym.sqrt(1-(q1**2 +q2**2 +q3**2)))
tmp2 = sym.simplify(tmp2)
tmp2 = tmp2.replace(1, (q0**2 +q1**2 +q2**2 +q3**2))
tmp[1,2] = tmp2
# 綺麗にならない成分を微調整
tmp2 = tmp[2,3]
tmp2 = tmp2.replace(q0, sym.sqrt(1-(q1**2 +q2**2 +q3**2)))
tmp2 = sym.simplify(tmp2)
tmp2 = tmp2.replace(1, (q0**2 +q1**2 +q2**2 +q3**2))
tmp[2,3] = tmp2
OMmat = tmp
OMmat<jupyter_output><empty_output><jupyter_text>LaTeXソースの出力<jupyter_code>print(sym.latex(OMmat))<jupyter_output><empty_output><jupyter_text>$\Omega(\boldsymbol{q},\dot{\boldsymbol{q}})$ を $\dot{\boldsymbol{q}}$ について解き，運動学的方程式 $\dot{\boldsymbol{q}}=K(\boldsymbol{q},\boldsymbol{\omega})$ を導く  $\dot{\boldsymbol{q}}$ について解く（注）若干，計算に時間が掛かる場合があります．<jupyter_code>var = dq
oms = sym.Matrix([om1, om2, om3])
equation1 = OMmat*var - oms
equation2 = q.dot(dq)
equations = equation1.row_insert(3, sym.Matrix([equation2]))
result = sym.solve(equations, var)
result<jupyter_output><empty_output><jupyter_text>解から$K(\boldsymbol{q},\boldsymbol{\omega})$を作る<jupyter_code>tmp = sym.Matrix([result[dq[i]] for i in range(4)])
tmp = tmp.replace(q0, sym.sqrt(1-q1**2-q2**2-q3**2))
tmp = sym.simplify(tmp)
tmp = tmp.replace(1, q0**2+q1**2+q2**2+q3**2)
Kfunc = tmp
Kfunc<jupyter_output><empty_output><jupyter_text>LaTeXソースの出力<jupyter_code>print(sym.latex(2*Kfunc))<jupyter_output><empty_output><jupyter_text>運動学的方程式 $\dot{\boldsymbol{q}}=K(\boldsymbol{q},\boldsymbol{\omega})$ の行列表示 - 線形変換 $K(\boldsymbol{q},\boldsymbol{\omega})$ のヤコビ行列＝表現行列なので，ヤコビ行列を求める関数 [jacobian(X)](https://docs.sympy.org/latest/modules/matrices/matrices.htmlsympy.matrices.matrices.MatrixCalculus.jacobian) を流用しました．  $\dot{\boldsymbol{q}} = K_{\omega}\boldsymbol{q}$ 型の表現行列<jupyter_code>Kω = Kfunc.jacobian(q)
Kω<jupyter_output><empty_output><jupyter_text>$\dot{\boldsymbol{q}} = K_{\boldsymbol{q}}\boldsymbol{\omega}$ 型の表現行列<jupyter_code>Kq = Kfunc.jacobian(oms)
Kq<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.1956130888169723, 'fraction_numerical': 0.04998202085580726, 'mean_word_length': 6.31011826544021, 'pattern_counts': {'":': 0, '<': 65, '<?xml version=': 0, '>': 65, 'https://': 4, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '16', 'dir': 'jupyter-structured-clean-dedup', 'id': '280954', 'license': 'MIT', 'n_tokens_mistral': 2976, 'n_tokens_neox': 2926, 'n_words': 308, 'path': 'm3d/Colab/Python_9.1.ipynb', 'repo_name': 'ktysd/_colab_test', 'size': '5562'}