# Problem:
Solution of the kinematic chain of the anthropomorphic robot shown below.



The mechanism has 2 links and 3 gaskets.

## Coordinate System
First, we have to determine frames location. The inertial reference, `B0`, is fixed and will be placed in the intercession of the first two joints axes. The second reference, `B1`, is in pure rotation of $\theta_1$ with respect to the $y$ axis of the `B0` reference, and their origins coincide. The origin of the `B2` reference is the same as `B1` and it follows the movement of the second joint, that is, it is in pure rotation, $\theta_2$, in relation to the $z$ axis of ` B1`. Finally, the origin of the `B3` reference coincides with the intercession of the $x$ axis of `B2` and the axis of rotation of the third joint, besides being a pure rotation of $\theta_3$ with respect to the axis $z$ of `B2`. The links are the rigid parts that connect the joints; so in this case we have three links. Since the first frame is coincident with the next two, we only need to define the length of two links. By defining the $A$ point at joint 2, the point $B$ at joint 3 and the point $C$ at the tip of the last link, we can define the distance between points A and B, and B and C as $l_1$ and $l_2$, respectively.
Thus, we can begin to implement the solution of the problem.


```python
# Funções das Bibliotecas Utilizadas
from sympy import symbols
from sympy.physics.mechanics import dynamicsymbols
from sympy.physics.vector import ReferenceFrame

# Variáveis Simbólicas
theta_1, theta_2, theta_3 = dynamicsymbols('theta_1 theta_2 theta_3')
l_1, l_2 = symbols('l_1 l_2', positive = True)

# Referenciais 
B0 = ReferenceFrame('B0')                 # Referencial Parado
B1 = ReferenceFrame('B1')     
B1.orient(B0, 'Axis', [theta_1, B0.y])    # Referencial móvel: theta_1 em relação a B0.y 
B2 = ReferenceFrame('B2')
B2.orient(B1, 'Axis', [theta_2, B1.z])    # Referencial móvel: theta_2 em relação a B1.z 
B3 = ReferenceFrame('B3')
B3.orient(B2, 'Axis', [theta_3, B2.z])    # Referencial móvel: theta_3 em relação a B2.z 
```

This code creates the symbolic and referential variables that will be used by `sympy`. $\theta_1, \theta_2$ and $\theta_3$ were defined using `dynammicsymbols` to show that these variables are time functions, and the `symbols` method was used together with the `positive = True` argument to define that the lengths of the links are positive values.
`ReferenceFrame` class is used to create inertial references. All references were defined as a rotation from the previous reference frame, excepting the fixed reference `B0`.

### Rotation Matrices
With these definitions, it is easy to represent the rotation matrices between any of the defined frames. For this, we invoke the `.dcm` method (Direction Cosine Matrix) that creates the matrix between the frames used. Here, are some examples, along with the representation of the results in LaTeX.


```python
# Matrizes de Rotação
T_B0B1 = B0.dcm(B1)    # Matriz de rotação de B1 para B0
T_B1B2 = B1.dcm(B2)    # Matriz de rotação de B2 para B1
T_B2B3 = B2.dcm(B3)    # Matriz de rotação de B3 para B2

T_B0B3 = (B0.dcm(B3)).simplify()    # Matriz de rotação de B0 para B3
T_B0B2 = (B0.dcm(B2)).simplify()

# Resultados em LaTeX
from IPython.display import Latex
from sympy import latex

Latex("\\begin{eqnarray}" +
     "T_{\\theta_1}&=" + "&" +str(latex(T_B0B1)) + " T_{\\theta_2}="
     + str(latex(T_B1B2)) + "T_{\\theta_3}=" + str(latex(T_B2B3)) + 
     "\\\\T_{\\theta_1, \\theta_3}&=" + "&" + str(latex(T_B0B3))
     + "\\end{eqnarray}")
```




