# ---
# jupyter:
#   jupytext:
#     text_representation:
#       extension: .py
#       format_name: light
#       format_version: '1.5'
#       jupytext_version: 1.14.4
#   kernelspec:
#     display_name: Python 3
#     language: python
#     name: python3
# ---

# + [markdown] slideshow={"slide_type": "slide"}
# # Velocity and Acceleration of a point of a rigid body
#
# > <NAME>, <NAME>  
# > [Laboratory of Biomechanics and Motor Control](http://pesquisa.ufabc.edu.br/bmclab)  
# > Federal University of ABC, Brazil

# + [markdown] slideshow={"slide_type": "skip"}
# This notebook shows the expressions of  the velocity and acceleration of a point on rigid body, given the angular velocity of the body.

# + [markdown] slideshow={"slide_type": "slide"}
# ## Frame of reference attached to a body
#
# The concept of reference frame in Biomechanics and motor control is very important and central to the understanding of human motion. For example, do we see, plan and control the movement of our hand with respect to reference frames within our body or in the environment we move? Or a combination of both?  
# The figure below, although derived for a robotic system, illustrates well the concept that we might have to deal with multiple coordinate systems.  
#
# <div class='center-align'><figure><img src="https://raw.githubusercontent.com/demotu/BMC/master/images/coordinatesystems.png" width=450/><figcaption><center><i>Figure. Multiple coordinate systems for use in robots (figure from Corke (2017)).</i></center></figcaption></figure></div>
#
# For three-dimensional motion analysis in Biomechanics, we may use several different references frames for convenience and refer to them as global, laboratory, local, anatomical, or technical reference frames or coordinate systems (we will study this later).  
# There has been proposed different standardizations on how to define frame of references for the main segments and joints of the human body. For instance, the International Society of Biomechanics has a [page listing standardization proposals](https://isbweb.org/activities/standards) by its standardization committee and subcommittees:

# + [markdown] slideshow={"slide_type": "slide"}
# ## Position of a point on a rigid body
#
# The description of the position of a point P of a rotating rigid body is given by:
#
# <span class="notranslate">
# \begin{equation}
# {\bf\vec{r}_{P/O}} = x_{P/O}^*{\bf\hat{i}'} + y_{P/O}^*{\bf\hat{j}'}
# \end{equation}
# </span>
#
# where $x_{P/O}^*$ and $y_{P/O}^*$ are the coordinates of the point P position at a reference state with the versors described as:
#
# <span class="notranslate">
# \begin{equation}
# {\bf\hat{i}'} = \cos(\theta){\bf\hat{i}}+\sin(\theta){\bf\hat{j}}
# \end{equation}
# </span>
#
# <span class="notranslate">
# \begin{equation}
# {\bf\hat{j}'} = -\sin(\theta){\bf\hat{i}}+\cos(\theta){\bf\hat{j}}
# \end{equation}
# </span>
#
#
# <img src="../images/rotBody.png" style="width: 1000000px;">
#
# Note that the vector ${\bf\vec{r}_{P/O}}$ has always the same description for any point P of the rigid body when described as a linear combination of <span class="notranslate">${\bf\hat{i}'}$</span> and <span class="notranslate">${\bf\hat{j}'}$</span>.
#

# + [markdown] slideshow={"slide_type": "slide"}
# ## Translation of a rigid body
#
#  Let's consider now the case in which, besides a rotation, a translation of the body happens. This situation is represented in the figure below. In this case, the position of the point P is given by:
#
# <span class="notranslate">
# \begin{equation}
# {\bf\vec{r}_{P/O}} = {\bf\vec{r}_{A/O}}+{\bf\vec{r}_{P/A}}= {\bf\vec{r}_{A/O}}+x_{P/A}^*{\bf\hat{i}'} + y_{P/A}^*{\bf\hat{j}'}
# \end{equation}
# </span>
#
# <img src="../images/rotTrBody.png" width=1000/>

# + [markdown] slideshow={"slide_type": "slide"}
# ## Angular velocity of a body
#
# The magnitude of the angular velocity of a rigid  body rotating on a plane is defined as:
#
# <span class="notranslate">
# \begin{equation}
#     \omega = \frac{d\theta}{dt}
# \end{equation}
# </span>
#
# Usually, it is defined an angular velocity vector perpendicular to the plane where the rotation occurs (in this case the x-y plane) and with magnitude $\omega$:
#
# <span class="notranslate">
# \begin{equation}
#     \vec{\bf{\omega}} = \omega\hat{\bf{k}}
# \end{equation}
# </span>
#
#
# <img src="../images/angvel.png" width=600/>

# + [markdown] slideshow={"slide_type": "slide"}
# ## Velocity of a point with no translation

