```python
# configure the notebook: display + common packages
from IPython.display import display, Math
import sympy
import sympy.physics.mechanics
import numpy as np
import scipy.linalg

# make sure latex formulae are displayed with mathjax
sympy.init_printing(use_latex='mathjax')

# function to print a formula, use it as disp("something", "else") to generate
# a latex formula like "something = else"
def disp(left, right):
    display(Math( sympy.latex(left) + "=" + sympy.latex(right) ))
```


```python
# state variables
p = sympy.Symbol("p")
pd = sympy.Symbol("pd")
th = sympy.Symbol("th")
thd = sympy.Symbol("thd")
# control input
u = sympy.Symbol("u")

# constants
g = sympy.Symbol('g')
ma = sympy.Symbol('ma')
Ia = sympy.Symbol('Ia')
mua = sympy.Symbol('mua')
fva = sympy.Symbol('fva')
tva = sympy.Symbol('tva')

# write down the model
cth = mua*sympy.cos(th)
sth = mua*sympy.sin(th)
d = ma*Ia - cth**2
f1 = thd**2*sth - fva*pd
f2 = -g*sth - tva*thd
num_p = Ia*(u+f1) - cth*f2
num_th = -cth*(u+f1) + ma*f2
pdd = num_p / d
thdd = num_th / d
```


```python
# Evaluate the jacobians of pdd
dpdd_dp = sympy.diff(pdd, p)
dpdd_dpd = sympy.diff(pdd, pd)
dpdd_dth = sympy.diff(pdd, th)
dpdd_dthd = sympy.diff(pdd, thd)
# Evaluate the jacobians of thdd
dthdd_dp = sympy.diff(thdd, p)
dthdd_dpd = sympy.diff(thdd, pd)
dthdd_dth = sympy.diff(thdd, th)
dthdd_dthd = sympy.diff(thdd, thd)
# Evaluate the jacobians with respect to the control
dpdd_du = sympy.diff(pdd, u)
dthdd_du = sympy.diff(thdd, u)

# copy-pastable python function
print(f"""
def sympy_jacobs(p, th, pd, thd, u, constants):
    from math import sin,cos
    g,ma,Ia,mua,fva,tva = constants
    
    dpdd_dp = {dpdd_dp}
    dpdd_dpd = {dpdd_dpd}
    dpdd_dth = {dpdd_dth}
    dpdd_dthd = {dpdd_dthd}

    dthdd_dp = {dthdd_dp}
    dthdd_dpd = {dthdd_dpd}
    dthdd_dth = {dthdd_dth}
    dthdd_dthd = {dthdd_dthd}
    
    Jx = np.array([
        [ dpdd_dp,  dpdd_dpd,  dpdd_dth,  dpdd_dthd],
        [dthdd_dp, dthdd_dpd, dthdd_dth, dthdd_dthd]
    ])
    Ju = np.array([
        [{dpdd_du}],
        [{dthdd_du}]
    ])
    return Jx,Ju
    
""")
```

    
    def sympy_jacobs(p, th, pd, thd, u, constants):
        from math import sin,cos
        g,ma,Ia,mua,fva,tva = constants
        
        dpdd_dp = 0
        dpdd_dpd = -Ia*fva/(Ia*ma - mua**2*cos(th)**2)
        dpdd_dth = -2*mua**2*(Ia*(-fva*pd + mua*thd**2*sin(th) + u) - mua*(-g*mua*sin(th) - thd*tva)*cos(th))*sin(th)*cos(th)/(Ia*ma - mua**2*cos(th)**2)**2 + (Ia*mua*thd**2*cos(th) + g*mua**2*cos(th)**2 + mua*(-g*mua*sin(th) - thd*tva)*sin(th))/(Ia*ma - mua**2*cos(th)**2)
        dpdd_dthd = (2*Ia*mua*thd*sin(th) + mua*tva*cos(th))/(Ia*ma - mua**2*cos(th)**2)
    
        dthdd_dp = 0
        dthdd_dpd = fva*mua*cos(th)/(Ia*ma - mua**2*cos(th)**2)
        dthdd_dth = -2*mua**2*(ma*(-g*mua*sin(th) - thd*tva) - mua*(-fva*pd + mua*thd**2*sin(th) + u)*cos(th))*sin(th)*cos(th)/(Ia*ma - mua**2*cos(th)**2)**2 + (-g*ma*mua*cos(th) - mua**2*thd**2*cos(th)**2 + mua*(-fva*pd + mua*thd**2*sin(th) + u)*sin(th))/(Ia*ma - mua**2*cos(th)**2)
        dthdd_dthd = (-ma*tva - 2*mua**2*thd*sin(th)*cos(th))/(Ia*ma - mua**2*cos(th)**2)
        
        Jx = np.array([
            [ dpdd_dp,  dpdd_dpd,  dpdd_dth,  dpdd_dthd],
            [dthdd_dp, dthdd_dpd, dthdd_dth, dthdd_dthd]
        ])
        Ju = np.array([
            [Ia/(Ia*ma - mua**2*cos(th)**2)],
            [-mua*cos(th)/(Ia*ma - mua**2*cos(th)**2)]
        ])
        return Jx,Ju
        
    



