baxterのmap求める


```python
import sympy as sy
from sympy import sin, cos, pi, sqrt
import math
#from math import pi
q = sy.Matrix(sy.MatrixSymbol('q', 7, 1))
L, h, H, L0, L1, L2, L3, L4, L5, L6, R = sy.symbols('L, h, H, L0, L1, L2, L3, L4, L5, L6, R')
# L = 278e-3
# h = 64e-3
# H = 1104e-3
# L0 = 270.35e-3
# L1 = 69e-3
# L2 = 364.35e-3
# L3 = 69e-3
# L4 = 374.29e-3
# L5 = 10e-3
# L6 = 368.3e-3
def HTM(alpha, a, d, theta):
    return sy.Matrix([
        [cos(theta), -sin(theta), 0, a],
        [sin(theta)*cos(alpha), cos(theta)*cos(alpha), -sin(alpha), -d*sin(alpha)],
        [sin(theta)*sin(alpha), cos(theta)*sin(alpha), cos(alpha), d*cos(alpha)],
        [0, 0, 0, 1],
    ])



DHparams = (
    (0, 0, 0, q[0, 0]),
    (-pi/2, L1, 0, q[1, 0]+pi/2),
    (pi/2, 0, L2, q[2, 0]),
    (-pi/2, L3, 0, q[3, 0]),
    (pi/2, 0, L4, q[4, 0]),
    (-pi/2, L5, 0, q[5, 0]),
    (pi/2, 0, 0, q[6, 0]),
)


T_RL_W0 = sy.Matrix([
    [-sqrt(2)/2, sqrt(2)/2, 0, -L,],
    [-sqrt(2)/2, -sqrt(2)/2, 0, -h,],
    [0, 0, 1, H,],
    [0, 0, 0, 1,],
])

T_0_RL = sy.Matrix([
    [1, 0, 0, 0,],
    [0, 1, 0, 0,],
    [0, 0, 1, L0,],
    [0, 0, 0, 1,],
])

Ts = [HTM(*dhparam) for dhparam in DHparams]

T_GR_7 = sy.Matrix([
    [1, 0, 0, 0,],
    [0, 1, 0, 0,],
    [0, 0, 1, L6,],
    [0, 0, 0, 1,],
])


### 変換前一覧 ###
T_all = [T_RL_W0, T_0_RL]
T_all += Ts
T_all.append(T_GR_7)


### 変換後 ###
for i, T in enumerate(T_all):
    if i == 0:
        T_abs = [T]
    else:
        T_abs.append(T_abs[i-1] @ T)


os = [T[0:3, 3:4] for T in T_abs]
Rxs = [T[0:3, 0:1] for T in T_abs]
Rys = [T[0:3, 1:2] for T in T_abs]
Rzs = [T[0:3, 2:3] for T in T_abs]

Jos = [o.jacobian(q) for o in os]
JRxs = [r.jacobian(q) for r in Rxs]
JRys = [r.jacobian(q) for r in Rys]
JRzs = [r.jacobian(q) for r in Rzs]



t = sy.Symbol("t")
q1 = sy.Function("q1")
q2 = sy.Function("q2")
q3 = sy.Function("q3")
q4 = sy.Function("q4")
q5 = sy.Function("q5")
q6 = sy.Function("q6")
q7 = sy.Function("q7")

dq = sy.Matrix(sy.MatrixSymbol('dq', 7, 1))

T_abs_ = []
for T in T_abs:
    T_ = T.subs([
        (q[0,0], q1(t)),
        (q[1,0], q2(t)),
        (q[2,0], q3(t)),
        (q[3,0], q4(t)),
        (q[4,0], q5(t)),
        (q[5,0], q6(t)),
        (q[6,0], q7(t)),
    ])
    T_abs_.append(T_)


os_ = [T[0:3, 3:4] for T in T_abs_]
Rxs_ = [T[0:3, 0:1] for T in T_abs_]
Rys_ = [T[0:3, 1:2] for T in T_abs_]
Rzs_ = [T[0:3, 2:3] for T in T_abs_]

q_ = sy.Matrix([
    [q1(t)],
    [q2(t)],
    [q3(t)],
    [q4(t)],
    [q5(t)],
    [q6(t)],
    [q7(t)],
])
Jos_ = [o.jacobian(q_) for o in os_]
JRxs_ = [r.jacobian(q_) for r in Rxs_]
JRys_ = [r.jacobian(q_) for r in Rys_]
JRzs_ = [r.jacobian(q_) for r in Rzs_]

Jos_dot_ = [sy.diff(J, t) for J in Jos_]
JRxs_dot_ = [sy.diff(J, t) for J in JRxs_]
JRys_dot_ = [sy.diff(J, t) for J in JRys_]
JRzs_dot_ = [sy.diff(J, t) for J in JRzs_]


Jos_dot = []
JRxs_dot = []
JRys_dot = []
JRzs_dot = []
for Js, newJs in zip((Jos_dot_, JRxs_dot_, JRys_dot_, JRzs_dot_), (Jos_dot, JRxs_dot, JRys_dot, JRzs_dot)):
    for J in Js:
        newJs.append(J.subs([
        (sy.Derivative(q1(t),t), dq[0, 0]),
        (sy.Derivative(q2(t),t), dq[1, 0]),
        (sy.Derivative(q3(t),t), dq[2, 0]),
        (sy.Derivative(q4(t),t), dq[3, 0]),
        (sy.Derivative(q5(t),t), dq[4, 0]),
        (sy.Derivative(q6(t),t), dq[5, 0]),
        (sy.Derivative(q7(t),t), dq[6, 0]),
        (q1(t), q[0, 0]),
        (q2(t), q[1, 0]),
        (q3(t), q[2, 0]),
        (q4(t), q[3, 0]),
        (q5(t), q[4, 0]),
        (q6(t), q[5, 0]),
        (q7(t), q[6, 0]),
    ]))

os = [sy.expand(e) for e in os]
Rxs = [sy.expand(e) for e in Rxs]
Rys = [sy.expand(e) for e in Rys]
Rzs = [sy.expand(e) for e in Rzs]
Jos = [sy.expand(e) for e in Jos]
JRxs = [sy.expand(e) for e in JRxs]
JRys = [sy.expand(e) for e in JRys]
JRzs = [sy.expand(e) for e in JRzs]
Jos_dot = [sy.expand(e) for e in Jos_dot]
JRxs_dot = [sy.expand(e) for e in JRxs_dot]
JRys_dot = [sy.expand(e) for e in JRys_dot]
JRzs_dot = [sy.expand(e) for e in JRzs_dot]

expr_all = [os, Rxs, Rys, Rzs, Jos, JRxs, JRys, JRzs, Jos_dot, JRxs_dot, JRys_dot, JRzs_dot]
names = ["W0", "BR"] + [str(i) for i in range(7)] + ["ee"]
expr_name = [
    ["o_" + n for n in names],
    ["rx_" + n for n in names],
    ["ry_" + n for n in names],
    ["rz_" + n for n in names],
    ["jo_" + n for n in names],
    ["jrx_" + n for n in names],
    ["jry_" + n for n in names],
    ["jrz_" + n for n in names],
    ["jo_" + n + "_dot" for n in names],
    ["jrx_" + n + "_dot" for n in names],
    ["jry_" + n + "_dot" for n in names],
    ["jrz_" + n + "_dot" for n in names],
]
```


