## Zadaca 1


```python
import sympy as sp
from sympy.physics import mechanics
import Resources.script as rt
mechanics.init_vprinting() 
```


```python
m, g = sp.symbols('m, g')
y = mechanics.dynamicsymbols('y')
y
```


```python
vy = y.diff('t')
vy
```


```python
K = m * vy**2 / 2
K
```


```python
P = m * g * y
P
```


```python
L = K-P
L
```


```python
rt.lagrangian(L, [y])

```




$\displaystyle \left[\begin{matrix}m \left(g + \ddot{y}\right)\end{matrix}\right]$



## Zadaca 2


```python
m, g, l = sp.symbols('m, g, l')
theta = mechanics.dynamicsymbols('theta')
```


```python
x = l*sp.cos(theta)
x
```


```python
y = l*sp.sin(theta)
y
```


```python
K = m * (x.diff('t')**2 + y.diff('t')**2) / 2 
K = sp.simplify(K)
K
```


```python
P = m*g*l*sp.sin(theta)
P
```


```python
L = K-P
L
```


```python
rt.lagrangian(L, [theta])
```




$\displaystyle \left[\begin{matrix}l m \left(g \cos{\left(\theta \right)} + l \ddot{\theta}\right)\end{matrix}\right]$



## Zadaca 3


```python
m1, m2, g = sp.symbols('m1, m2, g')
d1, d2 = mechanics.dynamicsymbols('d1, d2')
v1, v2 = d1.diff('t'), d2.diff('t')
```


```python
K1 = (m1*v1**2)/2
K2 = (m2*(v1**2+v2**2))/2
K = K1 + K2
K
```


```python
P1 = m1*g*d1
P2 = m2*g*d1
P = P1 + P2
P.simplify()
```


```python
L = K - P
L
```


```python
rt.lagrangian(L, [d1, d2])
```




$\displaystyle \left[\begin{matrix}g m_{1} + g m_{2} + m_{1} \ddot{d}_{1} + m_{2} \ddot{d}_{1}\\m_{2} \ddot{d}_{2}\end{matrix}\right]$



## Zacada 11


```python
m1, m2, g, l1, l2, lc1, lc2, I1, I2 = sp.symbols('m1, m2, g, l1, l2, l_c1, l_c2, I1, I2')
theta1, theta2 = mechanics.dynamicsymbols('theta1, theta2')
vtheta1, vtheta2 = theta1.diff('t'), theta2.diff('t')
lc1, lc2
```


```python
k1 = m1* lc1**2 *vtheta1**2 /2 + I1 * vtheta1**2 /2
k1.simplify()
```


```python
x2 = l1*sp.cos(theta1) + lc2*sp.cos(theta1+theta2)
y2 = l1*sp.sin(theta1) + lc2*sp.sin(theta1+theta2)
vx2 = x2.diff('t')
vy2 = y2.diff('t')
vx2,vy2
```


```python
k2 = m2 * (vx2**2 + vy2**2) / 2 + I2* (vtheta1**2 + vtheta2**2)/2
k2
```


```python
k = k1+k2
k
```


```python
p1 = m1*g*lc1*sp.sin(theta1)
p2 = m2*g*l1*sp.sin(theta1) + m2*g*lc2*sp.sin(theta1+theta2)
p = p1+p2
p
```


```python
L = k-p
rt.dynamic_model_with_3_matrices(L, [theta1, theta2])
```




$\displaystyle \left( \left[\begin{matrix}\left(I_{1} + I_{2} + l_{1}^{2} m_{2} + 2 l_{1} l_{c2} m_{2} \cos{\left(\theta_{2} \right)} + l_{c1}^{2} m_{1} + l_{c2}^{2} m_{2}\right) \ddot{\theta}_{1} & l_{c2} m_{2} \left(l_{1} \cos{\left(\theta_{2} \right)} + l_{c2}\right) \ddot{\theta}_{2}\\l_{c2} m_{2} \left(l_{1} \cos{\left(\theta_{2} \right)} + l_{c2}\right) \ddot{\theta}_{1} & \left(I_{2} + l_{c2}^{2} m_{2}\right) \ddot{\theta}_{2}\end{matrix}\right], \  \left[\begin{matrix}0 & - 2 l_{1} l_{c2} m_{2} \sin{\left(\theta_{2} \right)} \dot{\theta}_{1} \dot{\theta}_{2} & - l_{1} l_{c2} m_{2} \sin{\left(\theta_{2} \right)} \dot{\theta}_{2}^{2}\\l_{1} l_{c2} m_{2} \sin{\left(\theta_{2} \right)} \dot{\theta}_{1}^{2} & 0 & 0\end{matrix}\right], \  \left[\begin{matrix}g \left(l_{1} m_{2} \cos{\left(\theta_{1} \right)} + l_{c1} m_{1} \cos{\left(\theta_{1} \right)} + l_{c2} m_{2} \cos{\left(\theta_{1} + \theta_{2} \right)}\right)\\g l_{c2} m_{2} \cos{\left(\theta_{1} + \theta_{2} \right)}\end{matrix}\right]\right)$