```python
def sympy_jacobs(p, th, pd, thd, u, constants):
    from math import sin,cos
    g,ma,Ia,mua,fva,tva = constants
    
    dpdd_dp = 0
    dpdd_dpd = -Ia*fva/(Ia*ma - mua**2*cos(th)**2)
    dpdd_dth = -2*mua**2*(Ia*(-fva*pd + mua*thd**2*sin(th) + u) - mua*(-g*mua*sin(th) - thd*tva)*cos(th))*sin(th)*cos(th)/(Ia*ma - mua**2*cos(th)**2)**2 + (Ia*mua*thd**2*cos(th) + g*mua**2*cos(th)**2 + mua*(-g*mua*sin(th) - thd*tva)*sin(th))/(Ia*ma - mua**2*cos(th)**2)
    dpdd_dthd = (2*Ia*mua*thd*sin(th) + mua*tva*cos(th))/(Ia*ma - mua**2*cos(th)**2)

    dthdd_dp = 0
    dthdd_dpd = fva*mua*cos(th)/(Ia*ma - mua**2*cos(th)**2)
    dthdd_dth = -2*mua**2*(ma*(-g*mua*sin(th) - thd*tva) - mua*(-fva*pd + mua*thd**2*sin(th) + u)*cos(th))*sin(th)*cos(th)/(Ia*ma - mua**2*cos(th)**2)**2 + (-g*ma*mua*cos(th) - mua**2*thd**2*cos(th)**2 + mua*(-fva*pd + mua*thd**2*sin(th) + u)*sin(th))/(Ia*ma - mua**2*cos(th)**2)
    dthdd_dthd = (-ma*tva - 2*mua**2*thd*sin(th)*cos(th))/(Ia*ma - mua**2*cos(th)**2)
    
    Jx = np.array([
        [ dpdd_dp,  dpdd_dpd,  dpdd_dth,  dpdd_dthd],
        [dthdd_dp, dthdd_dpd, dthdd_dth, dthdd_dthd]
    ])
    Ju = np.array([
        [Ia/(Ia*ma - mua**2*cos(th)**2)],
        [-mua*cos(th)/(Ia*ma - mua**2*cos(th)**2)]
    ])
    return Jx,Ju


def my_jacobs(p, th, pd, thd, u, constants):
    from math import sin,cos
    g,ma,Ia,mua,fva,tva = constants
    
    # aux variables
    cth = mua*cos(th)
    sth = mua*sin(th)
    cth2 = cth**2
    thd2 = thd**2
    d = ma*Ia - cth2
    f1 = thd2*sth - fva*pd
    f2 = -g*sth - tva*thd
    uf1 = u + f1
    num_p = Ia*uf1 - cth*f2
    num_th = -cth*uf1 + ma*f2
    minus2_sthcth = -2*sth*cth
    dinv = 1/d
    ddinv_dth = minus2_sthcth * dinv**2
    
    dpdd_dp = 0
    dpdd_dpd = -Ia*fva*dinv
    dpdd_dth = ddinv_dth*num_p + (Ia*thd2*cth + sth*f2 + g*cth2) * dinv
    dpdd_dthd = (2*Ia*thd*sth + tva*cth) * dinv

    dthdd_dp = 0
    dthdd_dpd = fva*cth*dinv
    dthdd_dth = ddinv_dth*num_th + (sth*uf1 - thd2*cth2 - ma*g*cth) * dinv
    dthdd_dthd = (minus2_sthcth*thd - ma*tva) * dinv
    
    Jx = np.array([
        [ dpdd_dp,  dpdd_dpd,  dpdd_dth,  dpdd_dthd],
        [dthdd_dp, dthdd_dpd, dthdd_dth, dthdd_dthd]
    ])
    Ju = np.array([
        [Ia*dinv],
        [-cth*dinv]
    ])
    return Jx,Ju
```