```python
from sympy.printing import cxxcode
from sympy.utilities.codegen import codegen
import os as OS


original = "cpp_"
done = "cpp"

OS.makedirs(original, exist_ok=True)
OS.makedirs(done, exist_ok=True)


def gen_cpp_code(expr, name):
    code_txt = cxxcode(expr, assign_to="out", standard="c++17")
    with open(name+".cpp", "w") as f:
        f.write(code_txt)

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

    f = open(dir+h_name, 'w')
    f.write(c_header)
    f.close()
    
    return c_code, c_header

names = ["W0", "BR"] + [str(i) for i in range(7)] + ["ee"]

with open(original+"/htm.cpp", "w") as fc, open(original+"/htm.hpp", "w") as fh:
    for i, o in enumerate(os):
        c, h = gen_c(o, name="o_"+names[i])
        fc.write(c)
        fh.write(h)
    for i, o in enumerate(Rxs):
        c, h = gen_c(o, name="rx_"+names[i])
        fc.write(c)
        fh.write(h)
    for i, o in enumerate(Rys):
        c, h = gen_c(o, name="ry_"+names[i])
        fc.write(c)
        fh.write(h)
    for i, o in enumerate(Rzs):
        c, h = gen_c(o, name="rz_"+names[i])
        fc.write(c)
        fh.write(h)

with open(original+"/Jos.cpp", "w") as fc, open(original+"/Jos.hpp", "w") as fh:
    for i, o in enumerate(Jos):
        c, h = gen_c(o, name="jo_"+names[i])
        fc.write(c)
        fh.write(h)
with open(original+"/JRxs.cpp", "w") as fc, open(original+"/JRxs.hpp", "w") as fh:
    for i, o in enumerate(JRxs):
        c, h = gen_c(o, name="jrx_"+names[i])
        fc.write(c)
        fh.write(h)
with open(original+"/JRys.cpp", "w") as fc, open(original+"/JRys.hpp", "w") as fh:
    for i, o in enumerate(JRzs):
        c, h = gen_c(o, name="jry_"+names[i])
        fc.write(c)
        fh.write(h)
with open(original+"/JRzs.cpp", "w") as fc, open(original+"/JRzs.hpp", "w") as fh:
    for i, o in enumerate(JRzs):
        c, h = gen_c(o, name="jrz_"+names[i])
        fc.write(c)
        fh.write(h)

with open(original+"/Jo_dots.cpp", "w") as fc, open(original+"/Jo_dots.hpp", "w") as fh:
    for i, o in enumerate(Jos_dot):
        c, h = gen_c(o, name="jo_"+names[i]+"_dot")
        fc.write(c)
        fh.write(h)
with open(original+"/JRx_dots.cpp", "w") as fc, open(original+"/JRx_dots.hpp", "w") as fh:
    for i, o in enumerate(JRxs_dot):
        c, h = gen_c(o, name="jrx_"+names[i]+"_dot")
        fc.write(c)
        fh.write(h)
with open(original+"/JRy_dots.cpp", "w") as fc, open(original+"/JRy_dots.hpp", "w") as fh:
    for i, o in enumerate(JRzs_dot):
        c, h = gen_c(o, name="jry_"+names[i]+"_dot")
        fc.write(c)
        fh.write(h)
with open(original+"/JRz_dots.cpp", "w") as fc, open(original+"/JRz_dots.hpp", "w") as fh:
    for i, o in enumerate(JRzs_dot):
        c, h = gen_c(o, name="jrz_"+names[i]+"_dot")
        fc.write(c)
        fh.write(h)


```