# + [markdown] slideshow={"slide_type": "slide"}
# First we will consider the situation with no translation. The velocity of the point P is given by:
#
# <span class="notranslate">
# \begin{equation}
# {\bf\vec{v}_{P/O}} = \frac{d{\bf\vec{r}_{P/O}}}{dt} = \frac{d(x_{P/O}^*{\bf\hat{i}'} + y_{P/O}^*{\bf\hat{j}'})}{dt}
# \end{equation}
# </span>

# + [markdown] slideshow={"slide_type": "slide"}
# To continue this deduction, we have to find the expression of the derivatives of <span class="notranslate">
# ${\bf\hat{i}'}$</span> and <span class="notranslate">${\bf\hat{j}'}$</span>. This is very similar to the derivative expressions of ${\bf\hat{e_R}}$ and  ${\bf\hat{e_\theta}}$ of [polar basis](http://nbviewer.jupyter.org/github/BMClab/bmc/blob/master/notebooks/PolarBasis.ipynb).
#
# <span class="notranslate">
# \begin{equation}
# \frac{d{\bf\hat{i}'}}{dt} = -\dot{\theta}\sin(\theta){\bf\hat{i}}+\dot{\theta}\cos(\theta){\bf\hat{j}} = \dot{\theta}{\bf\hat{j}'}
# \end{equation}
# </span>
#
# <span class="notranslate">
# \begin{equation}
# \frac{d{\bf\hat{j}'}}{dt} = -\dot{\theta}\cos(\theta){\bf\hat{i}}-\dot{\theta}\sin(\theta){\bf\hat{j}} = -\dot{\theta}{\bf\hat{i}'}
# \end{equation}
# </span>

# + [markdown] slideshow={"slide_type": "slide"}
# Another way to represent the expressions above is by using the vector form to express the angular velocity $\dot{\theta}$. It is usual to represent the angular velocity as a vector in the direction ${\bf\hat{k}}$: ${\bf\vec{\omega}} = \dot{\theta}{\bf\hat{k}} = \omega{\bf\hat{k}}$. Using this definition of the angular velocity, we can write the above expressions as:
#
# <span class="notranslate">
# \begin{equation}
# \frac{d{\bf\hat{i}'}}{dt} = \dot{\theta}{\bf\hat{j}'} = \dot{\theta} {\bf\hat{k}}\times {\bf\hat{i}'} = {\bf\vec{\omega}} \times {\bf\hat{i}'}
# \end{equation}
# </span>
#
# <span class="notranslate">
# \begin{equation}
# \frac{d{\bf\hat{j}'}}{dt} = -\dot{\theta}{\bf\hat{i}'} = \dot{\theta} {\bf\hat{k}}\times {\bf\hat{j}'} ={\bf\vec{\omega}} \times {\bf\hat{j}'}
# \end{equation}
# </span>

# + [markdown] slideshow={"slide_type": "slide"}
# So, the velocity of the point P in the situation of no translation is:
#
# <span class="notranslate">
# \begin{equation}
# {\bf\vec{v}_{P/O}} = \frac{d(x_{P/O}^*{\bf\hat{i}'} + y_{P/O}^*{\bf\hat{j}'})}{dt} = x_{P/O}^*\frac{d{\bf\hat{i}'}}{dt} + y_{P/O}^*\frac{d{\bf\hat{j}'}}{dt}=x_{P/O}^*{\bf\vec{\omega}} \times {\bf\hat{i}'} + y_{P/O}^*{\bf\vec{\omega}} \times {\bf\hat{j}'} = {\bf\vec{\omega}} \times \left(x_{P/O}^*{\bf\hat{i}'}\right) + {\bf\vec{\omega}} \times \left(y_{P/O}^*{\bf\hat{j}'}\right) ={\bf\vec{\omega}} \times \left(x_{P/O}^*{\bf\hat{i}'}+y_{P/O}^*{\bf\hat{j}'}\right)
# \end{equation}
# </span>
#
# <span class="notranslate">
# \begin{equation}
# {\bf\vec{v}_{P/O}} = {\bf\vec{\omega}} \times {\bf{\vec{r}_{P/O}}}
# \end{equation}
# </span>
#
# This expression shows that the velocity vector of any point of a rigid body is orthogonal to the vector linking the point O and the point P.
#
# It is worth to note that despite the above expression was deduced for a planar movement, the expression above is general, including three dimensional movements.

