# Problema:
Solução da cadeia cinemática do robô antropomorphico mostrado abaixo.



O mecanismo possui 2 elos e 3 juntas.

## Sistema de Coordenadas
Primeiramente, temos que determinar a localização dos referenciais. O primeiro referencial, ```B0```, está fixo e será colocado na intercessão dos eixos das duas primeiras juntas. O segundo referencial, ```B1``` , está em rotação pura de $\theta_1$ em relação ao eixo $y$ do referencial ```B0```, e suas origens são coincidentes. A origem do referencial ```B2``` é a mesma de ```B1```  e ele segue o movimento da segunda junta, ou seja, ele está em rotação pura de $\theta_2$ em relação ao eixo $z$ de ```B1```. Por fim, a origem do referencial ```B3``` coincide com a intercessão do eixo $x$ de ```B2``` e o eixo de rotação da terceira junta, além de ser uma rotação pura de $\theta_3$ em relação ao eixo $z$ de ```B2```. Os elos são as partes rígidas que conectam as juntas; assim, nesse caso, temos três elos. Como o primeiro referencial é conincidente com os dois próximos, só precisaremos definir o comprimento de dois dos elos. Definindo o ponto $A$ na junta 2, o ponto $B$ na junta 3 e o ponto $C$ na ponta do último elo, podemos definir a distância entre os pontos  A e B, e B e C como sendo $l_1$ e $l_2$, respectivamente.
Assim, já podemos iniciar a implementação da solução do problema.


```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 
```

Esse código cria as variáveis simbolicas e os referenciais que serão usados pelo ```sympy```. $\theta_1, \theta_2$ e $\theta_3$ foram definidas usando ```dynammicsymbols``` para mostrar que as variáveis são funções do tempo, e o método ```symbols``` foi usado junto com o argumento ```positive=True``` para definir que os comprimentos dos elos assumem apenas valores positivos.
Através na classe ```ReferenceFrame``` podemos facilmente criar referenciais inerciais. Todos os referenciais foram definidos a partir de uma rotação de outro referencial definido anteriormente, com exceção do referencial fixo ```B0```.

### Matrizes de Rotação
Então com essas definições fica fácil representar as matrizes de rotação entre qualquer um dos referenciais definidos. Para isso, invocamos o método ```.dcm``` no referencial que queremos definir a partir do referencial do argumento. A seguir estão alguns exemplos, junto com o representação dos resultados em 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}



### Posições Relativas de $A$, $B$ e $C$
Por termos definido um referencial para cada grau de liberdade do mecanismo, podemos facilmente expressar a posição de cada um dos pontos $A$, $B$ e $C$ em relação a esses referênciais. O ponto $A$ coincide com a origem do referencial fixo, ou seja, $\vec{r_{OA}}=\vec{0}$. O ponto $B$ é facilmente expresso em relação ao referencial ```B2```, nesse caso temos que $\vec{r_{AB}}={l_1 \vec{{i}_{B2}}}$. Finalmente, o ponto $C$ é representado por $\vec{r_{BC}}={l_2 \vec{{i}_{B3}}}$. Sendo $\vec{{i}_{B2}}$ e $\vec{{i}_{B3}}$ os versores na direção $x$ dos referenciais ```B2``` e  ```B3``` respectivamente. A seguir, nota-se que a definição desses vetores posição é simples de ser implementado em ```sympy```. A principal vantagem é que não precisamos nos preocupar em relação a qual referencial estamos definindo os vetores, pois as relações entre os referenciais já foram criadas nas suas definições. Ou seja, podemos reescrever um vetor em relação a diferentes referenciais de maneira imediata, como mostrado abaixo.


```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}



## Resultados
Assim, já podemos encontrar os resultados da cinemática do ponto $C$ em relação a ```B0```. Para tal, só precisamos calcular a os vetores posição dos pontos $A$, $B$ e $C$, e suas primeiras e segunda derivadas no tempo, já que as variáveis $\theta_1$, $\theta_2$ e $\theta_3$ foram definidas como funções do tempo. Para melhor eficiência, poderíamos calcular os vetores aproveitando as fórmulas de Cinemática. Adicionalmente, a função ```trigsimp``` foi utilizada em cada elementos dos vetores colunas resultantes para simplificá-los. Então, segue o resultado.


```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}



93f55717fb947fafee2599caf6f9891deaf222ad
ipynb
Jupyter Notebook
examples/antropomorphic_robot/Notebook_Kinematics_3DoF-Antropomorphic_pt.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_pt.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_pt.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