```python
### これが本物 ###
from sympy.printing import cxxcode
from sympy.utilities.codegen import codegen
import os as OS


original = "cpp_original"
done = "cpp_done"

OS.makedirs(original, exist_ok=True)
OS.makedirs(original+"/include", exist_ok=True)
OS.makedirs(original+"/src", exist_ok=True)


def gen_cpp_code(expr, name, dir):
    [(c_name, c_code), (h_name, c_header)] = codegen(
        name_expr=(name, expr),
        language="C",
        project= name + "_BY_SYMPY_",
        to_files=False
    )
    
    f = open(dir+"/src/"+name+".cpp", 'w')
    f.write(c_code)
    f.close()

    f = open(dir+"/include/"+h_name.replace(".h", "")+".hpp", 'w')
    f.write(c_header)
    f.close()

for exprs, names in zip(expr_all, expr_name):
    for expr, name in zip(exprs, names):
        gen_cpp_code(expr, name, original)
```


```python
com = "#ifndef BAXTER_HPP\n" \
    + "#define BAXTER_HPP\n" \
    + "#include<eigen3/Eigen/Core>\n" \
    + "namespace baxter\n" \
    + "{\n" \
    + "    using Eigen::VectorXd;\n" \
    + "    using Eigen::MatrixXd;\n" \
    + "    static const double L = 278e-3;\n" \
    + "    static const double h = 64e-3;\n" \
    + "    static const double H = 1104e-3;\n" \
    + "    static const double L0 = 270.35e-3;\n" \
    + "    static const double L1 = 69e-3;\n" \
    + "    static const double L2 = 364.35e-3;\n" \
    + "    static const double L3 = 69e-3;\n" \
    + "    static const double L4 = 374.29e-3;\n" \
    + "    static const double L5 = 10e-3;\n" \
    + "    static const double L6 = 368.3e-3;\n"

for ns in expr_name[0:4]:
    for n in ns:
        com += ("    void " + n + "(const VectorXd& q, VectorXd& out);\n")
for ns in expr_name[4:8]:
    for n in ns:
        com += ("    void " + n + "(const VectorXd& q, MatrixXd& out);\n")
for ns in expr_name[8:12]:
    for n in ns:
        com += ("    void " + n + "(const VectorXd& q, const VectorXd& q_dot, MatrixXd& out);\n")

com += "};\n#endif"
```