\begin{eqnarray}T_{\theta_1}&=&\left[\begin{matrix}\cos{\left (\theta_{1}{\left (t \right )} \right )} & 0 & \sin{\left (\theta_{1}{\left (t \right )} \right )}\\0 & 1 & 0\\- \sin{\left (\theta_{1}{\left (t \right )} \right )} & 0 & \cos{\left (\theta_{1}{\left (t \right )} \right )}\end{matrix}\right] T_{\theta_2}=\left[\begin{matrix}\cos{\left (\theta_{2}{\left (t \right )} \right )} & - \sin{\left (\theta_{2}{\left (t \right )} \right )} & 0\\\sin{\left (\theta_{2}{\left (t \right )} \right )} & \cos{\left (\theta_{2}{\left (t \right )} \right )} & 0\\0 & 0 & 1\end{matrix}\right]T_{\theta_3}=\left[\begin{matrix}\cos{\left (\theta_{3}{\left (t \right )} \right )} & - \sin{\left (\theta_{3}{\left (t \right )} \right )} & 0\\\sin{\left (\theta_{3}{\left (t \right )} \right )} & \cos{\left (\theta_{3}{\left (t \right )} \right )} & 0\\0 & 0 & 1\end{matrix}\right]\\T_{\theta_1, \theta_3}&=&\left[\begin{matrix}\cos{\left (\theta_{2}{\left (t \right )} + \theta_{3}{\left (t \right )} \right )} \cos{\left (\theta_{1}{\left (t \right )} \right )} & - \sin{\left (\theta_{2}{\left (t \right )} + \theta_{3}{\left (t \right )} \right )} \cos{\left (\theta_{1}{\left (t \right )} \right )} & \sin{\left (\theta_{1}{\left (t \right )} \right )}\\\sin{\left (\theta_{2}{\left (t \right )} + \theta_{3}{\left (t \right )} \right )} & \cos{\left (\theta_{2}{\left (t \right )} + \theta_{3}{\left (t \right )} \right )} & 0\\- \sin{\left (\theta_{1}{\left (t \right )} \right )} \cos{\left (\theta_{2}{\left (t \right )} + \theta_{3}{\left (t \right )} \right )} & \sin{\left (\theta_{2}{\left (t \right )} + \theta_{3}{\left (t \right )} \right )} \sin{\left (\theta_{1}{\left (t \right )} \right )} & \cos{\left (\theta_{1}{\left (t \right )} \right )}\end{matrix}\right]\end{eqnarray}



### Relative positions of $A$, $B$, and $C$
As we defined a reference frame for each degree of freedom in the mechanism, the position of the points $A$, $B$ and $C$ can be easily represented. The $A$ point coincides with the origin of the fixed referential, that is, $\vec{r_{OA}}=\vec{0}$. The $B$ point is easily expressed relative to the `B2` reference, in which case we have $\vec{r_{AB}}={l_1 \vec{{i}_{B2}}}$. Finally, the $C$ point is represented by $\vec{r_{BC}}={l_2 \vec{{i}_{B3}}}$. $\vec{{i}_{B2}}$ and $\vec{{i}_{B3}}$ being the versors in the $x$ direction of the references `B2` and `B3` respectively. Next, should be noticed that the definition of these positional vectors is simple to implement in `sympy`. The main advantage is that we do not have to worry about which referential we are defining the vectors, because the relations between the frames were previously created in their definitions. That is, we can rewrite a vector in relation to different references immediately, as shown below.


```python
from sympy.physics.vector import Vector

# Vetores Posição entre os Pontos
r_OA = Vector(0)      # Vetor Nulo
r_AB = l_1 * B2.x     # Vetor que liga os pontos A e B expresso no referencial móvel B2
r_BC = l_2 * B3.x     # Vetor que liga os pontos B e C expresso no referencial móvel B3

R_AB = r_AB.express(B0) # Vetor que liga os pontos A e B expresso no referencial fixo B0
R_BC = r_BC.express(B0) # Vetor que liga os pontos B e C expresso no referencial fixo B0

# Resultado em LaTeX na Forma Vetorial ou Vetor Coluna
Latex("\\begin{eqnarray}" +
     "R_{AB}&=" + "&" +str(latex(R_AB)) + "\\\\" + 
     "R_{AB}&=" + "&" +str(latex(R_AB.to_matrix(B0))) +
     "\\end{eqnarray}")
```




\begin{eqnarray}R_{AB}&=&l_{1} \operatorname{cos}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2}\right)\mathbf{\hat{b0}_x} + l_{1} \operatorname{sin}\left(\theta_{2}\right)\mathbf{\hat{b0}_y} -  l_{1} \operatorname{sin}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2}\right)\mathbf{\hat{b0}_z}\\R_{AB}&=&\left[\begin{matrix}l_{1} \cos{\left (\theta_{1}{\left (t \right )} \right )} \cos{\left (\theta_{2}{\left (t \right )} \right )}\\l_{1} \sin{\left (\theta_{2}{\left (t \right )} \right )}\\- l_{1} \sin{\left (\theta_{1}{\left (t \right )} \right )} \cos{\left (\theta_{2}{\left (t \right )} \right )}\end{matrix}\right]\end{eqnarray}