## Zacada 12


```python
m1, m2, g, l1, l2, r1, r2 = sp.symbols('m1, m2, g, l1, l2, r1, r2')
theta1, theta2 = mechanics.dynamicsymbols('theta1, theta2')
vtheta1, vtheta2 = theta1.diff('t'), theta2.diff('t')
```


```python
Ix1, Iy1, Iz1, Ix2, Iy2, Iz2 = sp.symbols('I_x1, I_y1, I_z1, I_x2, I_y2, I_z2')
T1 = sp.Matrix([[Ix1, 0, 0], [0, Iy1, 0], [0, 0, Iz1]])
T2 = sp.Matrix([[Ix2, 0, 0], [0, Iy2, 0], [0, 0, Iz2]])
T1, T2
```




$\displaystyle \left( \left[\begin{matrix}I_{x1} & 0 & 0\\0 & I_{y1} & 0\\0 & 0 & I_{z1}\end{matrix}\right], \  \left[\begin{matrix}I_{x2} & 0 & 0\\0 & I_{y2} & 0\\0 & 0 & I_{z2}\end{matrix}\right]\right)$




```python
x1 = r1*sp.cos(theta1)
y1 = r1*sp.sin(theta1)
v1 = sp.Matrix([x1.diff('t'), y1.diff('t'), 0])
v1
```




$\displaystyle \left[\begin{matrix}- r_{1} \sin{\left(\theta_{1} \right)} \dot{\theta}_{1}\\r_{1} \cos{\left(\theta_{1} \right)} \dot{\theta}_{1}\\0\end{matrix}\right]$




```python
w1 = sp.Matrix([0, 0, theta1.diff('t')])
w1
```




$\displaystyle \left[\begin{matrix}0\\0\\\dot{\theta}_{1}\end{matrix}\right]$




```python
K1 = m1 * v1.T * v1 / 2  +  w1.T * T1 * w1 / 2
K1
```




$\displaystyle \left[\begin{matrix}\frac{I_{z1} \dot{\theta}_{1}^{2}}{2} + \frac{m_{1} r_{1}^{2} \sin^{2}{\left(\theta_{1} \right)} \dot{\theta}_{1}^{2}}{2} + \frac{m_{1} r_{1}^{2} \cos^{2}{\left(\theta_{1} \right)} \dot{\theta}_{1}^{2}}{2}\end{matrix}\right]$




```python
x2 = l1*sp.cos(theta1) + r2*sp.cos(theta1+theta2)
y2 = l1*sp.sin(theta1) + r2*sp.sin(theta1+theta2)
v2 = sp.Matrix([x2.diff('t'), y2.diff('t'), 0])
v2
```




$\displaystyle \left[\begin{matrix}- l_{1} \sin{\left(\theta_{1} \right)} \dot{\theta}_{1} - r_{2} \left(\dot{\theta}_{1} + \dot{\theta}_{2}\right) \sin{\left(\theta_{1} + \theta_{2} \right)}\\l_{1} \cos{\left(\theta_{1} \right)} \dot{\theta}_{1} + r_{2} \left(\dot{\theta}_{1} + \dot{\theta}_{2}\right) \cos{\left(\theta_{1} + \theta_{2} \right)}\\0\end{matrix}\right]$




```python
w2 = sp.Matrix([0, 0, theta2.diff('t')])
w2
```




$\displaystyle \left[\begin{matrix}0\\0\\\dot{\theta}_{2}\end{matrix}\right]$




```python
K2 = m2 * v2.T * v2 / 2  +  w2.T * T2 * w2 / 2
K2
```




$\displaystyle \left[\begin{matrix}\frac{I_{z2} \dot{\theta}_{2}^{2}}{2} + \frac{m_{2} \left(- l_{1} \sin{\left(\theta_{1} \right)} \dot{\theta}_{1} - r_{2} \left(\dot{\theta}_{1} + \dot{\theta}_{2}\right) \sin{\left(\theta_{1} + \theta_{2} \right)}\right)^{2}}{2} + \frac{m_{2} \left(l_{1} \cos{\left(\theta_{1} \right)} \dot{\theta}_{1} + r_{2} \left(\dot{\theta}_{1} + \dot{\theta}_{2}\right) \cos{\left(\theta_{1} + \theta_{2} \right)}\right)^{2}}{2}\end{matrix}\right]$