```python
### 変換 ###
import re
done = "cpp_done"
OS.makedirs(done, exist_ok=True)
OS.makedirs(done+"/include", exist_ok=True)
OS.makedirs(done+"/src", exist_ok=True)

pat = r'out_(.+?)\['
pat2 = r'out_(.+?)\)'
pat3 = r'\((.+?)\) {'
pat4 = r'#(.+?).h\"'

sout = ["out[" + str(i) + "]" for i in range(21)]
sout_2 = ["out(0,0)","out(0,1)","out(0,2)","out(0,3)","out(0,4)","out(0,5)","out(0,6)","out(1,0)","out(1,1)","out(1,2)","out(1,3)","out(1,4)","out(1,5)","out(1,6)","out(2,0)","out(2,1)","out(2,2)","out(2,3)","out(2,4)","out(2,5)","out(2,6)"]

with open("cpp_done/include/baxter.hpp", "w") as f:
    f.write(com)



def common_trans(line):
    r = re.findall(pat, line)
    r2 = re.findall(pat2, line)
    if len(r) != 0:
        line = line.replace("out_" + r[0], "out")
    if len(r2) != 0:
        line = line.replace("out_" + r2[0], "out")
    line = line.replace("q[0]", "q(0)")
    line = line.replace("q[1]", "q(1)")
    line = line.replace("q[2]", "q(2)")
    line = line.replace("q[3]", "q(3)")
    line = line.replace("q[4]", "q(4)")
    line = line.replace("q[5]", "q(5)")
    line = line.replace("q[6]", "q(6)")
    
    # line = line.replace("double L, ", "")
    # line = line.replace("double h, ", "")
    # line = line.replace("double H, ", "")
    # line = line.replace("double L0, ", "")
    # line = line.replace("double L1, ", "")
    # line = line.replace("double L2, ", "")
    # line = line.replace("double L3, ", "")
    # line = line.replace("double L4, ", "")
    # line = line.replace("double L5, ", "")
    # line = line.replace("double L6, ", "")
    
    
    r3 = re.findall(pat3, line)
    if "j" not in name:
        if len(r3) != 0:
            print("("+r3[0]+")")
            #line = line.replace("("+r3[0]+") {", "(const VectorXd& q, VectorXd& out) {")
            line = line.replace("("+r3[0]+") {", "(const VectorXd& q, double L, double h, double H, double L0, double L1, double L2, double L3, double L4, double L5, double L6, VectorXd& out) {")
        line = line.replace("double *out", "VectorXd& out")
        line = line.replace("out[0]", "out(0)")
        line = line.replace("out[1]", "out(1)")
        line = line.replace("out[2]", "out(2)")

    
    else:
        if "dot" in name:
            if len(r3) != 0:
                line = line.replace(r3[0], "const VectorXd& q, const VectorXd& dq, double L, double h, double H, double L0, double L1, double L2, double L3, double L4, double L5, double L6, MatrixXd& out")
        else:
            if len(r3) != 0:
                print(name)
                line = line.replace(r3[0], "const VectorXd& q, double L, double h, double H, double L0, double L1, double L2, double L3, double L4, double L5, double L6, MatrixXd& out")
        line = line.replace("double *out", "MatrixXd& out")
        for s, t in zip(sout, sout_2):
            line = line.replace(s, t)
        


    return line


def trans_cpp(name):
    origin = "cpp_original/src/" + name + ".cpp"
    done = "cpp_done/src/" + name + ".cpp"
    with open(origin, "r") as f, open(done, "w") as g:
        file_data = f.readlines()
        for line in file_data:
            line = line.replace('#include <math.h>', '#include <cmath>\nusing std::cos;\nusing std::sin;\nusing std::sqrt;\n')
            #line = line.replace("#include \"", "#include \"../../include/baxter/")
            #line = line.replace(".h\"", ".hpp\"\n#include \"../../include/baxter/common.hpp\"\n")
            r4 = re.findall(pat4, line)
            if len(r4) != 0:
                line = line.replace("#"+r4[0]+".h\"", "#include \"../include/baxter.hpp\"\n")
            line = line.replace("void ", "void baxter::")
            line = line.replace("double *q", "const VectorXd& q").replace("double *dq", "const VectorXd& dq")
            
            line = common_trans(line)


            g.write(line)


# def trans_hpp(name):
#     origin = "cpp_original/include/" + name + ".hpp"
#     done = "cpp_done/include/" + name + ".hpp"
#     with open(origin, "r") as f, open(done, "w") as g:
#         file_data = f.readlines()
#         for line in file_data:
#             line = line.replace("void ", "#include<eigen3/Eigen/Core>\nnamespace baxter\n{\nusing Eigen::VectorXd;\nusing Eigen::MatrixXd;\nvoid ").replace(");", ");\n}\n")
#             line = line.replace("double *q", "const VectorXd& q").replace("double *dq", "const VectorXd& dq")
            
#             line = common_trans(line)

#             g.write(line)

for names in expr_name:
    for name in names:
        trans_cpp(name)
        #trans_hpp(name)
```

    (double H, double L, double h, double *out)
    (double H, double L, double L0, double h, double *out)
    (double H, double L, double L0, double h, double *out)
    (double H, double L, double L0, double L1, double h, const VectorXd& q, double *out)
    (double H, double L, double L0, double L1, double L2, double h, const VectorXd& q, double *out)
    (double H, double L, double L0, double L1, double L2, double L3, double h, const VectorXd& q, double *out)
    (double H, double L, double L0, double L1, double L2, double L3, double L4, double h, const VectorXd& q, double *out)
    (double H, double L, double L0, double L1, double L2, double L3, double L4, double L5, double h, const VectorXd& q, double *out)
    (double H, double L, double L0, double L1, double L2, double L3, double L4, double L5, double h, const VectorXd& q, double *out)
    (double H, double L, double L0, double L1, double L2, double L3, double L4, double L5, double L6, double h, const VectorXd& q, double *out)
    (double *out)
    (double *out)
    (const VectorXd& q, double *out)
    (const VectorXd& q, double *out)
    (const VectorXd& q, double *out)
    (const VectorXd& q, double *out)
    (const VectorXd& q, double *out)
    (const VectorXd& q, double *out)
    (const VectorXd& q, double *out)
    (const VectorXd& q, double *out)
    (double *out)
    (double *out)
    (const VectorXd& q, double *out)
    (const VectorXd& q, double *out)
    (const VectorXd& q, double *out)
    (const VectorXd& q, double *out)
    (const VectorXd& q, double *out)
    (const VectorXd& q, double *out)
    (const VectorXd& q, double *out)
    (const VectorXd& q, double *out)
    (double *out)
    (double *out)
    (double *out)
    (const VectorXd& q, double *out)
    (const VectorXd& q, double *out)
    (const VectorXd& q, double *out)
    (const VectorXd& q, double *out)
    (const VectorXd& q, double *out)
    (const VectorXd& q, double *out)
    (const VectorXd& q, double *out)
    jo_W0
    jo_BR
    jo_0
    jo_1
    jo_2
    jo_3
    jo_4
    jo_5
    jo_6
    jo_ee
    jrx_W0
    jrx_BR
    jrx_0
    jrx_1
    jrx_2
    jrx_3
    jrx_4
    jrx_5
    jrx_6
    jrx_ee
    jry_W0
    jry_BR
    jry_0
    jry_1
    jry_2
    jry_3
    jry_4
    jry_5
    jry_6
    jry_ee
    jrz_W0
    jrz_BR
    jrz_0
    jrz_1
    jrz_2
    jrz_3
    jrz_4
    jrz_5
    jrz_6
    jrz_ee



