# セクション一個だけのダイナミクスを導出

必要なものをインポート


```python
import sympy as sy
from sympy.printing.numpy import NumPyPrinter
from sympy import julia_code
from sympy.utilities.codegen import codegen
import tqdm
import os
from pathlib import Path


from kinematics import Local
#from kinematics_mac2 import Local

from dynamics import operate_V, operate_tilde, operate_T2
from dynamics import Dynamics


```


```python
# パラメータ
m = 0.13
r = 0.0125
Ixx = 1/4 * r**2
G_v = sy.Matrix([[0, 0, -9.81]]).T
Ki = 1700  # 弾性係数
Di = 110  # ダンピング
```

変数を宣言


```python
# アクチュエータベクトル
l1, l2, l3 = sy.symbols("l1, l2, l3")
q = sy.Matrix([[l1, l2, l3]]).T

# アクチュエータベクトルの微分
l1_dot, l2_dot, l3_dot = sy.symbols("l1_dot, l2_dot, l3_dot")
q_dot = sy.Matrix([[l1_dot, l2_dot, l3_dot]]).T

# ディスク位置を表すスカラ変数
xi = sy.Symbol("xi")
```

運動学関係


```python
kinema = Local()

P = kinema.P(q, xi)  # 位置ベクトル
R = kinema.R(q, xi)  # 回転行列


# 位置のアクチュエータ微分
dPdl1 = sy.diff(P, l1)
dPdl2 = sy.diff(P, l2)
dPdl3 = sy.diff(P, l3)

# 回転行列のアクチュエータ微分
dRdl1 = sy.diff(R, l1)
dRdl2 = sy.diff(R, l2)
dRdl3 = sy.diff(R, l3)
```

慣性行列


```python
# 回転方向の慣性行列
M_omega_1_1 = Ixx * sy.integrate(
    operate_T2(dRdl1.T * dRdl1),
    (xi, 0, 1)
)
M_omega_1_2 = Ixx * sy.integrate(
    operate_T2(dRdl1.T * dRdl2),
    (xi, 0, 1)
)
M_omega_1_3 = Ixx * sy.integrate(
    operate_T2(dRdl1.T * dRdl3),
    (xi, 0, 1)
)
M_omega_2_1 = Ixx * sy.integrate(
    operate_T2(dRdl2.T * dRdl1),
    (xi, 0, 1)
)
M_omega_2_2 = Ixx * sy.integrate(
    operate_T2(dRdl2.T * dRdl2),
    (xi, 0, 1)
)
M_omega_2_3 = Ixx * sy.integrate(
    operate_T2(dRdl2.T * dRdl3),
    (xi, 0, 1)
)
M_omega_3_1 = Ixx * sy.integrate(
    operate_T2(dRdl3.T * dRdl1),
    (xi, 0, 1)
)
M_omega_3_2 = Ixx * sy.integrate(
    operate_T2(dRdl3.T * dRdl2),
    (xi, 0, 1)
)
M_omega_3_3 = Ixx * sy.integrate(
    operate_T2(dRdl3.T * dRdl3),
    (xi, 0, 1)
)
M_omega = sy.Matrix([
    [M_omega_1_1, M_omega_1_2, M_omega_1_3],
    [M_omega_2_1, M_omega_2_2, M_omega_2_3],
    [M_omega_3_1, M_omega_3_2, M_omega_3_3],
])

```