# + [markdown] slideshow={"slide_type": "slide"}
# ## Relative velocity of a point on a rigid body to another point
#
# To compute the velocity of a point on a rigid body that is translating, we need to find the expression of the velocity of a point (P) in relation to another point on the body (A). So:
#
# <span class="notranslate">
# \begin{equation}
# {\bf\vec{v}_{P/A}} = {\bf\vec{v}_{P/O}}-{\bf\vec{v}_{A/O}} = {\bf\vec{\omega}} \times {\bf{\vec{r}_{P/O}}} - {\bf\vec{\omega}} \times {\bf{\vec{r}_{A/O}}} = {\bf\vec{\omega}} \times ({\bf{\vec{r}_{P/O}}}-{\bf{\vec{r}_{A/O}}}) =  {\bf\vec{\omega}} \times {\bf{\vec{r}_{P/A}}}
# \end{equation}
# </span>
#
#

# + [markdown] slideshow={"slide_type": "slide"}
# ## Velocity of a point on rigid body translating
#
# The velocity of a point on a rigid body that is translating is given by:
#
# <span class="notranslate">
# \begin{equation}
# {\bf\vec{v}_{P/O}}  = \frac{d{\bf\vec{r}_{P/O}}}{dt} = \frac{d({\bf\vec{r}_{A/O}}+x_{P/A}^*{\bf\hat{i}'} + y_{P/A}^*{\bf\hat{j}'})}{dt} = \frac{d{\bf\vec{r}_{A/O}}}{dt}+\frac{d(x_{P/A}^*{\bf\hat{i}'} + y_{P/A}^*{\bf\hat{j}'})}{dt} = {\bf\vec{v}_{A/O}} + {\bf\vec{\omega}} \times {\bf{\vec{r}_{P/A}}}
# \end{equation}
# </span>

# + [markdown] slideshow={"slide_type": "slide"}
# Below is an example of a body rotating with the angular velocity of $\omega = \pi/10$ rad/s and translating at the velocity of <span class="notranslate">
# ${\bf\vec{v}} = 0.7 {\bf\hat{i}} + 0.5 {\bf\hat{j}}$ m/s</span>. The red arrow indicates the velocity of the geometric center of the body and the blue arrow indicates the velocity of the lower point of the body

# + slideshow={"slide_type": "slide"}
import numpy as np
import matplotlib.pyplot as plt
# %matplotlib notebook
from matplotlib.animation import FuncAnimation
from matplotlib.patches import FancyArrowPatch
t = np.linspace(0,13,10)
omega = np.pi/10 #[rad/s] 
voa = np.array([[0.7],[0.5]]) # velocity of center of mass
fig = plt.figure()
plt.grid()
ax = fig.add_axes([0, 0, 1, 1])    
ax.axis("on")
plt.rcParams['figure.figsize']=5,5
def run(i):
    ax.clear()
    theta = omega * t[i]
    phi = np.linspace(0,2*np.pi,100)
    B = np.squeeze(np.array([[2*np.cos(phi)],[6*np.sin(phi)]]))
    Baum = np.vstack((B,np.ones((1,np.shape(B)[1]))))
    roa = voa * t[i]
    R = np.array([[np.cos(theta), -np.sin(theta)],[np.sin(theta), np.cos(theta)]])
    T = np.vstack((np.hstack((R,roa)), np.array([0,0,1])))
    BRot = R@B
    BRotTr = T@Baum   
    
    
    
    plt.plot(BRotTr[0,:],BRotTr[1,:], roa[0], roa[1],'.')
    plt.fill(BRotTr[0,:],BRotTr[1,:], 'g')
    
    vVoa = FancyArrowPatch(np.array([float(roa[0]), float(roa[1])]), 
                           np.array([float(roa[0]+5*voa[0]), 
                                       float(roa[1]+5*voa[1])]), mutation_scale=20, 
                           lw=2, arrowstyle="->", color="r", alpha=1)
    ax.add_artist(vVoa)   
    
    element = 75
    Vp = np.array([voa[0]-omega*BRot[1,element], voa[1]+omega*BRot[0,element]])
    
    vVP = FancyArrowPatch(np.array([float(BRotTr[0,element]), float(BRotTr[1,element])]), 
                          np.array([float(BRotTr[0,element]+5*Vp[0]), 
                                      float(BRotTr[1,element]+5*Vp[1])]),
                          mutation_scale=20, 
                          lw=2, arrowstyle="->", color="b", alpha=1)
    ax.add_artist(vVP)
    
    plt.xlim((-10, 20))
    plt.ylim((-10, 20))
     
ani = FuncAnimation(fig, run, frames = 50,repeat=False,  interval =500)
plt.show()


