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

## Zadaca 9


```python
r, beta, gamma = sp.symbols('r, beta, gamma')
T = rt.hrotation3('z', gamma) * rt.hrotation3('y', beta) * rt.htranslation3(z=r)
T
```




$\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
pos = T[:3,3]
jacob = pos.jacobian([r,beta,gamma])
jacob
```




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




```python
detJ = jacob.det().simplify()
detJ
```




$\displaystyle r^{2} \sin{\left(\beta \right)}$




```python
interval = sp.Interval(-sp.pi, sp.pi, left_open=True)
sp.solveset(detJ,beta,interval)
```




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



## Zadaca 10


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


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



```python
robot.jacobian()
```




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



## Zadaca 18


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


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



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




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




```python
Jv2 = sp.Matrix.vstack(Jv[:1, :], Jv[2:, :])
Jv2
```




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




```python
Jv2.det()
```




$\displaystyle l_{2} \sin{\left(\theta_{2} \right)}$




```python
interval = sp.Interval(-sp.pi, sp.pi, left_open=True)
solution = sp.solveset(Jv2.det(), theta2, interval)
solution
```




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




```python
theta1, theta2, theta3, l1, l2, l3 = sp.symbols('theta1, theta2, theta3, l1, l2, l3')
robot = rt.SerialLinkRobot()
robot.add_revolute_joint(theta1, 0, l1, 0)
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=(HBox(children=(ToggleButton(value=True, description='Исклучи цртање'), Button(description='Пром…



```python
J = robot.jacobian()
J
```




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




```python
det_J = sp.det().simplify()
det_J
```




$\displaystyle 0$



## Zadaca 19


```python
theta1, theta2, d3, theta4, a1, a2, d4 = sp.symbols('theta1, theta2, d3, theta4, a1, a2, d4')
robot = rt.SerialLinkRobot()
robot.add_revolute_joint(theta1, 0, a1, 0)
robot.add_revolute_joint(theta2, 0, a2, sp.pi)
robot.add_prismatic_joint(0, d3, 0, 0)
robot.add_revolute_joint(theta4, d4, 0, 0)
robot.add_subs([(a1, 1), (a2, 1), (d4, 1)])
robot.interact()
```


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



```python
J = robot.jacobian()
J
```




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




```python
J4 = sp.Matrix.vstack(J[:3, :], J[5:, :])
J4
```




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




```python
det_J4 = sp.det(J[:2, :2]).simplify()
det_J4
```




$\displaystyle a_{1} a_{2} \sin{\left(\theta_{2} \right)}$




```python
interval = sp.Interval(-sp.pi, sp.pi, left_open=True)
solution = sp.solveset(det_J4, theta2, interval)
solution
```




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




```python

```

## Zadaca 20


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


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



```python
j = robot.jacobian()
j
```




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




```python
detj = sp.simplify(j[:2,1:3].det())
detj
```




$\displaystyle l_{2} l_{3} \sin{\left(\theta_{3} \right)}$




```python
interval = sp.Interval(-sp.pi, sp.pi, left_open=True)
solution = sp.solveset(detj, theta3, interval)
solution
```




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



## Zadaca 23


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


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



```python
T03 = robot.get_dh_joint_to_joint(0,3)
T03
```




$\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)} & \left(d_{2} \cos{\left(\theta_{2} \right)} + d_{3} \sin{\left(\theta_{2} \right)}\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)} & \left(d_{2} \cos{\left(\theta_{2} \right)} + d_{3} \sin{\left(\theta_{2} \right)}\right) \sin{\left(\theta_{1} \right)}\\\sin{\left(\theta_{2} \right)} & 0 & - \cos{\left(\theta_{2} \right)} & d_{1} + d_{2} \sin{\left(\theta_{2} \right)} - d_{3} \cos{\left(\theta_{2} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$




```python
J = robot.jacobian()
J = J.subs([(theta1,0),(theta2,0),(d3,5),(d2,1)])
poc = sp.Matrix([0.1, -0.1, 0.05])
Tdelta = J*poc
Tdelta
```




$\displaystyle \left[\begin{matrix}-0.5\\0.1\\-0.15\\0\\0.1\\0.1\end{matrix}\right]$




```python
delta = rt.hdelta3(dRx=0, dRy=0.1, dRz=0.1, dx=-0.5, dy=0, dz=-0.05)
delta
```




$\displaystyle \left[\begin{matrix}0 & -0.1 & 0.1 & -0.5\\0.1 & 0 & 0 & 0\\-0.1 & 0 & 0 & -0.05\\0 & 0 & 0 & 0\end{matrix}\right]$




```python
theta1, theta2, theta3, l1, l2, l3 = sp.symbols('theta1, theta2, theta3, l1, l2, l3')
robot = rt.SerialLinkRobot()
robot.add_revolute_joint(theta1, 0, l1, 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, 2)])
robot.interact()
```


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



```python
J = robot.jacobian()
J
```




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




```python
det_Jv = sp.simplify(J[:3,:].det())
det_Jv
```




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




```python
interval = sp.Interval(-sp.pi, sp.pi, left_open=True)
solution = sp.solveset(det_Jv, theta3, interval)
solution
```




$\displaystyle \left\{0, \pi\right\} \cup \left\{\theta_{3} \mid \theta_{3} \in \mathbb{R} \wedge \cos{\left(\theta_{2} + \theta_{3} \right)} + \frac{l_{1} + l_{2} \cos{\left(\theta_{2} \right)}}{l_{3}} = 0 \right\}$



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