```python
hoho = "void baxter::o_W0(VectorXd& out) {"

```




    '\\((.+?) {'




```python
# pythonコード生成（クラス）
from sympy.printing.numpy import NumPyPrinter

names = ["W0", "BR"] + [str(i) for i in range(7)] + ["ee"]

common_w = "import numpy as np\nfrom math import cos as c\nfrom math import sin as s\nfrom math import tan as t\nfrom math import sqrt as sq\nfrom base import Base\n"



with open("src_py_/htm.py", "w") as f:
    f.write(common_w + "class HTM(Base):\n")
    for name, z in zip(names, os):
        numpy_word = "    def o_" + name + "(self, q):\n        return "
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(z))
        f.write("\n")

    for name, z in zip(names, Rxs):
        numpy_word = "    def rx_" + name + "(self, q):\n        return "
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(z))
        f.write("\n")

    for name, z in zip(names, Rys):
        numpy_word = "    def ry_" + name + "(self, q):\n        return "
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(z))
        f.write("\n")

    for name, z in zip(names, Rzs):
        numpy_word = "    def rz_" + name + "(self, q):\n        return "
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(z))
        f.write("\n")


with open("src_py_/Jos.py", "w") as f:
    f.write(common_w + "class Jo(Base):\n")
    for name, z in zip(names, Jos):
        numpy_word = "    def jo_" + name + "(self, q):\n        return "
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(z))
        f.write("\n")

with open("src_py_/JRxs.py", "w") as f:
    f.write(common_w + "class JRx(Base):\n")
    for name, z in zip(names, JRxs):
        numpy_word = "    def jrx_" + name + "(self, q):\n        return "
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(z))
        f.write("\n")

with open("src_py_/JRys.py", "w") as f:
    f.write(common_w + "class JRy(Base):\n")
    for name, z in zip(names, JRys):
        numpy_word = "    def jry_" + name + "(self, q):\n        return "
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(z))
        f.write("\n")

with open("src_py_/JRzs.py", "w") as f:
    f.write(common_w + "class JRz(Base):\n")
    for name, z in zip(names, JRzs):
        numpy_word = "    def jrz_" + name + "(self, q):\n        return "
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(z))
        f.write("\n")


with open("src_py_/Jo_dots.py", "w") as f:
    f.write(common_w + "class Jo_dot(Base):\n")
    for name, z in zip(names, Jos_dot):
        numpy_word = "    def jo_" + name + "_dot(self, q, dq):\n        return "
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(z))
        f.write("\n")

with open("src_py_/JRx_dots.py", "w") as f:
    f.write(common_w + "class JRx_dot(Base):\n")
    for name, z in zip(names, JRxs_dot):
        numpy_word = "    def jrx_" + name + "_dot(self, q, dq):\n        return "
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(z))
        f.write("\n")

with open("src_py_/JRy_dots.py", "w") as f:
    f.write(common_w + "class JRy_dot(Base):\n")
    for name, z in zip(names, JRys):
        numpy_word = "    def jry_" + name + "_dot(self, q, dq):\n        return "
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(z))
        f.write("\n")

with open("src_py_/JRz_dots.py", "w") as f:
    f.write(common_w + "class JRz_dot(Base):\n")
    for name, z in zip(names, JRzs):
        numpy_word = "    def jrz_" + name + "_dot(self, q, dq):\n        return "
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(z))
        f.write("\n")


def translate_hoge(original, done):
    with open(original, "r") as f, open(done, "w") as g:
        file_data = f.readlines()
        for line in file_data:
            line = line.replace('numpy', 'np').replace('1/2', '0.5').replace('(0.5)', '0.5')
            line = line.replace('np.cos', 'c').replace('np.sin', 's').replace('np.sqrt', 'sq')
            #line = line.replace('L', 'self.L').replace('h', 'self.h').replace('H', 'self.H')
            line = line.replace('import np as np', 'import numpy as np')
            line = line.replace('matself.h', 'math')
            g.write(line)


translate_hoge("src_py_/htm.py", "src_py/htm.py")
translate_hoge("src_py_/Jos.py", "src_py/Jos.py")
translate_hoge("src_py_/JRxs.py", "src_py/JRxs.py")
translate_hoge("src_py_/JRys.py", "src_py/JRys.py")
translate_hoge("src_py_/JRzs.py", "src_py/JRzs.py")
translate_hoge("src_py_/Jo_dots.py", "src_py/Jo_dots.py")
translate_hoge("src_py_/JRx_dots.py", "src_py/JRx_dots.py")
translate_hoge("src_py_/JRy_dots.py", "src_py/JRy_dots.py")
translate_hoge("src_py_/JRz_dots.py", "src_py/JRz_dots.py")
```