# + [markdown] slideshow={"slide_type": "slide"}
# ## Acceleration of  a point on a rigid body
#
# The acceleration of a point on a rigid body is obtained by deriving the previous expression:
#
# <span class="notranslate">
# \begin{equation}
# {\bf\vec{a}_{P/O}} = {\bf\vec{a}_{A/O}} + \dot{\bf\vec{\omega}} \times {\bf{\vec{r}_{P/A}}} + {\bf\vec{\omega}} \times {\bf{\vec{v}_{P/A}}} = {\bf\vec{a}_{A/O}} + \dot{\bf\vec{\omega}} \times {\bf{\vec{r}_{P/A}}} + {\bf\vec{\omega}} \times ({\bf\vec{\omega}} \times {\bf{\vec{r}_{P/A}}}) = {\bf\vec{a}_{A/O}} + \ddot{\theta}\bf\hat{k} \times {\bf{\vec{r}_{P/A}}} - \dot{\theta}^2{\bf{\vec{r}_{P/A}}}
# \end{equation}
# </span>
#
# The acceleration has three terms:
#
# - ${\bf\vec{a}_{A/O}}$ -- the acceleration of the point O.
# - $\ddot{\theta}\bf\hat{k} \times {\bf{\vec{r}_{P/A}}}$ -- the acceleration of the point P due to the angular acceleratkion of the body.
# - $- \dot{\theta}^2{\bf{\vec{r}_{P/A}}}$ -- the acceleration of the point P due to the angular velocity of the body. It is known as centripetal acceleration.
#
#
#
# Below is an example of a rigid  body with an angular acceleration of $\alpha = \pi/150$ rad/s$^2$ and initial angular velocity of $\omega_0 = \pi/100$ rad/s. Consider also that the center of the body accelerates with <span class="notranslate">${\bf\vec{a}} = 0.01{\bf\hat{i}} + 0.05{\bf\hat{j}}$</span>, starting from rest.
#

# + slideshow={"slide_type": "slide"}
t = np.linspace(0, 20, 40)

alpha = np.pi/150 #[rad/s^2] angular acceleration 

omega0 = np.pi/100 #[rad/s] angular velocity

aoa = np.array([[0.01],[0.05]]) # linear acceleration

fig = plt.figure()
plt.grid()
ax = fig.add_axes([0, 0, 1, 1])    
ax.axis("on")
plt.rcParams['figure.figsize']=5,5
theta = 0
omega = 0
def run(i):
    ax.clear()
    phi = np.linspace(0,2*np.pi,100)
    B = np.squeeze(np.array([[2*np.cos(phi)],[6*np.sin(phi)]]))
    Baum = np.vstack((B,np.ones((1,np.shape(B)[1]))))
    
    omega = alpha*t[i]+omega0 #[rad/s] angular velocity
    
    theta = alpha/2*t[i]**2 + omega0*t[i] # [rad] angle
    
    voa = aoa*t[i] # linear velocity
    
    roa = aoa/2*t[i]**2 # position of the center of the body
    
    R = np.array([[np.cos(theta), -np.sin(theta)],[np.sin(theta), np.cos(theta)]])
    T = np.vstack((np.hstack((R,roa)), np.array([0,0,1])))
    BRot = R@B
    BRotTr = T@Baum   
    
    plt.plot(BRotTr[0,:],BRotTr[1,:], roa[0], roa[1],'.')
    plt.fill(BRotTr[0,:],BRotTr[1,:],'g')
    
    element = 75
    ap = np.array([aoa[0] - alpha*BRot[1,element] - omega**2*BRot[0,element],
                   aoa[1] + alpha*BRot[0,element] - omega**2*BRot[1,element]])
    vVP = FancyArrowPatch(np.array([float(BRotTr[0,element]), float(BRotTr[1,element])]), 
                          np.array([float(BRotTr[0,element]+5*ap[0]), 
                                      float(BRotTr[1,element]+5*ap[1])]),
                          mutation_scale=20, 
                          lw=2, arrowstyle="->", color="b", alpha=1)
    ax.add_artist(vVP)
    
    plt.xlim((-10, 20))
    plt.ylim((-10, 20))
       
    

ani = FuncAnimation(fig, run, frames=50,repeat=False,  interval=500)
plt.show()


# + [markdown] slideshow={"slide_type": "slide"}
# ## Problems
#
# 1. Solve the problems 16.2.5, 16.2.10, 16.2.11 and 16.2.20 from [Ruina and Rudra's book](http://ruina.tam.cornell.edu/Book/index.html).
# 2. Solve the problems 17.1.2, 17.1.8, 17.1.9, 17.1.10, 17.1.11 and 17.1.12 from [Ruina and Rudra's book](http://ruina.tam.cornell.edu/Book/index.html).
#

# + [markdown] slideshow={"slide_type": "slide"}
# ## Reference
#
# - <NAME>, <NAME> (2019) [Introduction to Statics and Dynamics](http://ruina.tam.cornell.edu/Book/index.html). Oxford University Press.
# - <NAME> (2017) [Robotics, Vision and Control: Fundamental Algorithms in MATLAB](http://www.petercorke.com/RVC/). 2nd ed. Springer-Verlag Berlin.

StarcoderdataJupyterScriptsDedupFiltered