```python
# 並進運動の回転行列
M_v_1_1 = m * sy.integrate(
    dPdl1.T * dPdl1,
    (xi, 0, 1)
)
M_v_1_2 = m * sy.integrate(
    dPdl1.T * dPdl2,
    (xi, 0, 1)
)
M_v_1_3 = m * sy.integrate(
    dPdl1.T * dPdl3,
    (xi, 0, 1)
)
M_v_2_1 = m * sy.integrate(
    dPdl2.T * dPdl1,
    (xi, 0, 1)
)
M_v_2_2 = m * sy.integrate(
    dPdl2.T * dPdl2,
    (xi, 0, 1)
)
M_v_2_3 = m * sy.integrate(
    dPdl2.T * dPdl3,
    (xi, 0, 1)
)
M_v_3_1 = m * sy.integrate(
    dPdl3.T * dPdl1,
    (xi, 0, 1)
)
M_v_3_2 = m * sy.integrate(
    dPdl3.T * dPdl2,
    (xi, 0, 1)
)
M_v_3_3 = m * sy.integrate(
    dPdl3.T * dPdl3,
    (xi, 0, 1)
)
M_v = sy.Matrix([
    [M_v_1_1, M_v_1_2, M_v_1_3],
    [M_v_2_1, M_v_2_2, M_v_2_3],
    [M_v_3_1, M_v_3_2, M_v_3_3],
])
```


```python
M = M_omega + M_v  # 慣性行列
```

遠心力とコリオリ


```python
# 慣性行列のアクチュエータ微分
M_1_1_diff_by_l1 = sy.diff(M[0, 0], l1)
M_1_1_diff_by_l2 = sy.diff(M[0, 0], l2)
M_1_1_diff_by_l3 = sy.diff(M[0, 0], l3)

M_1_2_diff_by_l1 = sy.diff(M[0, 1], l1)
M_1_2_diff_by_l2 = sy.diff(M[0, 1], l2)
M_1_2_diff_by_l3 = sy.diff(M[0, 1], l3)

M_1_3_diff_by_l1 = sy.diff(M[0, 2], l1)
M_1_3_diff_by_l2 = sy.diff(M[0, 2], l2)
M_1_3_diff_by_l3 = sy.diff(M[0, 2], l3)

M_2_1_diff_by_l1 = sy.diff(M[1, 0], l1)
M_2_1_diff_by_l2 = sy.diff(M[1, 0], l2)
M_2_1_diff_by_l3 = sy.diff(M[1, 0], l3)

M_2_2_diff_by_l1 = sy.diff(M[1, 1], l1)
M_2_2_diff_by_l2 = sy.diff(M[1, 1], l2)
M_2_2_diff_by_l3 = sy.diff(M[1, 1], l3)

M_2_3_diff_by_l1 = sy.diff(M[1, 2], l1)
M_2_3_diff_by_l2 = sy.diff(M[1, 2], l2)
M_2_3_diff_by_l3 = sy.diff(M[1, 2], l3)

M_3_1_diff_by_l1 = sy.diff(M[2, 0], l1)
M_3_1_diff_by_l2 = sy.diff(M[2, 0], l2)
M_3_1_diff_by_l3 = sy.diff(M[2, 0], l3)

M_3_2_diff_by_l1 = sy.diff(M[2, 1], l1)
M_3_2_diff_by_l2 = sy.diff(M[2, 1], l2)
M_3_2_diff_by_l3 = sy.diff(M[2, 1], l3)

M_3_3_diff_by_l1 = sy.diff(M[2, 2], l1)
M_3_3_diff_by_l2 = sy.diff(M[2, 2], l2)
M_3_3_diff_by_l3 = sy.diff(M[2, 2], l3)

M_dot = [
    [
        [M_1_1_diff_by_l1, M_1_1_diff_by_l2, M_1_1_diff_by_l3,],
        [M_1_2_diff_by_l1, M_1_2_diff_by_l2, M_1_3_diff_by_l3,],
        [M_1_3_diff_by_l1, M_1_3_diff_by_l2, M_1_3_diff_by_l3,],
    ],
    [
        [M_2_1_diff_by_l1, M_2_1_diff_by_l2, M_2_1_diff_by_l3,],
        [M_2_2_diff_by_l1, M_2_2_diff_by_l2, M_2_3_diff_by_l3,],
        [M_2_3_diff_by_l1, M_2_3_diff_by_l2, M_2_3_diff_by_l3,],
    ],
    [
        [M_3_1_diff_by_l1, M_3_1_diff_by_l2, M_3_1_diff_by_l3,],
        [M_3_2_diff_by_l1, M_3_2_diff_by_l2, M_3_3_diff_by_l3,],
        [M_3_3_diff_by_l1, M_3_3_diff_by_l2, M_3_3_diff_by_l3,],
    ],
]  # 混乱するのでまとめる
```