## Results
Thus, we are already able to find the resulting kinematics of point $C$ with respect to `` `B0``. For this, we only need to calculate the position vectors of points $A$, $B$ and $C$, and their first and second time derivatives, since the variables $\theta_1$, $\theta_2$ and $\theta_3$ were defined as time functions. For best efficiency, we could calculate the vectors using the Kinematics formulas. In addition, the `trigsimp` function was used on each element of the resulting column vectors to simplify them. The result is shown bellow.


```python
from sympy.physics.vector import time_derivative, vlatex
from sympy import trigsimp

# Cinemática do ponto A em relação ao referencial B0
r_A  = r_OA.express(B0)
v_A = time_derivative(r_A, B0)
a_A = time_derivative(v_A, B0)

# Cinemática do ponto B em relação ao referencial B0
r_B  = r_A.express(B0) + r_AB.express(B0)
v_B = time_derivative(r_B, B0)
a_B = time_derivative(v_B, B0)

# Cinemática do ponto C em relação ao referencial B0
r_C  = r_B.express(B0) + r_BC.express(B0)
v_C = (time_derivative(r_C, B0))
a_C = (time_derivative(v_C, B0))

# Simplifcação dos Resultados
r_A = (r_A.to_matrix(B0)).applyfunc(trigsimp)
v_A = (v_A.to_matrix(B0)).applyfunc(trigsimp)
a_A = (a_A.to_matrix(B0)).applyfunc(trigsimp)
r_B = (r_B.to_matrix(B0)).applyfunc(trigsimp)
v_B = (v_B.to_matrix(B0)).applyfunc(trigsimp)
a_B = (a_B.to_matrix(B0)).applyfunc(trigsimp)
r_C = (r_C.to_matrix(B0)).applyfunc(trigsimp)
v_C = (v_C.to_matrix(B0)).applyfunc(trigsimp)
a_C = (a_C.to_matrix(B0)).applyfunc(trigsimp)
```


```python
# Resultados em LaTeX na forma de Vetores Coluna
Latex("\\begin{align}"
     "r_{C}=&" + str(vlatex(r_C)) + "\\\\" + "\\\\"
     "v_{C}=&" + str(vlatex(v_C)) + "\\\\" + "\\\\"
     "a_{C}=& " + str(vlatex(a_C)) + "\\end{align}")
