```python
%load_ext autoreload
%autoreload 2

import utils
from sympy import sqrt
import sympy as sy
from sympy.physics.mechanics import dynamicsymbols, init_vprinting
init_vprinting()
```

    The autoreload extension is already loaded. To reload it, use:
      %reload_ext autoreload



```python
dh_params = [
    [0, 0, "d_1", 0],
    [0, 0, 0, "theta_2"],
    [0, "l_2", 0, "theta_3"],
]

# Center of mass of each link
m_center_points_0 = [
    sy.Matrix([0, 0, ]),
    sy.Matrix([0, 0, sy.Symbol("l_2")])
]

# Link mass
link_m = [sy.Symbol("m_1"), sy.Symbol("m_2"), sy.Symbol("m_3")]

# Link inertia tensor
link_I = [
    sy.diag(sy.Symbol("I_{xx1}"), sy.Symbol("I_{yy1}"), sy.Symbol("I_{zz1}")),
    sy.diag(sy.Symbol("I_{xx2}"), sy.Symbol("I_{yy2}"), sy.Symbol("I_{zz2}")),
    sy.diag(sy.Symbol("I_{xx3}"), sy.Symbol("I_{yy3}"), sy.Symbol("I_{zz3}"))
]
```


```python
utils.build_transf(dh_params)
```


$\displaystyle {}^0_1T = \left[\begin{matrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\\0 & 0 & 1 & d_{1}\\0 & 0 & 0 & 1\end{matrix}\right]$



$\displaystyle {}^1_2T = \left[\begin{matrix}\cos{\left(\theta_{2} \right)} & - \sin{\left(\theta_{2} \right)} & 0 & 0\\\sin{\left(\theta_{2} \right)} & \cos{\left(\theta_{2} \right)} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$



$\displaystyle {}^2_3T = \left[\begin{matrix}\cos{\left(\theta_{3} \right)} & - \sin{\left(\theta_{3} \right)} & 0 & l_{2}\\\sin{\left(\theta_{3} \right)} & \cos{\left(\theta_{3} \right)} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$





    [Matrix([
     [1, 0, 0,   0],
     [0, 1, 0,   0],
     [0, 0, 1, d_1],
     [0, 0, 0,   1]]),
     Matrix([
     [cos(theta_2), -sin(theta_2), 0, 0],
     [sin(theta_2),  cos(theta_2), 0, 0],
     [           0,             0, 1, 0],
     [           0,             0, 0, 1]]),
     Matrix([
     [cos(theta_3), -sin(theta_3), 0, l_2],
     [sin(theta_3),  cos(theta_3), 0,   0],
     [           0,             0, 1,   0],
     [           0,             0, 0,   1]])]




```python

```


```python
t = dynamicsymbols("theta_1")
t.diff()
```


```python
[[dynamicsymbols(p) for p in params if isinstance(p, str)] for params in dh_params]
```


```python
utils.lagrange(dh_params, m_center_points_0, link_m, link_I, verbose=True, simple=True)
```


```python

```

3849651e997a6e9411179b572f7c0080ac41cff1
ipynb
Jupyter Notebook
examples/test_ex4p2.ipynb
philippwulff/robotics_calc
8365ed3931206ca3788086e261d800ebe21ef86b
MIT
examples/test_ex4p2.ipynb
philippwulff/robotics_calc
8365ed3931206ca3788086e261d800ebe21ef86b
MIT
examples/test_ex4p2.ipynb
philippwulff/robotics_calc
8365ed3931206ca3788086e261d800ebe21ef86b
MIT
Qwen/Qwen-72B
1. YES
2. YES