```python
P = m1 * g * r1 * sp.sin(theta1) + m2 * g * l1 * sp.sin(theta1) + m2 * g * r2 * sp.sin(theta1 + theta2)
K = K1+K2
K
```




$\displaystyle \left[\begin{matrix}\frac{I_{z1} \dot{\theta}_{1}^{2}}{2} + \frac{I_{z2} \dot{\theta}_{2}^{2}}{2} + \frac{m_{1} r_{1}^{2} \sin^{2}{\left(\theta_{1} \right)} \dot{\theta}_{1}^{2}}{2} + \frac{m_{1} r_{1}^{2} \cos^{2}{\left(\theta_{1} \right)} \dot{\theta}_{1}^{2}}{2} + \frac{m_{2} \left(- l_{1} \sin{\left(\theta_{1} \right)} \dot{\theta}_{1} - r_{2} \left(\dot{\theta}_{1} + \dot{\theta}_{2}\right) \sin{\left(\theta_{1} + \theta_{2} \right)}\right)^{2}}{2} + \frac{m_{2} \left(l_{1} \cos{\left(\theta_{1} \right)} \dot{\theta}_{1} + r_{2} \left(\dot{\theta}_{1} + \dot{\theta}_{2}\right) \cos{\left(\theta_{1} + \theta_{2} \right)}\right)^{2}}{2}\end{matrix}\right]$




```python
L = K[0] - P
L
```


```python
rt.lagrangian(L, [theta1, theta2])
```




$\displaystyle \left[\begin{matrix}I_{z1} \ddot{\theta}_{1} + g l_{1} m_{2} \cos{\left(\theta_{1} \right)} + g m_{1} r_{1} \cos{\left(\theta_{1} \right)} + g m_{2} r_{2} \cos{\left(\theta_{1} + \theta_{2} \right)} + l_{1}^{2} m_{2} \ddot{\theta}_{1} - 2 l_{1} m_{2} r_{2} \sin{\left(\theta_{2} \right)} \dot{\theta}_{1} \dot{\theta}_{2} - l_{1} m_{2} r_{2} \sin{\left(\theta_{2} \right)} \dot{\theta}_{2}^{2} + 2 l_{1} m_{2} r_{2} \cos{\left(\theta_{2} \right)} \ddot{\theta}_{1} + l_{1} m_{2} r_{2} \cos{\left(\theta_{2} \right)} \ddot{\theta}_{2} + m_{1} r_{1}^{2} \ddot{\theta}_{1} + m_{2} r_{2}^{2} \ddot{\theta}_{1} + m_{2} r_{2}^{2} \ddot{\theta}_{2}\\I_{z2} \ddot{\theta}_{2} + g m_{2} r_{2} \cos{\left(\theta_{1} + \theta_{2} \right)} + l_{1} m_{2} r_{2} \sin{\left(\theta_{2} \right)} \dot{\theta}_{1}^{2} + l_{1} m_{2} r_{2} \cos{\left(\theta_{2} \right)} \ddot{\theta}_{1} + m_{2} r_{2}^{2} \ddot{\theta}_{1} + m_{2} r_{2}^{2} \ddot{\theta}_{2}\end{matrix}\right]$



## Zadaca 16


```python
m1, m2, g = sp.symbols('m1, m2, g')
l, r, theta = mechanics.dynamicsymbols('l, r, theta')
vl, vr, vtheta = l.diff('t'), r.diff('t'), theta.diff('t')
```


```python
k1 = m1 * vl**2 / 2
k1
```


```python
k2 = m2 * (vl**2 + vr**2) + m2*r**2*vtheta**2/2
k = k1+k2
k
```


```python
p = m1*g*l+m2*g*l
p.simplify()
```


```python
L = k-p
L
```


```python
rt.lagrangian(L, [l, r, theta])
```




$\displaystyle \left[\begin{matrix}g m_{1} + g m_{2} + m_{1} \ddot{l} + 2 m_{2} \ddot{l}\\m_{2} \left(- r \dot{\theta}^{2} + 2 \ddot{r}\right)\\m_{2} \left(r \ddot{\theta} + 2 \dot{r} \dot{\theta}\right) r\end{matrix}\right]$



## Zadaca 26


```python
H = sp.Matrix([
    [0.7071, 0.7071, 0, 2],
    [0, 0, 1, 1],
    [0.7071, -0.7071, 0, 5],
    [0, 0, 0, 1],
])
H
```




$\displaystyle \left[\begin{matrix}0.7071 & 0.7071 & 0 & 2\\0 & 0 & 1 & 1\\0.7071 & -0.7071 & 0 & 5\\0 & 0 & 0 & 1\end{matrix}\right]$