```python
def Gamma(i, j, k):
    """慣性行列のクリストッフェル記号"""
    return 1/2 * (M_dot[k][j][i] + M_dot[k][i][j] - M_dot[i][j][k])

C_1_1 = sum([Gamma(0, 0, i) * q_dot[i, 0] for i in range(3)])
C_1_2 = sum([Gamma(0, 1, i) * q_dot[i, 0] for i in range(3)])
C_1_3 = sum([Gamma(0, 2, i) * q_dot[i, 0] for i in range(3)])

C_2_1 = sum([Gamma(1, 0, i) * q_dot[i, 0] for i in range(3)])
C_2_2 = sum([Gamma(1, 1, i) * q_dot[i, 0] for i in range(3)])
C_2_3 = sum([Gamma(1, 2, i) * q_dot[i, 0] for i in range(3)])

C_3_1 = sum([Gamma(2, 0, i) * q_dot[i, 0] for i in range(3)])
C_3_2 = sum([Gamma(2, 1, i) * q_dot[i, 0] for i in range(3)])
C_3_3 = sum([Gamma(2, 2, i) * q_dot[i, 0] for i in range(3)])

C = sy.Matrix([
    [C_1_1, C_1_2, C_1_3,],
    [C_2_1, C_2_2, C_2_3,],
    [C_3_1, C_3_2, C_3_3,],
])  # コリオリ・遠心力
```

重力ベクトル


```python
# 線速度ヤコビアン
J_v_1 = R.T * dPdl1
J_v_2 = R.T * dPdl2
J_v_3 = R.T * dPdl3

G_1 = m * sy.integrate(
    J_v_1.T * R.T * G_v,
    (xi, 0, 1)
)
G_2 = m * sy.integrate(
    J_v_2.T * R.T * G_v,
    (xi, 0, 1)
)
G_3 = m * sy.integrate(
    J_v_3.T * R.T * G_v,
    (xi, 0, 1)
)
G = sy.Matrix([[G_1, G_2, G_3]]).T  # 重力ベクトル
```

その他


```python
K = sy.diag([Ki, Ki, Ki], unpack=True)
D = sy.diag([Di, Di, Di], unpack=True)
```

式を整理（時間かかる）


```python
M = sy.simplify(M)
C = sy.simplify(C)
G = sy.simplify(G)
```

結果をファイルに保存


```python
cwd = str(Path().resolve())
#base = cwd + "/../derived/mac2"
base = cwd + "/../derived/ikko_dake"
```


```python
# numpyスタイルの数式を生成
dir_name = base + '/eqs/numpy_style'
os.makedirs(dir_name, exist_ok=True)

numpy_word = "import numpy\ndef f(q, q_dot, xi):\n    l1, l2, l3 = q[0,0], q[1,0], q[2,0]\n    l1_dot, l2_dot, l3_dot = q_dot[0,0], q_dot[1,0], q_dot[2,0]\n\n    return "


name = dir_name + "/Phi0" + ".py"
f = open(name, 'w')
f.write(numpy_word)
f.write(NumPyPrinter().doprint(P))
f.close()


name = dir_name + "/Theta0" + ".py"
f = open(name, 'w')
f.write(numpy_word)
f.write(NumPyPrinter().doprint(R))
f.close()

for i in range(3):
    for j in range(3):
        f = open(dir_name + "/M" + str(i) + "_" + str(j) + ".py", 'w')
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(M[i, j]))
        f.close()

for i in range(3):
    for j in range(3):
        f = open(dir_name + "/C" + str(i) + "_" + str(j) + ".py", 'w')
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(C[i, j]))
        f.close()

for i in range(3):
    f = open(dir_name + "/G" + str(i) + ".py", 'w')
    f.write(numpy_word)
    f.write(NumPyPrinter().doprint(G[i, 0]))
    f.close()

f = open(dir_name + "/K" + ".py", 'w')
f.write(numpy_word)
f.write(NumPyPrinter().doprint(K))
f.close()

f = open(dir_name + "/D" + ".py", 'w')
f.write(numpy_word)
f.write(NumPyPrinter().doprint(D))
f.close()

```


