# Problema:
Encontrar as cinmática das pontos que representam o movimento de um braço `Scara` de dois graus de liberdade como mostrado na figura abaixo.


## Referênciais
A localização dos referência fixo, `B0`, foi definida na altura do braço (fazendo com que seja um problema no plano) no centro do da base. Os referenciais móveis, `B1` e `B2`, foram fixados nos pinos de rotação acompanhando o movimento dos elos 1 e 2, respectivamente. Também definiu-se os comprimentos de cada elo como sendo `l_1` e `l_2`.


```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 = dynamicsymbols('theta_1 theta_2')
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.z])    # Referencial móvel: theta_1 em relação a B0.z 
B2 = ReferenceFrame('B2')
B2.orient(B1, 'Axis', [theta_2, B1.z])    # Referencial móvel: theta_2 em relação a B1.z 
```

## Matrizes de Rotação
Com a definição dos referenciais as matrizes de rotação entre os referenciais podem ser facilmente encontradas.


```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_B0B2 = (B0.dcm(B2)).simplify()     # Matriz de rotação de B0 para B3

# 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_1, \\theta_2}&=" + "&" + str(latex(T_B0B2))    + "\\end{eqnarray}")
```




\begin{eqnarray}T_{\theta_1}&=&\left[\begin{matrix}\cos{\left (\theta_{1}{\left (t \right )} \right )} & - \sin{\left (\theta_{1}{\left (t \right )} \right )} & 0\\\sin{\left (\theta_{1}{\left (t \right )} \right )} & \cos{\left (\theta_{1}{\left (t \right )} \right )} & 0\\0 & 0 & 1\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_1, \theta_2}&=&\left[\begin{matrix}\cos{\left (\theta_{1}{\left (t \right )} + \theta_{2}{\left (t \right )} \right )} & - \sin{\left (\theta_{1}{\left (t \right )} + \theta_{2}{\left (t \right )} \right )} & 0\\\sin{\left (\theta_{1}{\left (t \right )} + \theta_{2}{\left (t \right )} \right )} & \cos{\left (\theta_{1}{\left (t \right )} + \theta_{2}{\left (t \right )} \right )} & 0\\0 & 0 & 1\end{matrix}\right]\end{eqnarray}



## Posições Relativas dos pontos A e B
Por termos definido um referencial móvel 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 `B1`, nesse caso temos que $\vec{r_{AB}}={l_1 \vec{{i}_{B1}}}$. Finalmente, o ponto $C$ é representado por $\vec{r_{BC}}={l_2 \vec{{i}_{B2}}}$. Sendo $\vec{{i}_{B2}}$ e $\vec{{i}_{B3}}$ os versores na direção $x$ dos referenciais ```B1``` e  ```B2```, 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 * B1.x     # Vetor que liga os pontos A e B expresso no referencial móvel B1
r_BC = l_2 * B2.x     # Vetor que liga os pontos B e C expresso no referencial móvel B2

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)\mathbf{\hat{b0}_x} + l_{1} \operatorname{sin}\left(\theta_{1}\right)\mathbf{\hat{b0}_y}\\R_{AB}&=&\left[\begin{matrix}l_{1} \cos{\left (\theta_{1}{\left (t \right )} \right )}\\l_{1} \sin{\left (\theta_{1}{\left (t \right )} \right )}\\0\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)

# Resultados em LaTeX na forma de Vetores Coluna dos pontos A e B
Latex("\\begin{align}"
     "r_{A}&=" + str(vlatex(r_A)) + "&v_{A}&=" + str(vlatex(v_A)) + "&a_{A}&= " + str(vlatex(a_A)) + "\\" + "\\"
     "r_{B}&=" + str(vlatex(r_B)) + "&v_{B}&=" + str(vlatex(v_B)) + "&a_{B}&=" + str(vlatex(a_B)) + "\\" + "\\"
      "\\end{align}")
```




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




```python
# Resultados em LaTeX na forma de Vetores Coluna do ponto C
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}l_{1} \operatorname{cos}\left(\theta_{1}\right) + l_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right)\\l_{1} \operatorname{sin}\left(\theta_{1}\right) + l_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right)\\0\end{matrix}\right]\\\\v_{C}=&\left[\begin{matrix}- l_{1} \operatorname{sin}\left(\theta_{1}\right) \dot{\theta}_{1} - l_{2} \left(\dot{\theta}_{1} + \dot{\theta}_{2}\right) \operatorname{sin}\left(\theta_{1} + \theta_{2}\right)\\l_{1} \operatorname{cos}\left(\theta_{1}\right) \dot{\theta}_{1} + l_{2} \left(\dot{\theta}_{1} + \dot{\theta}_{2}\right) \operatorname{cos}\left(\theta_{1} + \theta_{2}\right)\\0\end{matrix}\right]\\\\a_{C}=& \left[\begin{matrix}- l_{1} \operatorname{sin}\left(\theta_{1}\right) \ddot{\theta}_{1} - l_{1} \operatorname{cos}\left(\theta_{1}\right) \dot{\theta}_{1}^{2} - l_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) \ddot{\theta}_{1} - l_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) \ddot{\theta}_{2} - l_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) \dot{\theta}_{1}^{2} - 2 l_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) \dot{\theta}_{1} \dot{\theta}_{2} - l_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) \dot{\theta}_{2}^{2}\\- l_{1} \operatorname{sin}\left(\theta_{1}\right) \dot{\theta}_{1}^{2} + l_{1} \operatorname{cos}\left(\theta_{1}\right) \ddot{\theta}_{1} + l_{2} \left(- \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) \dot{\theta}_{1}^{2} - 2 \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) \dot{\theta}_{1} \dot{\theta}_{2} - \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) \dot{\theta}_{2}^{2} + \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) \ddot{\theta}_{1} + \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) \ddot{\theta}_{2}\right)\\0\end{matrix}\right]\end{align}




```python

```

2cfaf4179c3e4215f3fa0e6af8785245d81f7fd6
ipynb
Jupyter Notebook
examples/scara_arm/Kinematics/Notebook_Kinematics_2DoF-Scara.ipynb
abhikamath/pydy
0d11df897c40178bb0ffd9caa9e25bccd1d8392a
BSD-3-Clause
2015-01-31T11:43:22.000Z
2022-03-15T02:18:21.000Z
examples/scara_arm/Kinematics/Notebook_Kinematics_2DoF-Scara.ipynb
tvdbogert/pydy
f248e7ad7db52de3393295e9c677af8ec7d0e186
BSD-3-Clause
2015-01-17T16:56:42.000Z
2022-02-08T05:27:08.000Z
examples/scara_arm/Kinematics/Notebook_Kinematics_2DoF-Scara.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