```python
import sympy as sp
import Resources.script as rt

```

## Zadaca 1


```python
p = rt.hpoint3(8,2,5)
p*2
```




$\displaystyle \left[\begin{matrix}16\\4\\10\\2\end{matrix}\right]$




```python
rt.unit_vector(p,"homogeneous")
```




$\displaystyle \left[\begin{matrix}0.82956135578434\\0.207390338946085\\0.518475847365213\\0\end{matrix}\right]$



## Zadaca 4


```python
p = rt.hpoint3(3,5,7)
```


```python
rt.htranslation3(2, 3, 4)*p
```




$\displaystyle \left[\begin{matrix}5\\8\\11\\1\end{matrix}\right]$



## Zadaca 5


```python
b = sp.Matrix([[0, 1, 0, 2],
               [1, 0, 0, 4],
               [0, 0, -1, 6],
               [0, 0, 0, 1]])
```




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




```python
rt.htranslation3(5, 2, 6)*b
```




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



## Zadaca 6


```python
f = sp.Matrix([[0, 1, 0, 2],
               [1, 0, 0, 4],
               [0, 0, -1, 6],
               [0, 0, 0, 1]])
f
```




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




```python
rt.htranslation3(5,2,6).inv() * f
```




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



## Zadaca 7


```python
f = sp.Matrix([[0, 1, 0, 0],
               [1, 0, 0, 4],
               [0, 0, -1, 2],
               [0, 0, 0, 1]])
f
```




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




```python
rt.htranslation3(2,4,6).inv()*f
```




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



## Zadaca 8


```python
x1, x2, x3 = sp.symbols('x1, x2, x3')
F = sp.Matrix([[0, x2, 0, -2],
               [1, 0, 0, 0],
               [x1, 0, x3, 4],
               [0, 0, 0, 1]])
F
```




$\displaystyle \left[\begin{matrix}0 & x_{2} & 0 & -2\\1 & 0 & 0 & 0\\x_{1} & 0 & x_{3} & 4\\0 & 0 & 0 & 1\end{matrix}\right]$




```python
n, o, a = F[:3, 0], F[:3, 1], F[:3, 2]
```


```python
ndot, odot, adot = n.dot(n), o.dot(o), a.dot(a)
```


```python
sp.solveset(ndot - 1, x1)
```




$\displaystyle \left\{0\right\}$




```python
sp.solveset(odot - 1, x2)
```




$\displaystyle \left\{-1, 1\right\}$




```python
sp.solveset(adot - 1, x3)
```




$\displaystyle \left\{-1, 1\right\}$




```python
possible_solutions = []
possible_solutions.append(F.subs([(x1, 0), (x2, -1), (x3, -1)]))
possible_solutions.append(F.subs([(x1, 0), (x2, -1), (x3, 1)]))
possible_solutions.append(F.subs([(x1, 0), (x2, 1), (x3, -1)]))
possible_solutions.append(F.subs([(x1, 0), (x2, 1), (x3, 1)]))
possible_solutions[0]
```




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




```python
for possible_solution in possible_solutions:
    if is_right_hand_coordinate_system3(possible_solution):
        sp.pprint(possible_solution)
        print()
```

    ⎡0  -1  0  -2⎤
    ⎢            ⎥
    ⎢1  0   0  0 ⎥
    ⎢            ⎥
    ⎢0  0   1  4 ⎥
    ⎢            ⎥
    ⎣0  0   0  1 ⎦
    
    ⎡0  1  0   -2⎤
    ⎢            ⎥
    ⎢1  0  0   0 ⎥
    ⎢            ⎥
    ⎢0  0  -1  4 ⎥
    ⎢            ⎥
    ⎣0  0  0   1 ⎦
    


## Zadaca 9


```python
x1, x2, x3 = sp.symbols('x1, x2, x3')
F = sp.Matrix([[0, x2, x3, 0],
               [x1, 0, -sp.sqrt(2)/2, -1],
               [sp.sqrt(2)/2, 0, sp.sqrt(2)/2, 6],
               [0, 0, 0, 1]])
F
```




$\displaystyle \left[\begin{matrix}0 & x_{2} & x_{3} & 0\\x_{1} & 0 & - \frac{\sqrt{2}}{2} & -1\\\frac{\sqrt{2}}{2} & 0 & \frac{\sqrt{2}}{2} & 6\\0 & 0 & 0 & 1\end{matrix}\right]$