```python
# じゅりあのコード生成
dir_name = base + '/eqs/julia_style'
os.makedirs(dir_name, exist_ok=True)

julia_word = "function f(q::Vector{T}, q_dot::Vector{T}, xi::T) where T\n    l1 = q[1]\n    l2 = q[2]\n    l3 = q[3]\n    l1_dot = q_dot[1]\n    l2_dot = q_dot[2]\n    l3_dot = q_dot[3]\n    \n    "


name = dir_name + "/Phi0" + ".jl"
f = open(name, 'w')
f.write("module " + "Phi0" + "\n")
f.write(julia_word)
f.write(julia_code(P))
f.write("\nend\nend")
f.close()


name = dir_name + "/Theta0" + ".jl"
f = open(name, 'w')
f.write("module " + "Theta0" + "\n")
f.write(julia_word)
f.write(julia_code(R))
f.write("\nend\nend")
f.close()

for i in range(3):
    for j in range(3):
        f = open(dir_name + "/M" + str(i) + "_" + str(j) + ".jl", 'w')
        f.write("module " + "M" + str(i) + "_" + str(j) + "\n")
        f.write(julia_word)
        f.write(julia_code(M[i, j]))
        f.write("\nend\nend")
        f.close()

for i in range(3):
    for j in range(3):
        f = open(dir_name + "/C" + str(i) + "_" + str(j) + ".jl", 'w')
        f.write("module " + "C" + str(i) + "_" + str(j) + "\n")
        f.write(julia_word)
        f.write(julia_code(C[i, j]))
        f.write("\nend\nend")
        f.close()

for i in range(3):
    f = open(dir_name + "/G" + str(i) + ".jl", 'w')
    f.write("module " + "G" + str(i) + "\n")
    f.write(julia_word)
    f.write(julia_code(G[i, 0]))
    f.write("\nend\nend")
    f.close()

f = open(dir_name + "/K" + ".jl", 'w')
f.write("module " + "G" + "\n")
f.write(julia_word)
f.write(julia_code(K))
f.write("\nend\nend")
f.close()

f = open(dir_name + "/D" + ".jl", 'w')
f.write("module " + "G" + "\n")
f.write(julia_word)
f.write(julia_code(D))
f.write("\nend\nend")
f.close()

```


```python
# Cコード生成
dir_name = base + '/eqs/c_src/'
os.makedirs(dir_name, exist_ok=True)


def gen_c(f, name, dir_name):
    [(c_name, c_code), (h_name, c_header)] = codegen(
        name_expr=(name, f),
        language="C",
        project= name + "project",
        to_files=False
    )
    
    f = open(dir_name + c_name, 'w')
    f.write(c_code)
    f.close()

    f = open(dir_name + h_name, 'w')
    f.write(c_header)
    f.close()


dir_name = base + '/eqs/c_src/Phi0/'
os.makedirs(dir_name, exist_ok=True)
gen_c(P, "Phi0", dir_name)

dir_name = base + '/eqs/c_src/Theta0/'
os.makedirs(dir_name, exist_ok=True)
gen_c(R, "Theta0", dir_name)

dir_name = base + '/eqs/c_src/M/'
os.makedirs(dir_name, exist_ok=True)
gen_c(M, "M", dir_name)

dir_name = base + '/eqs/c_src/C/'
os.makedirs(dir_name, exist_ok=True)
gen_c(C, "C", dir_name)

dir_name = base + '/eqs/c_src/G/'
os.makedirs(dir_name, exist_ok=True)
gen_c(G, "G", dir_name)
```

# 状態方程式の導出