```python
R = H[:3, :3]
P = H[:3, 3]
R
```




$\displaystyle \left[\begin{matrix}0.7071 & 0.7071 & 0\\0 & 0 & 1\\0.7071 & -0.7071 & 0\end{matrix}\right]$




```python
F = sp.Matrix([10, 0, 5, 15, 20, 0])
F_wrt_U = F[:3,:]
M_wrt_U = F[3:,:]
```


```python
R.T * F_wrt_U,  R.T * (F_wrt_U.cross(P) + M_wrt_U)
```




$\displaystyle \left( \left[\begin{matrix}10.6065\\3.5355\\0\end{matrix}\right], \  \left[\begin{matrix}14.142\\0\\-20\end{matrix}\right]\right)$




```python
m1, m2, g = sp.symbols('m1, m2, g')
l, r, theta = mechanics.dynamicsymbols('l, r, theta')
robot = rt.SerialLinkRobot()
robot.add_revolute_joint(theta, 0, 0,0)
robot.add_prismatic_joint(0, l, 0, sp.pi/2)
robot.add_prismatic_joint(0, r, 0, 0)
robot.interact()
```


    VBox(children=(HBox(children=(ToggleButton(value=True, description='Исклучи цртање'), Button(description='Пром…



```python
x2, y2, z2 = robot.get_dh_matrix()[:3,3]
v2_squared = x2.diff('t')**2 + y2.diff('t')**2 + z2.diff('t')**2
K2 = m2 * v2_squared / 2
K2 = sp.simplify(K2)
K2
```


```python
x2, y2, z2
```


```python
l1, l2, l3 = sp.symbols('l1, l2, l3')
theta1, d2, theta3 = mechanics.dynamicsymbols('theta1, d2, theta3')
robot = rt.SerialLinkRobot()
robot.add_revolute_joint(theta1, 0, l1,-sp.pi/2)
robot.add_prismatic_joint(sp.pi/2, l2, 0, sp.pi/2)
robot.add_prismatic_joint(sp.pi/2, d2, 0, -sp.pi/2)
robot.add_revolute_joint(theta3, 0, l3, 0)
robot.add_subs([(l1,1),(l2,0.5),(l3,1)])
robot.interact()
```


    VBox(children=(HBox(children=(ToggleButton(value=True, description='Исклучи цртање'), Button(description='Пром…



```python
Jv = robot.linear_jacobian()
Jv
```




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




```python
F = sp.Matrix([20, 10, 5])
F
```




$\displaystyle \left[\begin{matrix}20\\10\\5\end{matrix}\right]$




```python
T = Jv.T * F
T
```




$\displaystyle \left[\begin{matrix}- 20 l_{1} \sin{\left(\theta_{1} \right)} + 10 l_{1} \cos{\left(\theta_{1} \right)} - 10 l_{2} \sin{\left(\theta_{1} \right)} - 20 l_{2} \cos{\left(\theta_{1} \right)} - 10 l_{3} \sin{\left(\theta_{1} + \theta_{3} \right)} - 20 l_{3} \cos{\left(\theta_{1} + \theta_{3} \right)} - 20 d_{2} \sin{\left(\theta_{1} \right)} + 10 d_{2} \cos{\left(\theta_{1} \right)}\\- 20 \sin{\left(\theta_{1} \right)} + 10 \cos{\left(\theta_{1} \right)}\\10 \sin{\left(\theta_{1} \right)} + 20 \cos{\left(\theta_{1} \right)}\\10 l_{3} \sin{\left(\theta_{1} + \theta_{3} \right)} - 20 l_{3} \cos{\left(\theta_{1} + \theta_{3} \right)}\end{matrix}\right]$




```python
T.subs(theta1, sp.pi/6).subs(d2, 2).subs(theta3, sp.pi/4).subs(l1, 1).subs(l2, 0.5).subs(l3, 1).evalf()
```




$\displaystyle \left[\begin{matrix}-30.0151310892523\\-1.33974596215561\\22.3205080756888\\4.48287736084027\end{matrix}\right]$




```python

```

be071e9c92335ea6ffcd4b89d0fc3d71b138a0c3
ipynb
Jupyter Notebook
Auditoriski/Tema4.ipynb
vasetrendafilov/Robotics1
27bfe8afcc6652c1cb95ea09b6b7eb3fe997bebb
MIT
Auditoriski/Tema4.ipynb
vasetrendafilov/Robotics1
27bfe8afcc6652c1cb95ea09b6b7eb3fe997bebb
MIT
Auditoriski/Tema4.ipynb
vasetrendafilov/Robotics1
27bfe8afcc6652c1cb95ea09b6b7eb3fe997bebb
MIT
Qwen/Qwen-72B
1. YES
2. YES