```python
n, o, a = F[:3, 0], F[:3, 1], F[:3, 2]
ndot, odot, adot = n.dot(n), o.dot(o), a.dot(a)
```


```python
sp.solveset(ndot - 1, x1)
```




$\displaystyle \left\{- \frac{\sqrt{2}}{2}, \frac{\sqrt{2}}{2}\right\}$




```python
sp.solveset(odot - 1, x2)
```




$\displaystyle \left\{-1, 1\right\}$




```python
sp.solveset(adot - 1, x3)
```




$\displaystyle \left\{0\right\}$




```python
possible_solutions = []
possible_solutions.append(F.subs([(x1, sp.sqrt(2)/2), (x2, 1), (x3, 0)]))
possible_solutions.append(F.subs([(x1, sp.sqrt(2)/2), (x2, -1), (x3, 0)]))
possible_solutions.append(F.subs([(x1, -sp.sqrt(2)/2), (x2, 1), (x3, 0)]))
possible_solutions.append(F.subs([(x1, -sp.sqrt(2)/2), (x2, -1), (x3, 0)]))
possible_solutions[0]
```




$\displaystyle \left[\begin{matrix}0 & 1 & 0 & 0\\\frac{\sqrt{2}}{2} & 0 & - \frac{\sqrt{2}}{2} & -1\\\frac{\sqrt{2}}{2} & 0 & \frac{\sqrt{2}}{2} & 6\\0 & 0 & 0 & 1\end{matrix}\right]$




```python
for possible_solution in possible_solutions:
    if rt.is_right_hand_coordinate_system3(possible_solution):
        sp.pprint(possible_solution)
        print()
```

    ⎡0   -1   0    0 ⎤
    ⎢                ⎥
    ⎢√2      -√2     ⎥
    ⎢──  0   ────  -1⎥
    ⎢2        2      ⎥
    ⎢                ⎥
    ⎢√2       √2     ⎥
    ⎢──  0    ──   6 ⎥
    ⎢2        2      ⎥
    ⎢                ⎥
    ⎣0   0    0    1 ⎦
    


## Zadaca 10


```python
T1 = rt.hrotation3('x',sp.pi/2)
T2 = rt.hrotation3('z',sp.pi/2)
T1*T2
```




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




