Welcome to JWN's note

Hello, World!

그래, 너 하고 싶은 거 다 해... 자세히보기

STUDY

sympy

JWN 2024. 7. 5. 23:32
import sympy as sp

def get_equations(self):
    """
    :return: Functions to calculate A, B and f given state x and input u
    """
    f = sp.zeros(3, 1)    # Matrix([[0], [0], [0]])

    x = sp.Matrix(sp.symbols('x y theta', real=True))   # Matrix([[x], [y], [theta]])
    u = sp.Matrix(sp.symbols('v w', real=True))         # Matrix([[v], [w]])

    f[0, 0] = u[0, 0] * sp.cos(x[2, 0])
    f[1, 0] = u[0, 0] * sp.sin(x[2, 0])
    f[2, 0] = u[1, 0]

    f = sp.simplify(f)
    A = sp.simplify(f.jacobian(x))
    B = sp.simplify(f.jacobian(u))

    f_func = sp.lambdify((x, u), f, 'numpy')
    A_func = sp.lambdify((x, u), A, 'numpy')
    B_func = sp.lambdify((x, u), B, 'numpy')

    return f_func, A_func, B_func

jacobian

\[ f.jacobian(x) = \nabla_x f(x, u) = \begin{bmatrix} \partial_{x} f_1 (x, u) & \partial_{y} f_1 (x, u) & \partial_{\theta} f_1 (x, u) \\ \partial_{x} f_2 (x, u) & \partial_{y} f_2 (x, u) & \partial_{\theta} f_2 (x, u) \\ \partial_{x} f_3 (x, u) & \partial_{y} f_3 (x, u) & \partial_{\theta} f_3 (x, u) \end{bmatrix} \; with \; x=\begin{bmatrix} x\\ y \\ \theta \end{bmatrix} \]

'STUDY' 카테고리의 다른 글

numpy / matplotlib  (0) 2024.07.05