```python
detM = M[0,0]*M[1,1]*M[2,2] + M[0,1]*M[1,2]*M[2,0] + M[0,2]*M[1,0]*M[2,1] -\
    (M[0,2]*M[1,1]*M[2,0] + M[0,1]*M[1,0]*M[2,2] + M[0,0]*M[1,2]*M[2,1])

_M = sy.Matrix([
    [
        M[1,1]*M[2,2] - M[1,2]*M[2,1],
        -(M[0,1]*M[2,2] - M[0,2]*M[2,1]),
        M[0,1]*M[1,2] - M[0,2]*M[1,1]
    ],
    [
        -(M[1,0]*M[2,2] - M[1,2]*M[2,0]),
        M[0,0]*M[2,2] - M[0,2]*M[2,0],
        -(M[0,0]*M[1,2] - M[0,2]*M[1,0])
    ],
    [
        M[1,0]*M[2,1] - M[1,1]*M[2,0],
        -(M[0,0]*M[2,1] - M[0,1]*M[2,0]),
        M[0,0]*M[1,1] - M[0,1]*M[1,0]
    ],
])

invM = _M / detM

# ヒステリシスベクトル
h1, h2, h3 = sy.symbols("h1, h2, h3")
H = sy.Matrix([[h1, h2, h3]]).T

# トルクベクトル
tau1, tau2, tau3 = sy.symbols("tau1, tau2, tau3")
tau = sy.Matrix([[tau1, tau2, tau3]]).T

q_dot_dot = invM*(-(C+D)*q_dot - K*q - G -H -tau)
alpha_h = 23.705
_fx = invM*(-(C+D)*q_dot - K*q - G + H)  # ドリフト項
fx = sy.Matrix([[
    l1_dot, l2_dot, l3_dot, _fx[0,0], _fx[1,0], _fx[2,0], alpha_h*l1, alpha_h*l2, alpha_h*l3
]]).T  # ドリフト項

X = sy.Matrix([[l1, l2, l3, l1_dot, l2_dot, l3_dot, h1, h2, h3]]).T
A = fx.jacobian(X)

```


```python
# 式整理
invM = sy.simplify(invM)
```


```python
dir_name = base + '/eqs/c_src/invM/'
os.makedirs(dir_name, exist_ok=True)

for i in range(3):
    for j in range(3):
        gen_c(invM[i,j], "invM_"+str(i)+"_"+str(j), dir_name)
```


```python
#q_dot_dot = sy.simplify(q_dot_dot)
```


```python
dir_name = base + '/eqs/c_src/q_dot_dot/'
os.makedirs(dir_name, exist_ok=True)

for i in range(3):
    gen_c(q_dot_dot[i, 0], "q_dot_dot_"+str(i), dir_name)
```


```python
#fx = sy.simplify(fx)
```


```python
dir_name = base + '/eqs/c_src/fx/'
os.makedirs(dir_name, exist_ok=True)

for i in range(9):
    gen_c(fx[i, 0], "fx_"+str(i), dir_name)
```


```python
#A = sy.simplify(A)
```


```python
dir_name = base + '/eqs/c_src/A/'
os.makedirs(dir_name, exist_ok=True)

for i in range(9):
    for j in range(9):
        gen_c(A[i, j], "A"+"_"+str(i)+"_"+str(j), dir_name)
```

ipynb
037a66d2af04ebf18dbce1dee1333d77f851be52
Jupyter Notebook
7841c981e6804cc92d34715a00e7c3efce41d1d0
MIT
YoshimitsuMatsutaIe/ctrlab2021_soudan
o/soft_robot/derivation_of_dynamics/section_ikko_dake.ipynb
7841c981e6804cc92d34715a00e7c3efce41d1d0
MIT
YoshimitsuMatsutaIe/ctrlab2021_soudan
o/soft_robot/derivation_of_dynamics/section_ikko_dake.ipynb
7841c981e6804cc92d34715a00e7c3efce41d1d0
MIT
YoshimitsuMatsutaIe/ctrlab2021_soudan
o/soft_robot/derivation_of_dynamics/section_ikko_dake.ipynb