```




\begin{align}
_{C}=&\left[\begin{matrix}\left(l_{1} \operatorname{cos}\left(\theta_{2}\right) + l_{2} \operatorname{cos}\left(\theta_{2} + \theta_{3}\right)\right) \operatorname{cos}\left(\theta_{1}\right)\\l_{1} \operatorname{sin}\left(\theta_{2}\right) + l_{2} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right)\\- \left(l_{1} \operatorname{cos}\left(\theta_{2}\right) + l_{2} \operatorname{cos}\left(\theta_{2} + \theta_{3}\right)\right) \operatorname{sin}\left(\theta_{1}\right)\end{matrix}\right]\\\\v_{C}=&\left[\begin{matrix}- l_{1} \operatorname{sin}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2}\right) \dot{\theta}_{1} - l_{1} \operatorname{sin}\left(\theta_{2}\right) \operatorname{cos}\left(\theta_{1}\right) \dot{\theta}_{2} - l_{2} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) \operatorname{cos}\left(\theta_{1}\right) \dot{\theta}_{2} - l_{2} \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) \operatorname{cos}\left(\theta_{1}\right) \dot{\theta}_{3} - l_{2} \operatorname{sin}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) \dot{\theta}_{1}\\l_{1} \operatorname{cos}\left(\theta_{2}\right) \dot{\theta}_{2} + l_{2} \left(\dot{\theta}_{2} + \dot{\theta}_{3}\right) \operatorname{cos}\left(\theta_{2} + \theta_{3}\right)\\l_{1} \operatorname{sin}\left(\theta_{1}\right) \operatorname{sin}\left(\theta_{2}\right) \dot{\theta}_{2} - l_{1} \operatorname{cos}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2}\right) \dot{\theta}_{1} + l_{2} \left(\operatorname{sin}\left(\theta_{2} + \theta_{3}\right) \operatorname{sin}\left(\theta_{1}\right) \dot{\theta}_{2} + \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) \operatorname{sin}\left(\theta_{1}\right) \dot{\theta}_{3} - \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) \operatorname{cos}\left(\theta_{1}\right) \dot{\theta}_{1}\right)\end{matrix}\right]\\\\a_{C}=& \left[\begin{matrix}2 l_{1} \operatorname{sin}\left(\theta_{1}\right) \operatorname{sin}\left(\theta_{2}\right) \dot{\theta}_{1} \dot{\theta}_{2} - l_{1} \operatorname{sin}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2}\right) \ddot{\theta}_{1} - l_{1} \operatorname{sin}\left(\theta_{2}\right) \operatorname{cos}\left(\theta_{1}\right) \ddot{\theta}_{2} - l_{1} \operatorname{cos}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2}\right) \dot{\theta}_{1}^{2} - l_{1} \operatorname{cos}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2}\right) \dot{\theta}_{2}^{2} + l_{2} \left(2 \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) \operatorname{sin}\left(\theta_{1}\right) \dot{\theta}_{1} \dot{\theta}_{2} + 2 \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) \operatorname{sin}\left(\theta_{1}\right) \dot{\theta}_{1} \dot{\theta}_{3} - \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) \operatorname{cos}\left(\theta_{1}\right) \ddot{\theta}_{2} - \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) \operatorname{cos}\left(\theta_{1}\right) \ddot{\theta}_{3} - \operatorname{sin}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) \ddot{\theta}_{1} - \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) \operatorname{cos}\left(\theta_{1}\right) \dot{\theta}_{1}^{2} - \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) \operatorname{cos}\left(\theta_{1}\right) \dot{\theta}_{2}^{2} - 2 \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) \operatorname{cos}\left(\theta_{1}\right) \dot{\theta}_{2} \dot{\theta}_{3} - \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) \operatorname{cos}\left(\theta_{1}\right) \dot{\theta}_{3}^{2}\right)\\- l_{1} \operatorname{sin}\left(\theta_{2}\right) \dot{\theta}_{2}^{2} + l_{1} \operatorname{cos}\left(\theta_{2}\right) \ddot{\theta}_{2} + l_{2} \left(- \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) \dot{\theta}_{2}^{2} - 2 \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) \dot{\theta}_{2} \dot{\theta}_{3} - \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) \dot{\theta}_{3}^{2} + \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) \ddot{\theta}_{2} + \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) \ddot{\theta}_{3}\right)\\l_{1} \operatorname{sin}\left(\theta_{1}\right) \operatorname{sin}\left(\theta_{2}\right) \ddot{\theta}_{2} + l_{1} \operatorname{sin}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2}\right) \dot{\theta}_{1}^{2} + l_{1} \operatorname{sin}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2}\right) \dot{\theta}_{2}^{2} + 2 l_{1} \operatorname{sin}\left(\theta_{2}\right) \operatorname{cos}\left(\theta_{1}\right) \dot{\theta}_{1} \dot{\theta}_{2} - l_{1} \operatorname{cos}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2}\right) \ddot{\theta}_{1} + l_{2} \left(\operatorname{sin}\left(\theta_{2} + \theta_{3}\right) \operatorname{sin}\left(\theta_{1}\right) \ddot{\theta}_{2} + \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) \operatorname{sin}\left(\theta_{1}\right) \ddot{\theta}_{3} + 2 \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) \operatorname{cos}\left(\theta_{1}\right) \dot{\theta}_{1} \dot{\theta}_{2} + 2 \operatorname{sin}\left(\theta_{2} + \theta_{3}\right) \operatorname{cos}\left(\theta_{1}\right) \dot{\theta}_{1} \dot{\theta}_{3} + \operatorname{sin}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) \dot{\theta}_{1}^{2} + \operatorname{sin}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) \dot{\theta}_{2}^{2} + 2 \operatorname{sin}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) \dot{\theta}_{2} \dot{\theta}_{3} + \operatorname{sin}\left(\theta_{1}\right) \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) \dot{\theta}_{3}^{2} - \operatorname{cos}\left(\theta_{2} + \theta_{3}\right) \operatorname{cos}\left(\theta_{1}\right) \ddot{\theta}_{1}\right)\end{matrix}\right]\end{align}



ea3bb9c0a82b0bab4f127ec6905c86a8b76c5e24
ipynb
Jupyter Notebook
examples/antropomorphic_robot/Notebook_Kinematics_3DoF-Antropomorphic_en.ipynb
abhikamath/pydy
0d11df897c40178bb0ffd9caa9e25bccd1d8392a
BSD-3-Clause
2015-01-31T11:43:22.000Z
2022-03-15T02:18:21.000Z
examples/antropomorphic_robot/Notebook_Kinematics_3DoF-Antropomorphic_en.ipynb
abhikamath/pydy
0d11df897c40178bb0ffd9caa9e25bccd1d8392a
BSD-3-Clause
2015-01-17T16:56:42.000Z
2022-02-08T05:27:08.000Z
examples/antropomorphic_robot/Notebook_Kinematics_3DoF-Antropomorphic_en.ipynb
pydy/pydy
4a2c46faae44d06017b64335e48992ee8c53e1b6
BSD-3-Clause
2015-02-03T13:02:45.000Z
2021-12-21T12:57:21.000Z
Qwen/Qwen-72B
1. YES
2. YES