```python
from sympy.printing.numpy import NumPyPrinter

names = ["W0", "BR"] + [str(i) for i in range(7)] + ["ee"]

common_w = "import numpy as np\nfrom math import cos as c\nfrom math import sin as s\nfrom math import tan as ta\nfrom math import sqrt as sq\n"

numba_word_q = "@njit(\"f8[:, :](f8[:, :])\")\n"
numba_word_q_dq = "@njit(\"f8[:, :](f8[:, :], f8[:, :])\")\n"

with open("src_py_/htm.py", "w") as f:
    f.write(common_w)
    for name, z in zip(names, os):
        numpy_word = "def o_" + name + "(q):\n    return "
        #f.write(numba_word_q)
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(z))
        f.write("\n")

    for name, z in zip(names, Rxs):
        numpy_word = "def rx_" + name + "(q):\n    return "
        #f.write(numba_word_q)
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(z))
        f.write("\n")

    for name, z in zip(names, Rys):
        numpy_word = "def ry_" + name + "(q):\n    return "
        #f.write(numba_word_q)
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(z))
        f.write("\n")

    for name, z in zip(names, Rzs):
        numpy_word = "def rz_" + name + "(q):\n    return "
        #f.write(numba_word_q)
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(z))
        f.write("\n")


with open("src_py_/Jos.py", "w") as f:
    f.write(common_w)
    for name, z in zip(names, Jos):
        numpy_word = "def jo_" + name + "(q):\n    return "
        #f.write(numba_word_q)
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(z))
        f.write("\n")

with open("src_py_/JRxs.py", "w") as f:
    f.write(common_w)
    for name, z in zip(names, JRxs):
        numpy_word = "def jrx_" + name + "(q):\n    return "
        #f.write(numba_word_q)
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(z))
        f.write("\n")

with open("src_py_/JRys.py", "w") as f:
    f.write(common_w)
    for name, z in zip(names, JRys):
        numpy_word = "def jry_" + name + "(q):\n    return "
        #f.write(numba_word_q)
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(z))
        f.write("\n")

with open("src_py_/JRzs.py", "w") as f:
    f.write(common_w)
    for name, z in zip(names, JRzs):
        numpy_word = "def jrz_" + name + "(q):\n    return "
        #f.write(numba_word_q)
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(z))
        f.write("\n")


with open("src_py_/Jo_dots.py", "w") as f:
    f.write(common_w)
    for name, z in zip(names, Jos_dot):
        numpy_word = "def jo_" + name + "_dot(q, dq):\n    return "
        #f.write(numba_word_q_dq)
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(sy.simplify(z)))
        f.write("\n")

with open("src_py_/JRx_dots.py", "w") as f:
    f.write(common_w)
    for name, z in zip(names, JRxs_dot):
        numpy_word = "def jrx_" + name + "_dot(q, dq):\n    return "
        #f.write(numba_word_q_dq)
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(sy.simplify(z)))
        f.write("\n")

with open("src_py_/JRy_dots.py", "w") as f:
    f.write(common_w)
    for name, z in zip(names, JRys):
        numpy_word = "def jry_" + name + "_dot(q, dq):\n    return "
        #f.write(numba_word_q_dq)
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(sy.simplify(z)))
        f.write("\n")

with open("src_py_/JRz_dots.py", "w") as f:
    f.write(common_w)
    for name, z in zip(names, JRzs):
        numpy_word = "def jrz_" + name + "_dot(q, dq):\n    return "
        #f.write(numba_word_q_dq)
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(sy.simplify(z)))
        f.write("\n")


def translate_hoge(original, done):
    with open(original, "r") as f, open(done, "w") as g:
        file_data = f.readlines()
        for line in file_data:
            line = line.replace('numpy', 'np').replace('1/2', '0.5').replace('(0.5)', '0.5')
            line = line.replace('np.cos', 'c').replace('np.sin', 's').replace('np.sqrt', 'sq')
            # line = line.replace(']])', ']], dtype=np.float64)')
            # line = line.replace('[0, 0, 0, 0, 0, 0, 0]', '[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]')
            # line = line.replace('[0]', '[0.0]').replace(' 0]],', ' 0.0]],').replace('[1]', '[1.0]').replace('[[0,', '[[0.0,').replace('0.0, 0],', '0.0, 0.0],')
            line = line.replace('import np as np', 'import numpy as np')
            g.write(line)


translate_hoge("src_py_/htm.py", "src_py_no_class/htm.py")
translate_hoge("src_py_/Jos.py", "src_py_no_class/Jos.py")
translate_hoge("src_py_/JRxs.py", "src_py_no_class/JRxs.py")
translate_hoge("src_py_/JRys.py", "src_py_no_class/JRys.py")
translate_hoge("src_py_/JRzs.py", "src_py_no_class/JRzs.py")
translate_hoge("src_py_/Jo_dots.py", "src_py_no_class/Jo_dots.py")
translate_hoge("src_py_/JRx_dots.py", "src_py_no_class/JRx_dots.py")
translate_hoge("src_py_/JRy_dots.py", "src_py_no_class/JRy_dots.py")
translate_hoge("src_py_/JRz_dots.py", "src_py_no_class/JRz_dots.py")
```