```python
# test the functions above
import random
random.seed(1993)

g = 9.806
ma = 16.36107771481117
Ia = 0.29504025711512755
mua = 0.6546880349055427
fva = 403.0267679493358
tva = 0.02537703984258163
constants = (g,ma,Ia,mua,fva,tva)

for _ in range(10):
    vars = tuple(random.uniform(-0.5, 0.5) for _ in range(5)) # p, th, pd, thd, u
    sJx,sJu = sympy_jacobs(*vars, constants)
    mJx,mJu = my_jacobs(*vars, constants)
    print("\nJx:")
#     print(np.round(mJx,3))
#     print(np.round(sJx,3))
    print(np.round(sJx-mJx,6))
    print("\nJu:")
#     print(np.round(mJu,3))
#     print(np.round(sJu,3))
    print(np.round(sJu-mJu,6))
    
```

    
    Jx:
    [[ 0. -0.  0.  0.]
     [ 0.  0. -0. -0.]]
    
    Ju:
    [[0.]
     [0.]]
    
    Jx:
    [[0. 0. 0. 0.]
     [0. 0. 0. 0.]]
    
    Ju:
    [[0.]
     [0.]]
    
    Jx:
    [[ 0. -0.  0.  0.]
     [ 0.  0.  0.  0.]]
    
    Ju:
    [[0.]
     [0.]]
    
    Jx:
    [[ 0.  0.  0.  0.]
     [ 0.  0. -0. -0.]]
    
    Ju:
    [[0.]
     [0.]]
    
    Jx:
    [[ 0.  0.  0.  0.]
     [ 0. -0. -0.  0.]]
    
    Ju:
    [[0.]
     [0.]]
    
    Jx:
    [[ 0.  0. -0.  0.]
     [ 0.  0. -0.  0.]]
    
    Ju:
    [[0.]
     [0.]]
    
    Jx:
    [[ 0.  0.  0.  0.]
     [ 0. -0. -0.  0.]]
    
    Ju:
    [[0.]
     [0.]]
    
    Jx:
    [[0. 0. 0. 0.]
     [0. 0. 0. 0.]]
    
    Ju:
    [[0.]
     [0.]]
    
    Jx:
    [[ 0.  0. -0.  0.]
     [ 0. -0.  0.  0.]]
    
    Ju:
    [[-0.]
     [ 0.]]
    
    Jx:
    [[ 0.  0. -0. -0.]
     [ 0. -0.  0.  0.]]
    
    Ju:
    [[0.]
     [0.]]


88ffceb83d15090b9be20bc7b330f7ecaaa7a250
ipynb
Jupyter Notebook
src/bin/identification/model-and-derivatives.ipynb
francofusco/pendule_pi
cd8f613bc61d1bfb4f4d7057734601232c5203cc
BSD-3-Clause
src/bin/identification/model-and-derivatives.ipynb
francofusco/pendule_pi
cd8f613bc61d1bfb4f4d7057734601232c5203cc
BSD-3-Clause
2021-06-02T09:30:54.000Z
2021-09-10T09:54:33.000Z
src/bin/identification/model-and-derivatives.ipynb
francofusco/pendule_pi
cd8f613bc61d1bfb4f4d7057734601232c5203cc
BSD-3-Clause
Qwen/Qwen-72B
1. YES
2. YES