```python
rt.AnimationPlayground().play()
```


    VBox(children=(HBox(children=(ToggleButtons(options=('Ротирај', 'Придвижи се'), value='Ротирај'), BoundedFloat…


## Zadaca 14


```python
theta = sp.symbols('theta')
pox = rt.hrotation3('x', theta)
poy = rt.hrotation3('y', theta)
poz = rt.hrotation3('z', theta)
rt.is_right_hand_coordinate_system3(pox) and rt.is_right_hand_coordinate_system3(poz) and rt.is_right_hand_coordinate_system3(poy)
```




    True



## Zadaca 15


```python
theta = sp.symbols('theta')
pox = rt.hrotation3('x', theta)
poy = rt.hrotation3('y', theta)
poz = rt.hrotation3('z', theta)
sp.simplify(sp.det(pox) * sp.det(poy) * sp.det(poz))
```




$\displaystyle 1$



## Zadaca 17


```python
p = rt.hpoint3(2,3,4)
T = rt.hrotation3('x',sp.pi/4)
T*p
```




$\displaystyle \left[\begin{matrix}2\\- \frac{\sqrt{2}}{2}\\\frac{7 \sqrt{2}}{2}\\1\end{matrix}\right]$



## Zadaca 18


```python
p = rt.point3(2,3,4)
T = rt.rotation3('z',sp.pi/6)
T*p
```




$\displaystyle \left[\begin{matrix}- \frac{3}{2} + \sqrt{3}\\1 + \frac{3 \sqrt{3}}{2}\\4\end{matrix}\right]$



## Zadaca 19


```python
p = rt.point3(3, 5, 7)
rt.rotation3('x', 2*sp.pi/3) * p
```




$\displaystyle \left[\begin{matrix}3\\- \frac{7 \sqrt{3}}{2} - \frac{5}{2}\\- \frac{7}{2} + \frac{5 \sqrt{3}}{2}\end{matrix}\right]$



## Zadaca 20


```python
R = sp.Matrix([
    [1, -8, -4],
    [4, 4, 7],
    [8, -1, 4],
]) / 9
R
```




$\displaystyle \left[\begin{matrix}\frac{1}{9} & - \frac{8}{9} & - \frac{4}{9}\\\frac{4}{9} & \frac{4}{9} & \frac{7}{9}\\\frac{8}{9} & - \frac{1}{9} & \frac{4}{9}\end{matrix}\right]$




```python
theta = sp.acos((R.trace() - 1) / 2)
theta
```




$\displaystyle \frac{\pi}{2}$




```python
I =sp.Matrix([
    [R[2,1] - R[1,2]],
    [R[0,2] - R[2,0]],
    [R[1,0] - R[0,1]]
])/2*sp.sin(theta)
sp.pprint(I)
sp.pprint(-I)
```

    ⎡-4/9⎤
    ⎢    ⎥
    ⎢-2/3⎥
    ⎢    ⎥
    ⎣2/3 ⎦
    ⎡4/9 ⎤
    ⎢    ⎥
    ⎢2/3 ⎥
    ⎢    ⎥
    ⎣-2/3⎦


## Zadaca 21


```python
r = sp.Matrix([
    ['1/2', sp.sqrt(6)/4, -sp.sqrt(6)/4],
    [-sp.sqrt(6)/4,' 3/4', '1/4'],
    [sp.sqrt(6)/4, '1/4', '3/4'],
])
r
```




$\displaystyle \left[\begin{matrix}\frac{1}{2} & \frac{\sqrt{6}}{4} & - \frac{\sqrt{6}}{4}\\- \frac{\sqrt{6}}{4} & \frac{3}{4} & \frac{1}{4}\\\frac{\sqrt{6}}{4} & \frac{1}{4} & \frac{3}{4}\end{matrix}\right]$




```python
theta = sp.acos((r.trace() - 1) / 2)
theta
```




$\displaystyle \frac{\pi}{3}$



## Zadaca 22


```python
I =sp.Matrix([
    [r[2,1] - r[1,2]],
    [r[0,2] - r[2,0]],
    [r[1,0] - r[0,1]]
])/2*sp.sin(theta)
I
```




$\displaystyle \left[\begin{matrix}0\\- \frac{3 \sqrt{2}}{8}\\- \frac{3 \sqrt{2}}{8}\end{matrix}\right]$



## Zadaca 23


```python
playground = rt.AnimationPlayground()
playground.play(point=rt.hpoint3(3, 4, -5))
```


    VBox(children=(HBox(children=(ToggleButtons(options=('Ротирај', 'Придвижи се'), value='Ротирај'), BoundedFloat…



```python
t1 = rt.hrotation3('x',-sp.pi/6)
t2 = rt.htranslation3(y=3)
t3 = rt.hrotation3('a',sp.pi/4)
t4 = rt.htranslation3(6,9)
t = t2 * t1 * t3 * t4
t
```




$\displaystyle \left[\begin{matrix}\frac{\sqrt{2}}{2} & - \frac{\sqrt{2}}{2} & 0 & - \frac{3 \sqrt{2}}{2}\\\frac{\sqrt{6}}{4} & \frac{\sqrt{6}}{4} & \frac{1}{2} & 3 + \frac{15 \sqrt{6}}{4}\\- \frac{\sqrt{2}}{4} & - \frac{\sqrt{2}}{4} & \frac{\sqrt{3}}{2} & - \frac{15 \sqrt{2}}{4}\\0 & 0 & 0 & 1\end{matrix}\right]$




```python
p = rt.hpoint3(3,4,-5)
(t*p).evalf()
```




$\displaystyle \left[\begin{matrix}-2.82842712474619\\13.9721935853075\\-12.1083016119742\\1.0\end{matrix}\right]$



## Zadaca 24


```python
t1 = rt.hrotation3('x',sp.pi/2)
t2 = rt.htranslation3(5,3,6)
t3 = rt.hrotation3('z',sp.pi/2)
t = t3*t2*t1
```


```python
p = rt.hpoint3(5,3,4)
(t*p).evalf()
```




$\displaystyle \left[\begin{matrix}1.0\\10.0\\9.0\\1.0\end{matrix}\right]$



## Zadaca 25


```python
t1 = rt.hrotation3('x',sp.pi/2)
t2 = rt.hrotation3('a',sp.pi/2)
t3 = rt.htranslation3(5,3,6)
t = t3 * t1 * t2
t
```




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




```python
p = rt.hpoint3(2,3,5)
t*p
```




$\displaystyle \left[\begin{matrix}2\\-2\\8\\1\end{matrix}\right]$



## Zadaca 26


```python
t1 = rt.hrotation3('n',sp.pi/2)
t2 = rt.hrotation3('a',sp.pi/2)
t3 = rt.hrotation3('z',sp.pi/2)
t4 = rt.hrotation3('o',sp.pi/2)
t5 = rt.hrotation3('x',sp.pi/2)
t = t5 * t3 * t1 * t2 * t4
t
```




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




```python
p = rt.hpoint3(2,1,0)
t*p
```




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



## Zadaca 27


```python
t1 = rt.hrotation3('x',-sp.pi/6)
t2 = rt.htranslation3(y=3)
t3 = rt.hrotation3('a',sp.pi/4)
t4 = rt.htranslation3(6,9)
t = t2 * t1 * t3 * t4
t
```




$\displaystyle \left[\begin{matrix}\frac{\sqrt{2}}{2} & - \frac{\sqrt{2}}{2} & 0 & - \frac{3 \sqrt{2}}{2}\\\frac{\sqrt{6}}{4} & \frac{\sqrt{6}}{4} & \frac{1}{2} & 3 + \frac{15 \sqrt{6}}{4}\\- \frac{\sqrt{2}}{4} & - \frac{\sqrt{2}}{4} & \frac{\sqrt{3}}{2} & - \frac{15 \sqrt{2}}{4}\\0 & 0 & 0 & 1\end{matrix}\right]$




```python
p = rt.hpoint3(3,4,-5)
(t*p).evalf()
```




$\displaystyle \left[\begin{matrix}-2.82842712474619\\13.9721935853075\\-12.1083016119742\\1.0\end{matrix}\right]$



## Zadaca 31


```python
F13 = sp.Matrix([
    [0, 0, -1],
    [0, 1, 0],
    [1, 0, 0],
])
F12 = sp.Matrix([
    [1, 0, 0],
    [0, '1/2', -sp.sqrt(3)/2],
    [0, sp.sqrt(3)/2, '1/2'],
])

```




$\displaystyle \left[\begin{matrix}1 & 0 & 0\\0 & \frac{1}{2} & - \frac{\sqrt{3}}{2}\\0 & \frac{\sqrt{3}}{2} & \frac{1}{2}\end{matrix}\right]$




```python
F23 = F12.inv()*F13
F23
```




$\displaystyle \left[\begin{matrix}0 & 0 & -1\\\frac{\sqrt{3}}{2} & \frac{1}{2} & 0\\\frac{1}{2} & - \frac{\sqrt{3}}{2} & 0\end{matrix}\right]$



## Zadaca 32


```python
theta, phi = sp.symbols('theta, phi')
T1 = rt.hrotation3('y', theta)
T2 = rt.hrotation3('z', phi)
T1*T2
```




$\displaystyle \left[\begin{matrix}\cos{\left(\phi \right)} \cos{\left(\theta \right)} & - \sin{\left(\phi \right)} \cos{\left(\theta \right)} & \sin{\left(\theta \right)} & 0\\\sin{\left(\phi \right)} & \cos{\left(\phi \right)} & 0 & 0\\- \sin{\left(\theta \right)} \cos{\left(\phi \right)} & \sin{\left(\phi \right)} \sin{\left(\theta \right)} & \cos{\left(\theta \right)} & 0\\0 & 0 & 0 & 1\end{matrix}\right]$



## Zadaca 33


```python
gamma, beta, r = sp.symbols('gamma, beta, r')
sphere = rt.hrotation3('a', gamma) * rt.hrotation3('o', beta) * rt.htranslation3(z=r)
sphere
```




$\displaystyle \left[\begin{matrix}\cos{\left(\beta \right)} \cos{\left(\gamma \right)} & - \sin{\left(\gamma \right)} & \sin{\left(\beta \right)} \cos{\left(\gamma \right)} & r \sin{\left(\beta \right)} \cos{\left(\gamma \right)}\\\sin{\left(\gamma \right)} \cos{\left(\beta \right)} & \cos{\left(\gamma \right)} & \sin{\left(\beta \right)} \sin{\left(\gamma \right)} & r \sin{\left(\beta \right)} \sin{\left(\gamma \right)}\\- \sin{\left(\beta \right)} & 0 & \cos{\left(\beta \right)} & r \cos{\left(\beta \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$




```python
unrotated = sphere * rt.hrotation3('o', -beta) * rt.hrotation3('a', -gamma)
sp.simplify(unrotated)
```




$\displaystyle \left[\begin{matrix}1 & 0 & 0 & r \sin{\left(\beta \right)} \cos{\left(\gamma \right)}\\0 & 1 & 0 & r \sin{\left(\beta \right)} \sin{\left(\gamma \right)}\\0 & 0 & 1 & r \cos{\left(\beta \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$



## Zadaca 34


```python
x,y,z = 3.1375, 2.195, 3.214
```


```python
gama = sp.atan(x/y)
gama
```




$\displaystyle 0.960337799284207$




```python
r = sp.sqrt(x**2 + y**2 + z**2)
r
```




$\displaystyle 4.99917265655028$




```python
beta = sp.sqrt((x**2 + y**2) / z**2)
beta
```




$\displaystyle 1.19137838625981$



## Zadaca 35


```python
x,y,z = 1.5, 1.5, sp.sqrt(6)/2
```


```python
r, beta, gamma = sp.sqrt(x**2 + y**2 + z**2),sp.atan(sp.sqrt((x**2 + y**2) / z**2)), sp.atan(x/y)
```


```python
S = rt.hrotation3('a', gamma) * rt.hrotation3('o', beta) * rt.htranslation3(z=r)
S.evalf()
```




$\displaystyle \left[\begin{matrix}0.353553390593274 & -0.707106781186547 & 0.612372435695795 & 1.5\\0.353553390593274 & 0.707106781186548 & 0.612372435695794 & 1.5\\-0.866025403784439 & 0 & 0.5 & 1.22474487139159\\0 & 0 & 0 & 1.0\end{matrix}\right]$



## Zadaca 36


```python
theta_1, theta_2, theta_3 = sp.symbols('theta_1, theta_2, theta_3')
T1 = rt.hrotation3('a', theta_1)
T2 = rt.hrotation3('o', theta_2)
T3 = rt.hrotation3('a', theta_3)
T1 * T2 * T3
```




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



## Zadaca 37


```python
T1 = rt.hrotation3('z', -sp.pi/2)
T2 = rt.hrotation3('y', sp.pi)
T3 = rt.htranslation3(x=3)
T = T3 * T2 * T1
T
```




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




```python
euler_angles = rt.euler_angles(T, 'zyz')
euler_angles
```




$\displaystyle \left[\begin{matrix}\frac{\pi}{2}\\\pi\\0\end{matrix}\right]$




```python
rt.rotation_matrix_from_euler_angles(euler_angles,'zyz')
```




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



## Zadaca 38


```python
t1 = rt.htranslation3(x=5)
t2 = rt.hrotation3('o', sp.pi/4)
t3 = rt.hrotation3('z', sp.pi/2)
t4 = rt.htranslation3(z=sp.sqrt(2))
t5 = rt.hrotation3('x', sp.pi/2)
T = t5 * t3 * t1 * t2 * t4
T
```




$\displaystyle \left[\begin{matrix}0 & -1 & 0 & 0\\\frac{\sqrt{2}}{2} & 0 & - \frac{\sqrt{2}}{2} & -1\\\frac{\sqrt{2}}{2} & 0 & \frac{\sqrt{2}}{2} & 6\\0 & 0 & 0 & 1\end{matrix}\right]$




```python
euler_angles = rt.euler_angles(T, 'zyz')
euler_angles
```




$\displaystyle \left[\begin{matrix}- \frac{\pi}{2}\\\frac{\pi}{4}\\\pi\end{matrix}\right]$



## Zadaca 39-41


```python
r11, r12, r13, r21, r22, r23, r31, r32, r33 = sp.symbols('r11, r12, r13, r21, r22, r23, r31, r32, r33')
T = sp.Matrix([[r11, r12, r13], [r21, r22, r23], [r31, r32, r33]])
T
```




$\displaystyle \left[\begin{matrix}r_{11} & r_{12} & r_{13}\\r_{21} & r_{22} & r_{23}\\r_{31} & r_{32} & r_{33}\end{matrix}\right]$




```python
sp.pprint(rt.euler_angles(T,'xyz'))
print()
sp.pprint(rt.euler_angles(T,'xzy'))
print('\n RPY se ojlerovi ZYX \n')
sp.pprint(rt.euler_angles(T,'zyx'))
```

    ⎡atan2(-r₂₃, r₃₃)⎤
    ⎢                ⎥
    ⎢   asin(r₁₃)    ⎥
    ⎢                ⎥
    ⎣atan2(-r₁₂, r₁₁)⎦
    
    ⎡atan2(r₃₂, r₂₂)⎤
    ⎢               ⎥
    ⎢  -asin(r₁₂)   ⎥
    ⎢               ⎥
    ⎣atan2(r₁₃, r₁₁)⎦
    
     RPY se ojlerovi ZYX 
    
    ⎡atan2(r₂₁, r₁₁)⎤
    ⎢               ⎥
    ⎢  -asin(r₃₁)   ⎥
    ⎢               ⎥
    ⎣atan2(r₃₂, r₃₃)⎦


## Zadaca 42


```python
t1 = rt.htranslation3(x=5)
t2 = rt.hrotation3('o', sp.rad(60))
t3 = rt.hrotation3('z', sp.rad(30))
t4 = rt.htranslation3(z=3)
t5 = rt.hrotation3('x', sp.rad(45))
T = t5 * t3 * t1 * t2 * t4
T
```




$\displaystyle \left[\begin{matrix}\frac{\sqrt{3}}{4} & - \frac{1}{2} & \frac{3}{4} & \frac{9}{4} + \frac{5 \sqrt{3}}{2}\\\frac{\sqrt{2}}{8} + \frac{\sqrt{6}}{4} & \frac{\sqrt{6}}{4} & - \frac{\sqrt{2}}{4} + \frac{\sqrt{6}}{8} & \frac{\sqrt{2}}{2} + \frac{3 \sqrt{6}}{8}\\- \frac{\sqrt{6}}{4} + \frac{\sqrt{2}}{8} & \frac{\sqrt{6}}{4} & \frac{\sqrt{6}}{8} + \frac{\sqrt{2}}{4} & \frac{3 \sqrt{6}}{8} + 2 \sqrt{2}\\0 & 0 & 0 & 1\end{matrix}\right]$




```python
sp.deg(rt.euler_angles(T,'zyx')).evalf()
```




$\displaystyle \left[\begin{matrix}61.246056618025\\25.8232078223648\\42.867573148363\end{matrix}\right]$



## Zadaca 43


```python
T = sp.Matrix([
    [0.527, -0.574, 0.628, 4],
    [0.369, 0.189, 0.439, 6],
    [-0.766, 0, 0.643, 9],
    [0, 0, 0, 1],
])
T
```




$\displaystyle \left[\begin{matrix}0.527 & -0.574 & 0.628 & 4\\0.369 & 0.189 & 0.439 & 6\\-0.766 & 0 & 0.643 & 9\\0 & 0 & 0 & 1\end{matrix}\right]$




```python
sp.deg(rt.euler_angles(T,'zyx')).evalf()
```




$\displaystyle \left[\begin{matrix}34.9993162356405\\49.996038663149\\0\end{matrix}\right]$




```python
rt.rotation_matrix_from_euler_angles(rt.euler_angles(T,'zyx'),'zyx')
```




$\displaystyle \left[\begin{matrix}0.52658856828585 & -0.573566660599361 & 0.627475709170925 & 0\\0.368711919729561 & 0.819158889257082 & 0.439352062019111 & 0\\-0.766 & 0 & 0.642840571214979 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$



## Zacada 47


```python
d1, d2 = sp.symbols('d1, d2')
robot = rt.SerialLinkRobot()
robot.add_prismatic_joint(0,d1,0,-sp.pi/2)
robot.add_prismatic_joint(0,d2,0,0)
robot.interact()
```


    VBox(children=(ToggleButtons(options=('Зглоб 1', 'Зглоб 2'), value='Зглоб 1'), FloatSlider(value=1.0, descript…



```python
a1 = robot.get_dh_joint_to_joint(0,1)
a2 = robot.get_dh_joint_to_joint(1,2)
a1 * a2 
```




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



## Zadaca 48


```python
d1, d2, d3 = sp.symbols('d1, d2, d3')
robot = rt.SerialLinkRobot()
robot.add_prismatic_joint(0,d1,0,-sp.pi/2)
robot.add_prismatic_joint(sp.pi/2,d2,0,-sp.pi/2)
robot.add_prismatic_joint(0,d3,0,0)
robot.interact()
```


    VBox(children=(ToggleButtons(options=('Зглоб 1', 'Зглоб 2', 'Зглоб 3'), value='Зглоб 1'), FloatSlider(value=1.…



```python
a1 = robot.get_dh_joint_to_joint(0,1)
a2 = robot.get_dh_joint_to_joint(1,2)
a3 = robot.get_dh_joint_to_joint(2,3)
a1*a2*a3
```




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



## Zadaca 49


```python
theta1, theta2, l1, l2 = sp.symbols('theta1, theta2, l1, l2')
robot = rt.SerialLinkRobot()
robot.add_revolute_joint(theta1,0,l1,0)
robot.add_revolute_joint(theta2,0,l2,-sp.pi/2)
robot.add_subs([(l1,1),(l2,1)])
robot.interact()
```


    VBox(children=(ToggleButtons(options=('Зглоб 1', 'Зглоб 2'), value='Зглоб 1'), FloatSlider(value=0.0, descript…



```python
a1 = robot.get_dh_joint_to_joint(0,1)
a2 = robot.get_dh_joint_to_joint(1,2)
sp.simplify(a1*a2)
```




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



## Zadaca 51


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


    VBox(children=(ToggleButtons(options=('Зглоб 1', 'Зглоб 2', 'Зглоб 3'), value='Зглоб 1'), FloatSlider(value=0.…



```python
a1 = robot.get_dh_joint_to_joint(0,1)
a2 = robot.get_dh_joint_to_joint(1,2)
a3 = robot.get_dh_joint_to_joint(2,3)
sp.simplify(a1*a2*a3)
```




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



## Zadaca 52


```python
theta1, d2, theta3, d1, l3 = sp.symbols('theta1, d2, theta3, d1, l3')
robot = rt.SerialLinkRobot()
robot.add_revolute_joint(theta1,0,0,-sp.pi/2)
robot.add_prismatic_joint(0,d1,0,sp.pi/2)
robot.add_revolute_joint(theta2,0,1,0)
robot.interact()
```


    VBox(children=(ToggleButtons(options=('Зглоб 1', 'Зглоб 2', 'Зглоб 3'), value='Зглоб 1'), FloatSlider(value=0.…



```python
a1 = robot.get_dh_joint_to_joint(0,1)
a2 = robot.get_dh_joint_to_joint(1,2)
a3 = robot.get_dh_joint_to_joint(2,3)
sp.simplify(a1*a2*a3)
```




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



## Zadaca 53


```python
theta1, d1, d2 = sp.symbols('theta1, d1, d2')
robot = rt.SerialLinkRobot()
robot.add_revolute_joint(theta1,0,0,0)
robot.add_prismatic_joint(0,d1,0,-sp.pi/2)
robot.add_prismatic_joint(0,d2,0,0)
robot.interact()
```


    VBox(children=(ToggleButtons(options=('Зглоб 1', 'Зглоб 2', 'Зглоб 3'), value='Зглоб 1'), FloatSlider(value=0.…



```python
a1 = robot.get_dh_joint_to_joint(0,1)
a2 = robot.get_dh_joint_to_joint(1,2)
a3 = robot.get_dh_joint_to_joint(2,3)
sp.simplify(a1*a2*a3)
```




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



## Zadaca 54


```python
theta4, theta5, theta6 = sp.symbols('theta4, theta5, theta6')
robot = rt.SerialLinkRobot()
robot.add_revolute_joint(theta4,0,0,sp.pi/2)
robot.add_revolute_joint(theta5,0,0,-sp.pi/2)
robot.add_revolute_joint(theta6,1,0,0)
robot.interact()
```


    VBox(children=(ToggleButtons(options=('Зглоб 1', 'Зглоб 2', 'Зглоб 3'), value='Зглоб 1'), FloatSlider(value=0.…


## Zadaca 57


```python
theta1, theta2, d1, d3 = sp.symbols('theta1, theta2, d1, d3')
robot = rt.SerialLinkRobot()
robot.add_revolute_joint(theta1,d1,0,sp.pi/2)
robot.add_revolute_joint(theta2,0,0,-sp.pi/2)
robot.add_prismatic_joint(0,d3,0,0)
robot.add_subs([(d1,1)])
robot.interact()
```


    VBox(children=(ToggleButtons(options=('Зглоб 1', 'Зглоб 2', 'Зглоб 3'), value='Зглоб 1'), FloatSlider(value=0.…


## Zadaca 58


```python
theta1, theta2, d1, l2, l3, d4 = sp.symbols('theta1, theta2, d1, l2, l3, d4')
robot = rt.SerialLinkRobot()
robot.add_prismatic_joint(0,d1,0,0)
robot.add_revolute_joint(theta1,0,l2,0)
robot.add_revolute_joint(theta2,0,l3,sp.pi)
robot.add_prismatic_joint(0,d4,0,0)
robot.add_subs([(l2,1),(l3,1)])
robot.interact()
```


    VBox(children=(ToggleButtons(options=('Зглоб 1', 'Зглоб 2', 'Зглоб 3', 'Зглоб 4'), value='Зглоб 1'), FloatSlid…



```python
a1 = robot.get_dh_joint_to_joint(0,1)
a2 = robot.get_dh_joint_to_joint(1,2)
a3 = robot.get_dh_joint_to_joint(2,3)
a4 = robot.get_dh_joint_to_joint(3,4)
sp.simplify(a1*a2*a3*a4)
```




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



## Zadaca 60


```python
theta1, theta2, l1, l2 = sp.symbols('theta1, theta2, l1, l2')
robot = rt.SerialLinkRobot()
robot.add_revolute_joint(theta1, 0, l1, 0)
robot.add_revolute_joint(theta2, 0, l2, 0)
robot.add_subs([(l1, 1), (l2, 1)])
T = robot.get_dh_matrix()
T
```




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



## Zadaca 61


```python
theta1, theta2, d3, d1 ,x,y,z= sp.symbols('theta1, theta2, d3, d1,x,y,z')
robot = rt.SerialLinkRobot()
robot.add_revolute_joint(theta1, d1, 0, -sp.pi/2)
robot.add_revolute_joint(theta2, 0, 0, sp.pi/2)
robot.add_prismatic_joint(0, d3, 0, 0)
robot.add_subs([(d1, 1)])
T = robot.get_dh_matrix()
T
```




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




```python

```


```python
sp.solve([T[0,-1]-x,T[1,-1]-y,T[2,-1]-z],(theta1, theta2, d3, d1))
```




    [(-2*atan((x - sqrt(x**2 + y**2))/y),
      theta2,
      (-x**2 + x*sqrt(x**2 + y**2) - y**2)/((x - sqrt(x**2 + y**2))*sin(theta2)),
      y*cos(theta2)/(sin(theta2)*sin(2*atan((x - sqrt(x**2 + y**2))/y))) + z),
     (-2*atan((x + sqrt(x**2 + y**2))/y),
      theta2,
      -(x**2 + x*sqrt(x**2 + y**2) + y**2)/((x + sqrt(x**2 + y**2))*sin(theta2)),
      y*cos(theta2)/(sin(theta2)*sin(2*atan((x + sqrt(x**2 + y**2))/y))) + z)]




```python
x, y, z = sp.symbols('x, y, z')
eq1 = sp.Eq(x + y + z, 1)             # x + y + z  = 1
eq2 = sp.Eq(x + y + 2 * z, 3)         # x + y + 2z = 3
sp.solve((eq1, eq2), (x, y, z))
```




    {x: -y - 1, z: 2}




```python

```




$\displaystyle d_{3} \sin{\left(\theta_{2} \right)} \cos{\left(\theta_{1} \right)} - x$




```python

```

0209723640e72eb077c2768aa739cc12c1891beb
ipynb
Jupyter Notebook
Auditoriski/Tema2.ipynb
vasetrendafilov/Robotics1
27bfe8afcc6652c1cb95ea09b6b7eb3fe997bebb
MIT
Auditoriski/Tema2.ipynb
vasetrendafilov/Robotics1
27bfe8afcc6652c1cb95ea09b6b7eb3fe997bebb
MIT
Auditoriski/Tema2.ipynb
vasetrendafilov/Robotics1
27bfe8afcc6652c1cb95ea09b6b7eb3fe997bebb
MIT
Qwen/Qwen-72B
1. YES
2. YES