# Clustering Analysis
In this chapter we will concern ourselves with the study of **cluster analysis**.
In general terms cluster analysis, or clustering, is the task of grouping a
data-set into different distinct categories based on some measure of equality of
the data. This measure is often referred to as a **metric** or **similarity
measure** in the literature (note: sometimes we deal with a **dissimilarity
measure** instead). Usually, these metrics are formulated as some kind of
distance function between points in a high-dimensional space.

There exists a lot of such distance measures. The simplest, and also the most
common is the **Euclidean distance** (i.e. Pythagoras). A good source for those of
you wanting a thorough overview is the article (DOI:10.5120/ijca2016907841
Irani, Pise, Phatak). A few other metrics mentioned there are: *cosine
similarity*, *Manhattan distance*, *Chebychev distance* and the *Minkowski
distance*. The Minkowski distance is a general formulation which encapsulates a
range of metrics. All of these, and many more, can be used in clustering. There
exists different categories of clustering algorithms. A few of the most
common are: *centroid-*, *distribution-*, *density-* and *hierarchical-
clustering*. We will concern ourselves primarily with the first one.

## Basic Idea of the K-means Clustering Algorithm
The simplest of all clustering algorithms is the aptly named **k-means algorithm**
, sometimes also referred to as *Lloyds algorithm*. It is the simplest and also
the most common. From its simplicity it obtains both strengths and weaknesses.
These will be discussed in more detail later. The k-means algorithm is a
**centroid based** clustering algorithm.

Assume, we are given $n$ data points and we wish to split the data into $K < n$
different categories, or clusters. We label each cluster by an integer $k\in\{
1, \cdots, K \}$. In the basic k-means algorithm each point is assigned to only
one cluster $k$, and these assignments are *non-injective* i.e. many-to-one. We
can think of these mappings as an encoder $k = C(i)$, which assigns the $i$-th
data-point $\bf x_i$ to the $k$-th cluster. Before we jump into the mathematics
let us describe the k-means algorithm in words:
1. We start with guesses / random initializations of our $k$ cluster centers / centroids

2. For each centroid the points that are most similar are identified

3. Then we move / replace each centroid with a coordinate average of all the points that were assigned to that centroid.

4. Iterate this points 2, 3) until the centroids no longer move (to some tolerance)

Now we consider the method formally. Again, we assume we have $n$ data-points
(vectors)

$$
\begin{equation}\label{eq:kmeanspoints} \tag{1}
  \boldsymbol{x_i}  = \{x_{i, 1}, \cdots, x_{i, p}\}\in\mathbb{R}^p.
\end{equation}
$$

which we wish to group into $K < n$ clusters. For our dissimilarity measure we
will use the *squared Euclidean distance*

$$
\begin{equation}\label{eq:squaredeuclidean} \tag{2}
  d(\boldsymbol{x_i}, \boldsymbol{x_i'}) = \sum_{j=1}^p(x_{ij} - x_{i'j})^2
                         = ||\boldsymbol{x_i} - \boldsymbol{x_{i'}}||^2
\end{equation}
$$

Next we define the so called *within-cluster point scatter* which gives us a
measure of how close each data point assigned to the same cluster tends to be to
the all the others.

$$
\begin{equation}\label{eq:withincluster} \tag{3}
  W(C) = \frac{1}{2}\sum_{k=1}^K\sum_{C(i)=k}
          \sum_{C(i')=k}d(\boldsymbol{x_i}, \boldsymbol{x_{i'}}) =
          \sum_{k=1}^KN_k\sum_{C(i)=k}||\boldsymbol{x_i} - \boldsymbol{\overline{x_k}}||^2
\end{equation}
$$

where $\boldsymbol{\overline{x_k}}$ is the mean vector associated with the $k$-th
cluster, and $N_k = \sum_{i=1}^nI(C(i) = k)$, where the $I()$ notation is
similar to the Kronecker delta (*Commonly used in statistics, it just means that
when $i = k$ we have the encoder $C(i)$*). In other words,  the within-cluster
scatter measures the compactness of each cluster with respect to the data points
assigned to each cluster. This is the quantity that the $k$-means algorithm aims
to minimize. We refer to this quantity $W(C)$ as the within cluster scatter
because of its relation to the *total scatter*.

$$
\begin{equation}\label{eq:totalscatter} \tag{4}
  T = W(C) + B(C) = \frac{1}{2}\sum_{i=1}^n
                    \sum_{i'=1}^nd(\boldsymbol{x_i}, \boldsymbol{x_{i'}})
                  = \frac{1}{2}\sum_{k=1}^K\sum_{C(i)=k}
                    \Big(\sum_{C(i') = k}d(\boldsymbol{x_i}, \boldsymbol{x_{i'}})
                  + \sum_{C(i')\neq k}d(\boldsymbol{x_i}, \boldsymbol{x_{i'}})\Big)
\end{equation}
$$

Which is a quantity that is conserved throughout the $k$-means algorithm. It can
be thought of as the total amount of information in the data, and it is composed
of the aforementioned within-cluster scatter and the *between-cluster scatter*
$B(C)$. In methods such as principle component analysis the total scatter is not
conserved.

Given a cluster mean $\boldsymbol{m_k}$ we define the **total cluster variance**

$$
\begin{equation}\label{eq:totalclustervariance} \tag{5}
  \min_{C, \{\boldsymbol{m_k}\}_1^K}\sum_{k=1}^KN_k\sum||\boldsymbol{x_i} - \boldsymbol{m_k}||^2
\end{equation}
$$

Now we have all the pieces necessary to formally revisit the k-means algorithm.
If you at this point feel like some of the above definitions came a bit out of
no-where, don't fret, the method does get a whole lot simpler once we start
programming.

## The K-means Clustering Algorithm
The k-means clustering algorithm goes as follows (note in my opinion this
description is a bit complicated and is lifted directly out of ESL HASTIE for
deeper understanding purposes)

1. For a given cluster assignment $C$, and $k$ cluster means $\{m_1, \cdots, m_k\}$. We minimize the total cluster variance with respect to the cluster means $\{m_k\}$ yielding the means of the currently assigned clusters.

2. Given a current set of $k$ means $\{m_k\}$ the total cluster variance is minimized by assigning each observation to the closest (current) cluster mean. That is $$C(i) = \underset{1\leq k\leq K}{\mathrm{argmin}} ||\boldsymbol{x_i} - \boldsymbol{m_k}||^2$$

3. Steps 1 and 2 are repeated until the assignments do not change.

As previously stated the above formulation can be a bit difficult to understand,
*at least the first time*, due to the dense notation used. But all in all the
concept is fairly simple when explained in words. The math needs to be
understood but to help you along the way we summarize the algorithm as follows
(try to look at the terms above to match with the summary).

1. Before we start we specify a number $k$ which is the number of clusters we want to try to separate our data into.

2. We initially choose $k$ random data points in our data as our initial centroids, *or means* (this is where the name comes from).

3. Assign each data point to their closest centroid, based on the squared Euclidean distance.

4. For each of the $k$ cluster we update the centroid by calculating new mean values for all the data points in the cluster.

5. Iteratively minimize the within cluster scatter by performing steps (3, 4) until the new assignments stop changing (can be to some tolerance) or until a maximum number of iterations have passed.

That's it, nothing magical happening.

## Writing Our Own Code
In the following section we will work to develop a deeper understanding of the
previously discussed mathematics through developing codes to do k-means cluster
analysis.

### Basic Python

Let us now program the most basic version of the algorithm using nothing but
Python with numpy arrays. This code is kept intentionally simple to gradually
progress our understanding. There is no vectorization of any kind, and even most
helper functions are not utilized. Throughout our implementation process it will
be helpful to keep in mind both the mathematical description of the algorithm
*and* our summary from above. In addition, try to think of ways to optimize this
while reading the next section. We will get to it, take it as a challenge to see
if your optimizations are better.

First of all we need a dataset to do our cluster analysis on, for clarity (and
lack of googling beforehand) we generate it ourselves using Gaussians. First we
import

```python
%matplotlib inline

%matplotlib inline
import matplotlib.pyplot as plt
import numpy as np
import time
from IPython.display import display

np.random.seed(2021)
```

Next we define functions, for ease of use later, to generate Gaussians and to
set up our toy data set.

```python
def gaussian_points(dim=2, n_points=1000, mean_vector=np.array([0, 0]),
                    sample_variance=1):
    """
    Very simple custom function to generate gaussian distributed point clusters
    with variable dimension, number of points, means in each direction
    (must match dim) and sample variance.

    Inputs:
        dim (int)
        n_points (int)
        mean_vector (np.array) (where index 0 is x, index 1 is y etc.)
        sample_variance (float)

    Returns:
        data (np.array): with dimensions (dim x n_points)
    """

    mean_matrix = np.zeros(dim) + mean_vector
    covariance_matrix = np.eye(dim) * sample_variance
    data = np.random.multivariate_normal(mean_matrix, covariance_matrix,
                                    n_points)
    return data

def generate_simple_clustering_dataset(dim=2, n_points=1000, plotting=True,
                                    return_data=True):
    """
    Toy model to illustrate k-means clustering
    """

    data1 = gaussian_points(mean_vector=np.array([5, 5]))
    data2 = gaussian_points()
    data3 = gaussian_points(mean_vector=np.array([1, 4.5]))
    data4 = gaussian_points(mean_vector=np.array([5, 1]))
    data = np.concatenate((data1, data2, data3, data4), axis=0)

    if plotting:
        fig, ax = plt.subplots()
        ax.scatter(data[:, 0], data[:, 1], alpha=0.2)
        ax.set_title('Toy Model Dataset')
        plt.show()

    if return_data:
        return data

data = generate_simple_clustering_dataset()
```

Now that we are our, albeit very simple, dataset we are ready to start
implementing the k-means algorithm.

```python
n_samples, dimensions = data.shape
n_clusters = 4

# we randomly initialize our centroids
np.random.seed(2021)
centroids = data[np.random.choice(n_samples, n_clusters, replace=False), :]
distances = np.zeros((n_samples, n_clusters))

# first we need to calculate the distance to each centroid from our data
for k in range(n_clusters):
    for n in range(n_samples):
        dist = 0
        for d in range(dimensions):
            dist += np.abs(data[n, d] - centroids[k, d])**2
            distances[n, k] = dist

# we initialize an array to keep track of to which cluster each point belongs
# the way we set it up here the index tracks which point and the value which
# cluster the point belongs to
cluster_labels = np.zeros(n_samples, dtype='int')

# next we loop through our samples and for every point assign it to the cluster
# to which it has the smallest distance to
for n in range(n_samples):
    # tracking variables (all of this is basically just an argmin)
    smallest = 1e10
    smallest_row_index = 1e10
    for k in range(n_clusters):
        if distances[n, k] < smallest:
            smallest = distances[n, k]
            smallest_row_index = k

    cluster_labels[n] = smallest_row_index
```

Let's plot and see

```python
fig = plt.figure()
ax = fig.add_subplot()
unique_cluster_labels = np.unique(cluster_labels)
for i in unique_cluster_labels:
    ax.scatter(data[cluster_labels == i, 0],
               data[cluster_labels == i, 1],
               label = i,
               alpha = 0.2)
    ax.scatter(centroids[:, 0], centroids[:, 1], c='black')

ax.set_title("First Grouping of Points to Centroids")

plt.show()
```

So what do we have so far? We have 'picked' $k$ centroids at random from our
data points. There are other ways of more intelligently choosing their
initializations, however for our purposes randomly is fine. Then we have
initialized an array 'distances' which holds the information of the distance,
*or dissimilarity*, of every point to of our centroids. Finally, we have
initialized an array 'cluster_labels' which according to our distances array
holds the information of to which centroid every point is assigned. This was the
first pass of our algorithm. Essentially, all we need to do now is repeat the
distance and assignment steps above until we have reached a desired convergence
or a maximum amount of iterations.

```python

max_iterations = 100
tolerance = 1e-8
start_time = time.time()

for iteration in range(max_iterations):
    prev_centroids = centroids.copy()
    for k in range(n_clusters):
        # this array will be used to update our centroid positions
        vector_mean = np.zeros(dimensions)
        mean_divisor = 0
        for n in range(n_samples):
            if cluster_labels[n] == k:
                vector_mean += data[n, :]
                mean_divisor += 1

        # update according to the k means
        centroids[k, :] = vector_mean / mean_divisor

    # we find the dissimilarity
    for k in range(n_clusters):
        for n in range(n_samples):
            dist = 0
            for d in range(dimensions):
                dist += np.abs(data[n, d] - centroids[k, d])**2
                distances[n, k] = dist

    # assign each point
    for n in range(n_samples):
        smallest = 1e10
        smallest_row_index = 1e10
        for k in range(n_clusters):
            if distances[n, k] < smallest:
                smallest = distances[n, k]
                smallest_row_index = k

        cluster_labels[n] = smallest_row_index

    # convergence criteria
    centroid_difference = np.sum(np.abs(centroids - prev_centroids))
    if centroid_difference < tolerance:
        print(f'Converged at iteration {iteration}')
        print(f'Runtime: {time.time() - start_time} seconds')
        break

    elif iteration == max_iterations:
        print(f'Did not converge in {max_iterations} iterations')
        print(f'Runtime: {time.time() - start_time} seconds')
```

    Converged at iteration 5
    Runtime: 0.5119049549102783 seconds

And thats it! We now have an extremely barebones, un-optimized k-means
clustering implementation. Lets plot the final result

```python
fig = plt.figure()
ax = fig.add_subplot()
unique_cluster_labels = np.unique(cluster_labels)
for i in unique_cluster_labels:
    ax.scatter(data[cluster_labels == i, 0],
               data[cluster_labels == i, 1],
               label = i,
               alpha = 0.2)
    ax.scatter(centroids[:, 0], centroids[:, 1], c='black')

ax.set_title("Final Result of K-means Clustering")

plt.show()
```

Now there are a few glaring improvements to be done here. First of all is
organizing things into functions for better readability. Second is getting rid
of the small inefficiencies like manually calculating distances and argmin. And
finally, we need to optimize for better run-time. It's like we always say: the
best way of looping in Python is to not loop in Python. Let us tackle the first
two improvements.

## Towards a More Numpythonic Code

```python

def get_distances_to_clusters(data, centroids):
    """
    Function that for each cluster finds the squared Euclidean distance
    from every data point to the cluster centroid and returns a numpy array
    containing the distances such that distance[i, j] means the distance between
    the i-th point and the j-th centroid.
    Inputs:
        data (np.array): with dimensions (n_samples x dim)
        centroids (np.array): with dimensions (n_clusters x dim)

    Returns:
        distances (np.array): with dimensions (n_samples x n_clusters)
    """

    n_samples, dimensions = data.shape
    n_clusters = centroids.shape[0]
    distances = np.zeros((n_samples, n_clusters))
    for k in range(n_clusters):
        for i in range(n_samples):
            dist = 0
            for j in range(dimensions):
                dist += np.abs(data[i, j] - centroids[k, j])**2
                distances[i, k] = dist

    return distances

def assign_points_to_clusters(distances):
    """
    Function to assign each data point to the cluster to which it is the closest
    based on the squared Euclidean distance from the get_distances_to_clusters
    method.
    Inputs:
        distances (np.array): with dimensions (n_samples x n_clusters)

    Returns:
        cluster_labels (np.array): with dimensions (n_samples)
    """
    cluster_labels = np.argmin(distances, axis=1)

    return cluster_labels

def k_means(data, n_clusters=4, max_iterations=100, tolerance=1e-8):
    """
    Naive implementation of the k-means clustering algorithm. A short summary of
    the algorithm is as follows: we randomly initialize k centroids / means.
    Then we assign, using the squared Euclidean distance, every data-point to a
    cluster. We then update the position of the k centroids / means, and repeat
    until convergence or we reach our desired maximum iterations. The method
    returns the cluster assignments of our data-points and a sequence of
    centroids.
    Inputs:
        data (np.array): with dimesions (n_samples x dim)
        n_clusters (int): hyperparameter which depends on dataset
        max_iterations (int): hyperparameter which depends on dataset
        tolerance (float): convergence measure

    Returns:
        cluster_labels (np.array): with dimension (n_samples)
        centroid_list (list): list of centroids (np.array)
                              with dimensions (n_clusters x dim)
    """

    samples, dimensions = data.shape
    np.random.seed(2021)
    centroids = data[np.random.choice(len(data), n_clusters, replace=False), :]
    distances = get_distances_to_clusters(data, centroids)
    cluster_labels = assign_points_to_clusters(distances)

    start_time = time.time()

    for iteration in range(max_iterations):
        prev_centroids = centroids.copy()
        for k in range(n_clusters):
            vector_mean = np.zeros(dimensions)
            mean_divisor = 0
            for n in range(n_samples):
                if cluster_labels[n] == k:
                    vector_mean += data[n, :]
                    mean_divisor += 1
            # And update according to the new means
            centroids[k, :] = vector_mean / mean_divisor

        distances = get_distances_to_clusters(data, centroids)
        cluster_labels = assign_points_to_clusters(distances)

        centroid_difference = np.sum(np.abs(centroids - prev_centroids))
        if centroid_difference < tolerance:
            print(f'Converged at iteration: {iteration}')
            print(f'Runtime: {time.time() - start_time} seconds')

            return cluster_labels, centroids

    print(f'Did not converge in {max_iterations} iterations')
    print(f'Runtime: {time.time() - start_time} seconds')

    return cluster_labels, centroids

# quirk of numpy / Jupyter need to set seed again
cluster_labels, centroids = k_means(data)
```

    Converged at iteration: 5
    Runtime: 0.42575693130493164 seconds

**Note**: the start of the timing is after the random initialization, and first
'cycle' of our algorithm. This is technically not the correct way to time it but
due to this being in a Jupyter notebook and the way it is structured this way of
comparing our algorithms will produce a more equal result. When timing code we
should always encapsulate our whole computation block.

So we see an improvement from just switching to numpy's argmin function. There
is a very nice tool (or category of tools) called profilers. These can be
utilized to make clearer which improvements to our code we should care most
about here is an [excellent source](https://ipython-books.github.io/42-profiling-your-code-easily-with-cprofile-and-ipython/)
on the topic. Even before optimizing we can understand which parts of our code
will be taking the most of the run-time. It will be the longest Python loop,
i.e. the loop over all the samples. Nonetheless, let us do some profiling!

```python
test_data = generate_simple_clustering_dataset(n_points=10000, plotting=False)
%prun -l 10 cluster_labels, centroids = k_means(test_data)
```

    Converged at iteration: 11
    Runtime: 0.8621249198913574 seconds

Here we can see the reason for profiling. We now know for certain a lot can be
gained just by vectorizing our distance function. Ideally we wish to perform
most of our loops in numpy, i.e. C. To do this we need our array shapes to match
and clever reshaping will let us do so.

```python

def np_get_distances_to_clusters(data, centroids):
    """
    Squared Euclidean distance between all data-points and every centroid. For
    the function to work properly it needs data and centroids to be numpy
    broadcastable. We sum along the dimension axis.
    Inputs:
        data (np.array): with dimensions (samples x 1 x dim)
        centroids (np.array): with dimensions (1 x n_clusters x dim)

    Returns:
        distances (np.array): with dimensions (samples x n_clusters)
    """

    distances = np.sum(np.abs((data - centroids))**2, axis=2)
    return distances

def np_assign_points_to_clusters(distances):
    """
    Assigning each data-point to a cluster given an array distances containing
    the squared Euclidean distance from every point to each centroid. We do
    np.argmin along the cluster axis to find the closest cluster. Returns a
    numpy array with corresponding labels.
    Inputs:
        distances (np.array): with dimensions (samples x n_clusters)

    Returns:
        cluster_labels (np.array): with dimensions (samples x None)
    """
    cluster_labels = np.argmin(distances, axis=1)
    return cluster_labels

def np_k_means(data, n_clusters=4, max_iterations=100, tolerance=1e-8):
    """
    Numpythonic implementation of the k-means clusting algorithm.
    Inputs:
        data (np.array): with dimesions (samples x dim)
        n_clusters (int): hyperparameter which depends on dataset
        max_iterations (int): hyperparameter which depends on dataset
        tolerance (float): convergence measure
        progression_plot (bool): activation flag for plotting
    Returns:
        cluster_labels (np.array): with dimension (samples)
        centroid_list (list): list of centroids (np.array)
                              with dimensions (n_clusters x dim)
    """
    n_samples, dimensions = data.shape
    np.random.seed(2021)
    centroids = data[np.random.choice(len(data), n_clusters, replace=False), :]

    distances = np_get_distances_to_clusters(np.reshape(data,
                                            (n_samples, 1, dimensions)),
                                          np.reshape(centroids,
                                            (1, n_clusters, dimensions)))
    cluster_labels = np_assign_points_to_clusters(distances)

    start_time = time.time()

    for iteration in range(max_iterations):
        prev_centroids = centroids.copy()
        for k in range(n_clusters):
            points_in_cluster = data[cluster_labels == k]
            mean_vector = np.mean(points_in_cluster, axis=0)
            centroids[k] = mean_vector

        distances = np_get_distances_to_clusters(np.reshape(data,
                                                (n_samples, 1, dimensions)),
                                              np.reshape(centroids,
                                                (1, n_clusters, dimensions)))
        cluster_labels = np_assign_points_to_clusters(distances)

        centroid_difference = np.sum(np.abs(centroids - prev_centroids))
        if centroid_difference < tolerance:
            print(f'Converged at iteration: {iteration}')
            print(f'Runtime: {time.time() - start_time} seconds')

            return cluster_labels, centroids

    print(f'Did not converge in {max_iterations} iterations')
    print(f'Runtime: {time.time() - start_time} seconds')

    return cluster_labels, centroids
```

When working towards becoming a data scientist using Python this last step is
arguably one of the most important. Thinking of ways to avoid explicitly looping
by adding dimensions to our arrays in such a way that they become broadcastable
using numpy (also tensorflow and many others). Let us take a look at our the
fruits of our labor.

```python
cluster_labels, centroids = np_k_means(data)
```

    Converged at iteration: 5
    Runtime: 0.008977890014648438 seconds

```python

```


# ATSC 507 Assignment X - Due Mar 12 [Delayed]

```python
__author__ = 'Yingkai (Kyle) Sha'
__email__  = 'yingkai@eos.ubc.ca'
```

```python
import sympy as sp
import numpy as np
import matplotlib.pyplot as plt
from __future__ import division
from mpl_toolkits.axes_grid1 import make_axes_locatable
% matplotlib inline
```

```python
sp.init_printing()
```

```python
def search_stable(A):
    '''
    Get the indeices of all stable amplitude factor.
    '''
    N = np.size(A, 0)
    index = np.ones(N)*-99
    for i in range(N):
        if (A[i, :].max()-1 <= 1e-15):
            index[i] = i
        index[index==-99]=np.nan
    return index
```

#Content
1. [**3p. centered in time, 5p. centered in space**](#3p.-centered-in-time,-5p.-centered-in-space)
2. [**WRF-ARW version of 3rd order Runge-Kutta, 3p centered in space**](#WRF-ARW-version-of-3rd-order-Runge-Kutta,-3p-centered-in-space)

# 3p. centered in time, 5p. centered in space

## Analytical soutions of amplification factor

Apply the scheme to 1D advection equation with constant advection speed ($u_0$):

$$
    \frac{1}{2\Delta t}\left[T_{j, n+1}-T_{j, n-1}\right] = - \frac{u_0}{12\Delta x}\left[ T_{j-2,n} - 8T_{j-1,n} + 8T_{j+1, n} - T_{j+2, n} \right]
$$

Dividing the both sides with $T_{j,n}$, and define *Courant number* $C = u_0\frac{\Delta t}{\Delta x}$:

$$
    T_{0, 1}-T_{0, -1} = - \frac{C}{6}\left[ T_{-2, 0} - 8T_{-1, 0} + 8T_{1, 0} - T_{2, 0} \right]
$$

For $T_{j, n}$ in a wave-like solution (eigenmode):

$$
    T_{j, n} = [A]^n\exp\left[ik\left(j\Delta x\right)\right]
$$

Substituding $T_{j, n}$ into the equation, we get:

$$
    A - \frac1A + \frac{C}6\left[ - (\exp(2ik\Delta x) - \exp(-2ik\Delta x)) +  8\exp(ik\Delta x) - 8\exp(-ik\Delta x) \right] = 0
$$

Substitude in Euler formular, we have:

$$
    A - \frac1A + \frac{C}{3}[8\sin(k\Delta x)i - 2\sin(k\Delta x)\cos(k\Delta x)i] = 0
$$

And in order to make things easier, I applied *non-dimensional spatial step* $X = k\Delta x$.

For von Neumann instability analysis, we solving this equation respect to $A$, and looking for $C$ solves $|A| \leq 1$ for any $X = k\Delta x$ .

```python
A =sp.symbols('A') # symbols
C, X = sp.symbols('C, X', real=True)
#equ = A - 1/A + C/6*(sp.exp(-2*X*sp.I)-8*sp.exp(-1*X*sp.I)+8*sp.exp(X*sp.I)-sp.exp(2*X*sp.I))
equ = A - 1/A + C/3*(8*sp.sin(X)*sp.I - 2*sp.sin(X)*sp.cos(X)*sp.I)
result = sp.solve(equ, A) # A1 = result[0]; A2 = result[1]
```

We expected 2 roots for this quadratic equation in the form of:

$$
    A_1 = \frac{-b+\sqrt{b^2-4ac}}{2a} \qquad\ A_2 = \frac{-b-\sqrt{b^2-4ac}}{2a}
$$

**Root I** for $A_1$:

```python
sp.simplify(result[0].expand(trig=True))
```

For $A_1$, the third term under square root can be both real and imagnary, if it is **real**, I can prove it always equals to 1 (and I use the expression for $A^2$):

```python
equ_A1=(-4*C*sp.sin(X)/3 + C*sp.sin(2*X)/6)**2 + \
(-1*sp.sqrt(C**2*sp.sin(X)**4 + 8*C**2*sp.sin(X)**2*sp.cos(X) - 17*C**2*sp.sin(X)**2 + 9)/3)**2
equ_A1.expand(trig=True)
```

We know that:

$$
    \sin^2(X)\sin^2(X) + \sin^2(X)\cos^2(X) = \sin^2(X)
$$

so we see:

```python
sp.simplify(equ_A1.expand(trig=True))
```

And the folllowing numerical test will show that, the **stable** case belongs to the "**real** case" hear.

We see similar things for **Root II** & $A_2$:

```python
sp.simplify(result[1].expand(trig=True))
```

```python
equ_A1=(-4*C*sp.sin(X)/3 + C*sp.sin(2*X)/6)**2 + \
(sp.sqrt(C**2*sp.sin(X)**4 + 8*C**2*sp.sin(X)**2*sp.cos(X) - 17*C**2*sp.sin(X)**2 + 9)/3)**2
sp.simplify(equ_A1.expand(trig=True))
```

## Exploring amplification factors with numerics

*Warner p69* states that, this scheme is stable when $C \leq 0.729$, so here I substituding $C\ \epsilon\ [0.5, 1.0]$  and $k\Delta x\ \epsilon\ [0.5, 3]$ and do the following things:
* Plot amplification factors in a frame of Courant numbers and $k\Delta x$.
* Verify `Warner`'s answer in a certain toleratence of accuracy.
* Find the amount of damping for different Courant number and $k\Delta x$

```python
NUM1 = 200
NUM2 = 200
C_num = np.linspace(0.5, 1.0, NUM1)
X_num = np.linspace(0.5, 3, NUM2)
x, c = np.meshgrid(X_num, C_num)
```

```python
num_result1 = np.empty([NUM1, NUM2]) + 1j*np.empty([NUM1, NUM2])
num_result2 = np.empty([NUM1, NUM2]) + 1j*np.empty([NUM1, NUM2])
for i in range(NUM1):
    for m in range(NUM2):
        num_result1[i, m] = result[0].evalf(subs={X:x[i, m], C:c[i, m]})
        num_result2[i, m] = result[1].evalf(subs={X:x[i, m], C:c[i, m]})
num_result1 = np.abs(num_result1)
num_result2 = np.abs(num_result2)
```

```python
Alev = [0, 0.25, 0.5, 0.75, 1, 1.25, 1.5,  1.75, 2]

fig = plt.figure(figsize=(16, 4))

ax1=plt.subplot2grid((1, 2), (0, 0), colspan=1, rowspan=1)
ax2=plt.subplot2grid((1, 2), (0, 1), colspan=1, rowspan=1)
divider1 = make_axes_locatable(ax1); cax1 = divider1.append_axes("right", "5%", pad="3%")
divider2 = make_axes_locatable(ax2); cax2 = divider2.append_axes("right", "5%", pad="3%")

CS1 = ax1.contourf(x, c, num_result1, Alev, cmap=plt.cm.RdBu_r, extend='max')
ax1.contour(x, c, num_result1, Alev, colors=('k', ), linewidths=1)
ax1.set_xlabel('$X = k\Delta x$', fontsize=12); ax1.grid()
ax1.set_ylabel('Courant number ( $\mathrm{C}$ )', fontsize=12); plt.colorbar(CS1, cax=cax1)
ax1.set_title('Root I, Amplification factor', fontsize=14, fontweight='bold')

CS2 = ax2.contourf(x, c, num_result2, Alev, cmap=plt.cm.RdBu_r, extend='max')
ax2.contour(x, c, num_result2, Alev, colors=('k', ), linewidths=1)
ax2.set_xlabel('$X = k\Delta x$', fontsize=12); ax2.grid()
ax2.set_title('Root II, Amplification factor', fontsize=14, fontweight='bold')
plt.colorbar(CS2, cax=cax2)
```

Based on the amplification factor $|A|$ for 2 roots:

* For small Courant number regime, we see $|A| \leq 1$ for any $k\Delta x$ with no significant damping, which agrees with *Warner p69*:
> "These centered-in-space and centered-in-time schemes, ... do not damp for any stable value of the Courant number."

* For big Courant number regime, root I shows unstable $A$, and root II shows significant damping.

* Root I has a more strict requirement for Courant number, and thus **Root I been satisfied means the whole system is stable**.

## Searching Courant numbers for stable system

Since we have highlighted the importance of Root I, so here we only need to satisfy Root I.

```python
index1 = search_stable(num_result1)
index2 = search_stable(num_result2)
bad_kdx1 = X_num[num_result1[np.nanmax(index1)+1, :]>1]
bad_A1 = num_result1[np.nanmax(index1)+1, num_result1[np.nanmax(index1)+1, :]>1]
bad_kdx1 = bad_kdx1[bad_A1==bad_A1.max()]
bad_A1 = bad_A1[bad_A1==bad_A1.max()]
```

```python
print('Criteria of Courant number: C <= {}'.format(C_num[np.nanmax(index1)]))
print('The most unstable kdx: {}'.format(bad_kdx1))
```

    Criteria of Courant number: C <= 0.72864321608
    The most unstable kdx: [ 1.79396985]

We see $C \leq 0.729$ agrees with *Warner p69*.

## Check the amounty of damping

Plot amplification factors against Courant number for $C\ \epsilon\ [0.5, 1.0]$ to see the amount of damping:

```python
fig = plt.figure(figsize = (14, 6))
ax = fig.gca()
ax.plot(C_num, num_result1, 'k-', linewidth=0.5)
ax.plot(C_num, num_result2, 'k-', linewidth=0.5)
ax.plot([0.729, 0.729], [0, 3], color=[0.3, 0.5, 0.7], linewidth=3, label='$C\ \leq\ 0.729$')
legend=ax.legend(shadow=True, fancybox=True, loc=2, fontsize=12);
frame=legend.get_frame(); frame.set_color('white'); ax.set_xlim(0.5, 1); ax.grid()
ax.set_title('Amplification factors v.s. Courant number', fontsize=14, fontweight='bold')
ax.set_xlabel('Courant number', fontsize=12); ax.set_ylabel('Amplification factors', fontsize=12)
```

Check some of the amplification factors in stable case (according to the figure above, and analytical analysis, they are 1.0).

```python
num_result1[~np.isnan(index1), :]
```

    array([[ 1.,  1.,  1., ...,  1.,  1.,  1.],
           [ 1.,  1.,  1., ...,  1.,  1.,  1.],
           [ 1.,  1.,  1., ...,  1.,  1.,  1.],
           ...,
           [ 1.,  1.,  1., ...,  1.,  1.,  1.],
           [ 1.,  1.,  1., ...,  1.,  1.,  1.],
           [ 1.,  1.,  1., ...,  1.,  1.,  1.]])

Thus, no damping for $C \leq 0.729$.

##A quick summary

* We find Courant number $C \leq 0.729$ makes all $k\Delta x$ **stable** and **no damping**. (In the scope of $k\Delta x\ \epsilon\ [0.5, 3]$).
* The answer in *Warner p69* has been verified.
* One of the most unstable wave is $k\Delta x = 1.794$, and there will be more (periodically) for larger $k\Delta x$ since we are play with sin & cos.

# WRF-ARW version of 3rd order Runge-Kutta, 3p centered in space

## Analytical soutions of amplification factor

3rd Runge-Kutta applied in WRF-ARW is:

$$
\begin{align}
T_j^*  &= T_{j, n} + \frac{\Delta t}{3} f(T_{j, n}) \\
T_j^{**}  &= T_{j, n} + \frac{\Delta t}{2} f(T_j^*) \\
T_{j, n+1}  &= T_{j, n} + \Delta t \ f(T_j^{**}) \\
\end{align}
$$

Apply that scheme into 1D advection equation (as section 1 did), with a discretization of 3 points centered in space, we have:

$$
\begin{align}
T_j^*  &= T_{j, n} + \frac{\Delta t}{3}\left[-u_0\frac{T_{j+1, n}-T_{j-1, n}}{2\Delta x}\right] \\
T_j^{**}  &= T_{j, n} + \frac{\Delta t}{2} \left[-u_0\frac{T_{j+1, n}^*-T_{j-1, n}^*}{2\Delta x}\right] \\
T_{j, n+1}  &= T_{j, n} + \Delta t \ \left[-u_0\frac{T_{j+1, n}^{**}-T_{j-1, n}^{**}}{2\Delta x}\right] \\
\end{align}
$$

Simplified the equations above with *Courant number* $C = u_0\frac{\Delta t}{\Delta x}$:

$$
\begin{align}
T_j^*  &= T_{j, n} + \frac{\Delta t}{3}\left[-u_0\frac{T_{j+1, n}-T_{j-1, n}}{2\Delta x}\right] \\
T_{j, n+1}  &= T_{j, n} + \frac{C}{2}\left[ T_{j+1, n}-T_{j-1, n}-\frac{C}{4}T_{j+2, n}^* + \frac{C}{2}T_{j, n}^* - \frac{C}{4}T_{j-2, n}^*\right] \\
\end{align}
$$

Then:

$$
T_{j, n+1} = \left[ \left(1-\frac{C^2}{4}\right)T_j - \left(\frac{C}{2}-\frac{C^3}{16}\right)\left(T_{j+1}-T_{j-1}\right)+\left(\frac{C^2}{8}\right)\left(T_{j+2}+T_{j-2}\right)-\left(\frac{C^3}{48}\left(T_{j+3}-T_{j-3}\right)\right)  \right]_n
$$

Dividing the both sides with $T_{j,n}$:

$$
T_{0, 1} = \left[ \left(1-\frac{C^2}{4}\right)T_0 - \left(\frac{C}{2}-\frac{C^3}{16}\right)\left(T_{1}-T_{-1}\right)+\left(\frac{C^2}{8}\right)\left(T_{2}+T_{-2}\right)-\left(\frac{C^3}{48}\right)\left(T_{3}-T_{-3}\right)  \right]_n
$$

Substituding the eigenmode $T_{j, n} = [A]^n\exp\left[iXj\right]$, $\left(X=k\Delta x\right)$, we get an expression for $A$:

$$
A = \left(1-\frac{C^2}{4}\right)-\left(\frac{C}{2}-\frac{C^3}{16}\right)\left(e^{Xi}-e^{-Xi}\right)+\left(\frac{C^2}{8}\right)\left(e^{2Xi}+e^{-2Xi}\right)-\left(\frac{C^3}{48}\right)\left(e^{3Xi}-e^{-3Xi}\right)
$$

Considering Euler's formula $e^{ix} = \cos x + i\sin x$, the magnitude of $A$ is:

$$
|A| = \sqrt{\left(\frac{C^{3}}{24} \sin{\left (3 X \right )} + \left(- \frac{C^{3}}{8} + C\right) \sin{\left (X \right )}\right)^{2} + \left(\frac{C^{2}}{4} \cos{\left (2 X \right )} - \frac{C^{2}}{4} + 1\right)^{2}}
$$

```python
A, C, X =sp.symbols('A, C, X')
orig = sp.sqrt(((1-C**2/4)+(C**2/4)*sp.cos(2*X))**2 + (-(C-C**3/8)*sp.sin(X)-(C**3/24)*sp.sin(3*X))**2)
```

Substitude in trigonometric identities, and collecting terms.

```python
result = sp.simplify(orig.expand(trig=True))
result
```

We use this expression for von Neumann instability analysis.

## Exploring amplification factor with numerics

As HW10.B required, here I substituding $C\ \epsilon\ [0.1, 3]$ and $k\Delta x\ \epsilon\ [2, 20]$, and then do the following things:
* Plot amplification factors in a frame of Courant numbers and $X=k\Delta x$.
* Find the worst case(s) with corresponds $X$.
* Find the critical value of Courant number.

```python
NUM1 = 400
NUM2 = 400
C_num = np.linspace(0.1, 3.0, NUM1)
X_num = np.linspace(0, 20, NUM2)
x, c = np.meshgrid(X_num, C_num)
select_x = np.array([2, 2.5, 3, 4, 5, 10, 20])
x1, c1 = np.meshgrid(select_x, C_num)
```

```python
num_result = np.empty([NUM1, NUM2]) + 1j*np.empty([NUM1, NUM2])
select_result = np.empty([NUM1, len(select_x)]) + 1j*np.empty([NUM1, len(select_x)])
for i in range(NUM1):
    for m in range(NUM2):
        num_result[i, m] = result.evalf(subs={X:x[i, m], C:c[i, m]})
    for n in range(len(select_x)):
        select_result[i, n] = result.evalf(subs={X:x1[i, n], C:c1[i, n]})
num_result = np.abs(num_result)
select_result = np.abs(select_result)
```

```python
Alev = [0, 0.25, 0.5, 0.75, 1, 1.25, 1.5, 1.75, 2]

fig = plt.figure(figsize=(16, 4))

ax1=plt.subplot2grid((1, 2), (0, 0), colspan=1, rowspan=1)
ax2=plt.subplot2grid((1, 2), (0, 1), colspan=1, rowspan=1)
divider1 = make_axes_locatable(ax1); cax1 = divider1.append_axes("right", "5%", pad="3%")

CS1 = ax1.contourf(x, c, num_result, Alev, cmap=plt.cm.RdBu_r, extend='max')
ax1.contour(x, c, num_result, Alev, colors=('k', ), linewidths=1)
ax1.set_xlabel('$X = k\Delta x$', fontsize=12); ax1.grid()
ax1.set_ylabel('Courant number ($\mathrm{C}$)', fontsize=12); plt.colorbar(CS1, cax=cax1)
ax1.set_title('(a) Amplification factor $\mathrm{|A|}$', fontsize=14, fontweight='bold')

for i in range(len(select_x)):
    ax2.plot(C_num, select_result[:,i], linewidth=2.5, label='$k\Delta x$ = '+str(select_x[i]))
legend=ax2.legend(shadow=True, fancybox=True, loc=2, fontsize=12);
frame=legend.get_frame(); frame.set_color('white')
ax2.grid(); ax2.set_xlim(0.1, 3.0)
ax2.set_xlabel('Courant number ($\mathrm{C}$)', fontsize=12)
ax2.set_ylabel('Amplification factor $\mathrm{|A|}$', fontsize=12)
ax2.set_title('(b) Courant number v.s. Amplification factor', fontweight='bold', fontsize=14)
```

```python
index = search_stable(num_result)
bad_kdx = X_num[num_result[np.nanmax(index)+1, :]>1]
bad_A = num_result[np.nanmax(index)+1, num_result[np.nanmax(index)+1, :]>1]
```

```python
print('Criteria of Courant number: C <= {}'.format(C_num[np.nanmax(index)]))
print('The most unstable kdx: {}'.format(bad_kdx))
```

    Criteria of Courant number: C <= 1.72807017544
    The most unstable kdx: [  1.55388471   1.60401003   4.66165414   4.71177945   4.76190476
       7.81954887   7.86967419  10.97744361  11.02756892  14.08521303
      14.13533835  14.18546366  17.24310777  17.29323308]

We see the most unstable waves distribute like: ~1.5, ~4.7, ~7.8, ~11, ~14.1, 17.3. And 'link' these numbers with $\pi$ (we are playing with sin & cos), we will find:

```python
print(np.pi/2*np.array([1, 3, 5, 7, 9, 11]))
```

    [  1.57079633   4.71238898   7.85398163  10.99557429  14.13716694
      17.27875959]

So the most unstable waves could be:
$$
k\Delta x = n\frac{\pi}{2}, \quad\ n=1, 3, 5, 7, 9, 11, \ldots
$$

##A quick summary

* For WRF-ARW version of 3rd order Runge-Kutta, 3p centered in space. We find Courant number $C \leq 1.728$ makes all $k\Delta x$ stable.
* The most unstable set of $k\Delta x$ could be:
$$
k\Delta x = n\frac{\pi}{2}, \quad\ n=1, 3, 5, 7, 9, \ldots
$$
* According to "courant number v.s. Amplification factor" plot, we see unstable $|A|$ for large Courant number; and we also see damping for all $k\Delta x$, especially $C\ \epsilon\ [1, 1.728]$ for stable Courant number. The damping is smaller than 0.25, so it cannot be identified from contours in Fig.a.

** Comparing with the higher-order stability criteria in *WRF-ARW tech. note***:

Combining the result above with the results in the 'tech. note', we see:

    Time scheme
    2nd spacial
    3rd spacial
    4th spacial
    5th spacial
    6th spacial

    R.-K. 3rd order
    **1.728**
    1.61
    1.26
    1.42
    1.08

We find that 2nd order centered difference in space has the most flexible choice of Courant number, it allows bigger timesteps.


# Kinematic chain in a plane (2D)

> Marcos Duarte
> Laboratory of Biomechanics and Motor Control ([http://demotu.org/](http://demotu.org/))
> Federal University of ABC, Brazil

Kinematic chain refers to an assembly of rigid bodies (links) connected by joints that is the mathematical model for a mechanical system which in turn can represent a biological system such as the human arm ([Wikipedia](http://en.wikipedia.org/wiki/Kinematic_chain)).

The term chain refers to the fact that the links are constrained by their connections (typically, by a hinge joint which is also called pin joint or revolute joint) to other links. As consequence of this constraint, a kinematic chain in a plane is an example of circular motion of a rigid object.

Chapter 15 of Ruina and Rudra's book is a good formal introduction on the topic of circular motion of a rigid object. However, in this notebook we will not employ the mathematical formalism introduced in that chapter - the concept of a rotating reference frame and the related rotation matrix - we will do that in a future notebook when talking about rigid body transformation. For now we will specify the kinematics in a Cartesian coordinate system using trigonometry and calculus. This approach is simpler and more intuitive but it gets too complicated for a kinematic chain with many links or in the 3D space. For such more complicated problems, it would be recommended using rigid transformations (see for example, Siciliano et al. (2009)).

We will deduce the kinematic properties of kinematic chains algebraically using [Sympy](http://sympy.org/), a Python library for symbolic mathematics. With Sympy, we could have used the [mechanics module](http://docs.sympy.org/latest/modules/physics/mechanics/index.html), a specific module for creation of symbolic equations of motion for multibody systems, but let's deduce most of the stuff by ourselves to understand the details.

## Properties of kinematic chains

For a kinematic chain, the base is the extremity (origin) of a kinematic chain which is typically considered attached to the ground, body or fixed. The endpoint is the other extremity (end) of a kinematic chain and typically can move. In robotics, the term end-effector is used and usually refers to a last link (rigid body) in this chain.

In topological terms, a kinematic chain is termed open when there is only one sequence of links connecting the two ends of the chain. Otherwise it's termed closed and in this case a sequence of links forms a loop. A kinematic chain can be classified as serial or parallel or a mixed of both. In a serial chain the links are connected in a serial order. A serial chain is an open chain, otherwise it is a parallel chain or a branched chain (e.g., hand and fingers).

Another important term to characterize a kinematic chain is degree of freedom (DOF). In mechanics, the degree of freedom of a mechanical system is the number of independent parameters that define its configuration or that determine the state of a physical system. A particle in the 3D space has three DOFs because we need three coordinates to specify its position. A rigid body in the 3D space has six DOFs because we need three coordinates of one point at the body to specify its position and three angles to to specify its orientation in order to completely define the configuration of the rigid body. For a link attached to a fixed body by a hinge joint in a plane, all we need to define the configuration of the link is one angle and then this link has only one DOF. A kinematic chain with two links in a plane has two DOFs, and so on.

The mobility of a kinematic chain is its total number of degrees of freedom. The redundancy of a kinematic chain is its mobility minus the number of degrees of freedom of the endpoint.

## The kinematics of one-link system

First, let's study the case of a system composed by one planar hinge joint and one link, which technically it's not a chain but it will be useful to review (or introduce) key concepts.

Figure. One link attached to a fixed body by a hinge joint in a plane.

First, let's import the necessary libraries from Python and its ecosystem:

```python
import numpy as np
import matplotlib.pyplot as plt
%matplotlib inline
import seaborn as sns
sns.set_context("notebook", font_scale=1.2, rc={"lines.linewidth": 2,
                                                "lines.markersize": 10})
from IPython.display import display, Math

from sympy import Symbol, symbols, Function, Matrix, simplify, lambdify, expand, latex
from sympy import diff, cos, sin, sqrt, acos, atan2, atan
from sympy.vector import CoordSys3D
from sympy.physics.mechanics import dynamicsymbols, mlatex, init_vprinting
init_vprinting()

import sys
sys.path.insert(1, r'./../functions')  # add to pythonpath
```

We need to define a Cartesian coordinate system and the symbolic variables, $t$, $\ell$, $\theta$ (and make $\theta$ a function of time):

```python
G = CoordSys3D('')
t = Symbol('t')
l = Symbol('ell', positive=True)
theta = dynamicsymbols('theta')  # or Function('theta')(t)
```

Using trigonometry, the endpoint position in terms of the joint angle and link length is:

```python
r_p = l*cos(theta)*G.i + l*sin(theta)*G.j
r_p
```

With the components:

```python
r_p.components
```

### Forward and inverse kinematics

Computing the configuration of a link or a chain (including the endpoint location) from the joint parameters (joint angles and link lengths) as we have done is called [forward or direct kinematics](https://en.wikipedia.org/wiki/Forward_kinematics).

If the linear coordinates of the endpoint position are known (for example, if they are measured with a motion capture system) and one wants to obtain the joint angle(s), this process is known as [inverse kinematics](https://en.wikipedia.org/wiki/Inverse_kinematics). For the one-link system above:

$$ \theta = arctan\left(\frac{y_P}{x_P}\right) $$

### Matrix representation of the kinematics

The mathematical manipulation will be easier if we use the matrix formalism (and let's drop the explicit dependence on $t$):

```python
r = Matrix((r_p.dot(G.i), r_p.dot(G.j)))
r
```

$$\left[\begin{matrix}\ell \operatorname{cos}\left(\theta\right)\\\ell \operatorname{sin}\left(\theta\right)\end{matrix}\right]$$

We could have used Sympy for switching to matrix representation:

```python
r_p.to_matrix(G)
```

$$\left[\begin{matrix}\ell \operatorname{cos}\left(\theta\right)\\\ell \operatorname{sin}\left(\theta\right)\\0\end{matrix}\right]$$

The third element of the matrix above refers to the $\hat{\mathbf{k}}$ component which is zero for the present case.

## Differential kinematics

Differential kinematics gives the relationship between the joint velocities and the corresponding endpoint linear velocity. This mapping is described by a matrix, termed [Jacobian matrix](http://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant), which depends on the kinematic chain configuration and it is of great use in the study of kinematic chains.
First, let's deduce the endpoint velocity without using the Jacobian and then we will see how to calculate the endpoint velocity using the Jacobian matrix.

The velocity of the endpoint can be obtained by the first-order derivative of the position vector. The derivative of a vector is obtained by differentiating each vector component:

$$
\frac{\mathrm{d}\overrightarrow{\mathbf{r}}}{\mathrm{d}t} =
\large
\begin{bmatrix}
\frac{\mathrm{d}x_P}{\mathrm{d}t} \\
\frac{\mathrm{d}y_P}{\mathrm{d}t} \\
\end{bmatrix}
$$

Note that the derivative is with respect to time but $x_P$ and $y_P$ depend explicitly on $\theta$ and it's $\theta$ that depends on $t$ ($x_P$ and $y_P$ depend implicitly on $t$). To calculate this type of derivative we will use the [chain rule](http://en.wikipedia.org/wiki/Chain_rule).

Chain rule

For variable $f$ which is function of variable $g$ which in turn is function of variable $t$, $f(g(t))$ or $(f\circ g)(t)$, the derivative of $f$  with respect to $t$ is (using Lagrange's notation):

$$(f\circ g)^{'}(t) = f'(g(t)) \cdot g'(t)$$

Or using what is known as Leibniz's notation:

$$\frac{\mathrm{d}f}{\mathrm{d}t} = \frac{\mathrm{d}f}{\mathrm{d}g} \cdot \frac{\mathrm{d}g}{\mathrm{d}t}$$

If $f$ is function of two other variables which both are function of $t$, $ f(x(t),y(t))$, the chain rule for this case is:

$$\frac{\mathrm{d}f}{\mathrm{d}t} = \frac{\partial f}{\partial x} \cdot \frac{\mathrm{d}x}{\mathrm{d}t} + \frac{\partial f}{\partial y} \cdot \frac{\mathrm{d}y}{\mathrm{d}t}$$

Where $df/dt$ represents the total derivative and $\partial f / \partial x$ represents the partial derivative of a function.

Product rule

The derivative of the product of two functions is:

$$ (f \cdot g)' = f' \cdot g + f \cdot g'  $$

### Linear velocity of the endpoint

For the planar one-link case, the linear velocity of the endpoint is:

```python
v = r.diff(t)
v
```

$$\left[\begin{matrix}- \ell \operatorname{sin}\left(\theta\right) \dot{\theta}\\\ell \operatorname{cos}\left(\theta\right) \dot{\theta}\end{matrix}\right]$$

Where we used the [Newton's notation](http://en.wikipedia.org/wiki/Notation_for_differentiation) for differentiation. Note that $\dot{\theta}$ represents the unknown angular velocity of the joint; this is why the derivative of $\theta$ is not explicitly solved.
The magnitude or [Euclidian norm](http://en.wikipedia.org/wiki/Vector_norm) of the vector $\overrightarrow{\mathbf{v}}$ is:

$$ ||\overrightarrow{\mathbf{v}}||=\sqrt{v_x^2+v_y^2} $$

```python
simplify(sqrt(v[0]**2+v[1]**2))
```

Which is $\ell\dot{\theta}$.
We could have used the function norm of Sympy, v.norm(), but the output does not simplify nicely:

```python
simplify(v.norm())
```

The direction of $\overrightarrow{\mathbf{v}}$ is tangent to the circular trajectory of the endpoint as can be seen in the figure below.

Figure. Endpoint velocity of one link attached to a fixed body by a hinge joint in a plane.

### Linear acceleration of the endpoint

The acceleration of the endpoint position can be given by the second-order derivative of the position or by the first-order derivative of the velocity.
Using the chain and product rules for differentiation, the linear acceleration of the endpoint is:

```python
acc = v.diff(t, 1)
acc
```

$$\left[\begin{matrix}- \ell \operatorname{sin}\left(\theta\right) \ddot{\theta} - \ell \operatorname{cos}\left(\theta\right) \dot{\theta}^{2}\\- \ell \operatorname{sin}\left(\theta\right) \dot{\theta}^{2} + \ell \operatorname{cos}\left(\theta\right) \ddot{\theta}\end{matrix}\right]$$

Examining the terms of the expression for the linear acceleration, we see there are two types of them: the term (in each direction) proportional to the angular acceleration $\ddot{\theta}$ and other term proportional to the square of the angular velocity $\dot{\theta}^{2}$.

#### Tangential acceleration

The term proportional to angular acceleration, $a_t$, is always tangent to the trajectory of the endpoint (see figure below) and it's magnitude or Euclidean norm is:

```python
A = theta.diff(t, 2)
simplify(sqrt(expand(acc[0]).coeff(A)**2 + expand(acc[1]).coeff(A)**2))*A
```

#### Centripetal acceleration

The term proportional to angular velocity, $a_c$, always points to the joint, the center of the circular motion (see figure below), because of that this term is termed [centripetal acceleration](http://en.wikipedia.org/wiki/Centripetal_acceleration#Tangential_and_centripetal_acceleration). Its magnitude is:

```python
A = theta.diff(t)**2
simplify(sqrt(expand(acc[0]).coeff(A)**2+expand(acc[1]).coeff(A)**2))*A
```

This means that there will be a linear acceleration even if the angular acceleration is zero because although the magnitude of the linear velocity is constant in this case, its direction varies (due to the centripetal acceleration).

Figure. Endpoint tangential and centripetal acceleration terms of one link attached to a fixed body by a hinge joint in a plane.

Let's plot some simulated data to have an idea of the one-link kinematics. Consider $\ell=1\:m,\theta_0=0^o,\theta_f=90^o $, and $1\:s$ of movement duration, and that it is a [minimum-jerk movement](http://nbviewer.ipython.org/github/demotu/BMC/blob/master/notebooks/MinimumJerkHypothesis.ipynb).

```python
theta0, thetaf, d = 0, np.pi/2, 1
ts = np.arange(0.01, 1.01, .01)
mjt  = theta0 + (thetaf - theta0)*(10*(t/d)**3 - 15*(t/d)**4 + 6*(t/d)**5)

ang  = lambdify(t, mjt, 'numpy'); ang = ang(ts)
vang = lambdify(t, mjt.diff(t,1), 'numpy'); vang = vang(ts)
aang = lambdify(t, mjt.diff(t,2), 'numpy'); aang = aang(ts)
jang = lambdify(t, mjt.diff(t,3), 'numpy'); jang = jang(ts)

b, c, d, e = symbols('b c d e')
dicti = {l:1, theta:b, theta.diff(t, 1):c, theta.diff(t, 2):d, theta.diff(t, 3):e}

r2 = r.subs(dicti);
rxfu = lambdify(b, r2[0], modules = 'numpy')
ryfu = lambdify(b, r2[1], modules = 'numpy')

v2 = v.subs(dicti);
vxfu = lambdify((b, c), v2[0], modules = 'numpy')
vyfu = lambdify((b, c), v2[1], modules = 'numpy')

acc2 = acc.subs(dicti);
axfu = lambdify((b, c, d), acc2[0], modules = 'numpy')
ayfu = lambdify((b, c, d), acc2[1], modules = 'numpy')

jerk = r.diff(t,3)
jerk2 = jerk.subs(dicti);
jxfu = lambdify((b, c, d, e), jerk2[0], modules = 'numpy')
jyfu = lambdify((b, c, d, e), jerk2[1], modules = 'numpy')
```

```python
fig, hax = plt.subplots(2, 4, sharex = True, figsize=(14, 7))
hax[0, 0].plot(ts,ang*180/np.pi, linewidth=3)
hax[0, 0].set_title('Angular displacement [ $^o$]');
hax[0, 0].set_ylabel('Joint')
hax[0, 1].plot(ts,vang*180/np.pi, linewidth=3)
hax[0, 1].set_title('Angular velocity [ $^o/s$]');
hax[0, 2].plot(ts,aang*180/np.pi, linewidth=3)
hax[0, 2].set_title('Angular acceleration [ $^o/s^2$]');
hax[0, 3].plot(ts,jang*180/np.pi, linewidth=3)
hax[0, 3].set_title('Angular jerk [ $^o/s^3$]');
hax[1, 0].plot(ts,rxfu(ang), 'r', linewidth=3, label = 'x')
hax[1, 0].plot(ts,ryfu(ang), 'k', linewidth=3, label = 'y')
hax[1, 0].set_xlabel('Time [s]');
hax[1, 0].set_title('Linear displacement [$m$]');
hax[1, 0].legend(loc='best').get_frame().set_alpha(0.8)
hax[1, 0].set_ylabel('Endpoint')
hax[1, 1].plot(ts,vxfu(ang,vang), 'r', linewidth=3)
hax[1, 1].plot(ts,vyfu(ang,vang), 'k', linewidth=3)
hax[1, 1].set_xlabel('Time [s]');
hax[1, 1].set_title('Linear velocity [$m/s$]');
hax[1, 2].plot(ts,axfu(ang,vang,aang), 'r', linewidth=3)
hax[1, 2].plot(ts,ayfu(ang,vang,aang), 'k', linewidth=3)
hax[1, 2].set_xlabel('Time [s]');
hax[1, 2].set_title('Linear acceleration [$m/s^2$]');
hax[1, 3].plot(ts,jxfu(ang,vang,aang,jang), 'r', linewidth=3)
hax[1, 3].plot(ts,jyfu(ang,vang,aang,jang), 'k', linewidth=3)
hax[1, 3].set_xlabel('Time [s]');
hax[1, 3].set_title('Linear jerk [$m/s^2$]');
fig.suptitle('Minimum jerk trajectory kinematics of one-link system', fontsize=20);
for hax2 in hax.flat:
        hax2.locator_params(nbins=5)
        hax2.grid(True)
plt.subplots_adjust(hspace=0.15) #plt.tight_layout()
```

### Jacobian matrix

The [Jacobian matrix](http://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant) is the matrix of all first-order partial derivatives of a vector-valued function.

In the context of kinematic chains, the Jacobian is a matrix of all first-order partial derivatives of the linear position vector of the endpoint with respect to the angular position vector.

The Jacobian matrix for a kinematic chain relates differential changes in the joint angle vector with the resulting differential changes in the linear position vector of the endpoint.

In a general form, the Jacobian matrix is:

$$
\mathbf{J}=
\large
\begin{bmatrix}
\frac{\partial F_{1}}{\partial q_{1}} & ... & \frac{\partial F_{1}}{\partial q_{n}} \\
\vdots  & \ddots  & \vdots \\
\frac{\partial F_{m}}{\partial q_{1}} & ... & \frac{\partial F_{m}}{\partial q_{n}} \\
\end{bmatrix}
$$

For a kinematic chain, the function $F_{i}$ is the expression of the endpoint position in $m$ Cartesian coordinates and the variable $q_{i}$ is the angle of each $n$ joints.

#### Jacobian matrix of one-link chain

For the planar one-link case, the Jacobian matrix is:

$$
\mathbf{J}=
\large
\begin{bmatrix}
\frac{\partial \:x_P}{\partial \theta} \\
\frac{\partial \:y_P}{\partial \theta} \\
\end{bmatrix}
$$

Which calculates to:

```python
J = r.diff(theta)
J
```

$$\left[\begin{matrix}- \ell \operatorname{sin}\left(\theta\right)\\\ell \operatorname{cos}\left(\theta\right)\end{matrix}\right]$$

And Sympy has a function to calculate the Jacobian:

```python
J = r.jacobian([theta])
J
```

$$\left[\begin{matrix}- \ell \operatorname{sin}\left(\theta\right)\\\ell \operatorname{cos}\left(\theta\right)\end{matrix}\right]$$

We can recalculate the kinematic expressions using the Jacobian matrix, which can be useful for simplifying the deduction.

The linear velocity of the end-effector is given by the product between the Jacobian of the kinematic link and the angular velocity:

$$ \overrightarrow{\mathbf{v}} = \mathbf{J} \cdot \overrightarrow{\mathbf{\omega}} $$

Where:

```python
w = Matrix([theta.diff(t)])
w
```

$$\left[\begin{matrix}\dot{\theta}\end{matrix}\right]$$

The angular velocity is also a vector; it's direction is perpendicular to the plane of rotation and using the [right-hand rule](http://en.wikipedia.org/wiki/Right-hand_rule) this direction is the same as of the versor $\hat{\mathbf{k}}$ coming out of the screen (paper).

Then:

```python
velJ = J*w
velJ
```

$$\left[\begin{matrix}- \ell \operatorname{sin}\left(\theta\right) \dot{\theta}\\\ell \operatorname{cos}\left(\theta\right) \dot{\theta}\end{matrix}\right]$$

And the linear acceleration of the endpoint is given by the derivative of this product:

$$ \overrightarrow{\mathbf{a}} = \dot{\mathbf{J}} \cdot \overrightarrow{\mathbf{\omega}} + \mathbf{J} \cdot \dot{\overrightarrow{\mathbf{\omega}}} $$

Let's calculate this derivative:

```python
accJ = J.diff(t)*w + J*w.diff(t)
accJ
```

$$\left[\begin{matrix}- \ell \operatorname{sin}\left(\theta\right) \ddot{\theta} - \ell \operatorname{cos}\left(\theta\right) \dot{\theta}^{2}\\- \ell \operatorname{sin}\left(\theta\right) \dot{\theta}^{2} + \ell \operatorname{cos}\left(\theta\right) \ddot{\theta}\end{matrix}\right]$$

These two expressions derived with the Jacobian are the same as the direct derivatives of the equation for the endpoint position.

## The kinematics of a two-link chain

We now will look at the case of a planar kinematic chain with two links, as shown below. The deduction will be similar to the case with one link we just saw.

Figure. Kinematics of a two-link chain with hinge joints in a plane.

We need to define a Cartesian coordinate system and the symbolic variables $t,\:\ell_1,\:\ell_2,\:\theta_1,\:\theta_2$ (and make $\theta_1$ and $\theta_2$ function of time):

```python
O = CoordSys3D('')
t = Symbol('t')
l1, l2 = symbols('ell_1 ell_2', positive=True)
theta1, theta2 = dynamicsymbols('theta1 theta2')
```

The position of the endpoint in terms of the joint angles and link lengths is:

```python
r2_p = (l1*cos(theta1) + l2*cos(theta1 + theta2))*O.i + (l1*sin(theta1) + l2*sin(theta1 + theta2))*O.j
r2_p
```

With the components:

```python
r2_p.components
```

And in matrix form:

```python
r2 = Matrix((r2_p.dot(O.i), r2_p.dot(O.j)))
r2
```

$$\left[\begin{matrix}\ell_{1} \operatorname{cos}\left(\theta_{1}\right) + \ell_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right)\\\ell_{1} \operatorname{sin}\left(\theta_{1}\right) + \ell_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right)\end{matrix}\right]$$

### Joint and segment angles

Note that $\theta_2$ is a joint angle (referred as measured in the joint space); the angle of the segment 2 with respect to the horizontal is $\theta_1+\theta_2$ and is referred as an angle in the segmental space. A joint and segment angles are also referred as relative and absolute angles, respectively.

### Inverse kinematics

Using the [cosine rule](http://en.wikipedia.org/wiki/Law_of_cosines), in terms of the endpoint position, the angle $\theta_2$ is:

$$ x_P^2 + y_P^2 = \ell_1^2+\ell_2^2 - 2\ell_1 \ell_2 cos(\pi-\theta_2) $$

$$ \theta_2 = arccos\left(\frac{x_P^2 + y_P^2 - \ell_1^2 - \ell_2^2}{2\ell_1 \ell_2}\;\;\right) $$

To find the angle $\theta_1$, if we now look at the triangle in red in the figure below, its angle $\phi$ is:

$$\phi = arctan\left(\frac{\ell_2 sin(\theta_2)}{\ell_1 + \ell_2 cos(\theta_2)}\right) $$

And the angle of its hypotenuse with the horizontal is:

$$ \theta_1 + \phi = arctan\left(\frac{y_P}{x_P}\right) $$

Then, the angle $\theta_1$ is:

$$ \theta_1 = arctan\left(\frac{y_P}{x_P}\right) - arctan\left(\frac{\ell_2 sin(\theta_2)}{\ell_1+\ell_2 cos(\theta_2)}\right) $$

Note that there are two possible sets of $(\theta_1, \theta_2)$ angles for the same $(x_P, y_P)$ coordinate that satisfy the equations above. The figure below shows in orange another possible configuration of the kinematic chain with the same endpoint coordinate. The other solution is $\theta_2'=2\pi - \theta_2$, but $sin(\theta_2')=-sin(\theta_{2})$ and then the $arctan()$ term in the last equation becomes negative.
Even for a simple two-link chain we already have a problem of redundancy, there is more than one joint configuration for the same endpoint position; this will be much more problematic for chains with more links (more degrees of freedom).

Figure. Indetermination in the inverse kinematics approach to determine one of the joint angles for a two-link chain with hinge joints in a plane.

## Differential kinematics

The linear velocity of the endpoint is:

```python
vel2 = r2.diff(t)
vel2
```

$$\left[\begin{matrix}- \ell_{1} \operatorname{sin}\left(\theta_{1}\right) \dot{\theta}_{1} - \ell_{2} \left(\dot{\theta}_{1} + \dot{\theta}_{2}\right) \operatorname{sin}\left(\theta_{1} + \theta_{2}\right)\\\ell_{1} \operatorname{cos}\left(\theta_{1}\right) \dot{\theta}_{1} + \ell_{2} \left(\dot{\theta}_{1} + \dot{\theta}_{2}\right) \operatorname{cos}\left(\theta_{1} + \theta_{2}\right)\end{matrix}\right]$$

The linear velocity of the endpoint is the sum of the velocities at each joint, i.e., it is the velocity of the endpoint in relation to joint 2, for instance, $\ell_2cos(\theta_1 + \theta_2)\dot{\theta}_1$, plus the velocity of joint 2 in relation to joint 1, for instance, $\ell_1\dot{\theta}_1 cos(\theta_1)$, and this last term we already saw for the one-link example. In classical mechanics this is known as [relative velocity](http://en.wikipedia.org/wiki/Relative_velocity), an example of [Galilean transformation](http://en.wikipedia.org/wiki/Galilean_transformation).

The linear acceleration of the endpoint is:

```python
acc2 = r2.diff(t, 2)
acc2
```

$$\left[\begin{matrix}- (\ell_{1} \operatorname{sin}\left(\theta_{1}\right) \ddot{\theta}_{1} + \ell_{1} \operatorname{cos}\left(\theta_{1}\right) \dot{\theta}_{1}^{2} + \ell_{2} \dot{\theta}_{1} + \dot{\theta}_{2}^{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) + \ell_{2} \left(\ddot{\theta}_{1} + \ddot{\theta}_{2}\right) \operatorname{sin}\left(\theta_{1} + \theta_{2}\right))\\- \ell_{1} \operatorname{sin}\left(\theta_{1}\right) \dot{\theta}_{1}^{2} + \ell_{1} \operatorname{cos}\left(\theta_{1}\right) \ddot{\theta}_{1} - \ell_{2} \dot{\theta}_{1} + \dot{\theta}_{2}^{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) + \ell_{2} \left(\ddot{\theta}_{1} + \ddot{\theta}_{2}\right) \operatorname{cos}\left(\theta_{1} + \theta_{2}\right)\end{matrix}\right]$$

We can separate the equation above for the linear acceleration in three types of terms: proportional to $\ddot{\theta}$ and to $\dot{\theta}^2$, as we already saw for the one-link case, and a new term, proportional to $\dot{\theta}_1\dot{\theta}_2$:

```python
acc2 = acc2.expand()
A = theta1.diff(t, 2)
B = theta2.diff(t, 2)
tg = A*Matrix((acc2[0].coeff(A),acc2[1].coeff(A)))+B*Matrix((acc2[0].coeff(B),acc2[1].coeff(B)))

A = theta1.diff(t)**2
B = theta2.diff(t)**2
ct = A*Matrix((acc2[0].coeff(A),acc2[1].coeff(A)))+B*Matrix((acc2[0].coeff(B),acc2[1].coeff(B)))

A = theta1.diff(t)*theta2.diff(t)
co = A*Matrix((acc2[0].coeff(A),acc2[1].coeff(A)))

display(Math(mlatex(r'Tangential:\:') + mlatex(tg)))
display(Math(mlatex(r'Centripetal:') + mlatex(ct)))
display(Math(mlatex(r'Coriolis:\;\;\;\;\:') + mlatex(co)))
```

$$Tangential:\:\left[\begin{matrix}- \ell_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) \ddot{\theta}_{2} + \left(- \ell_{1} \operatorname{sin}\left(\theta_{1}\right) - \ell_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right)\right) \ddot{\theta}_{1}\\\ell_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) \ddot{\theta}_{2} + \left(\ell_{1} \operatorname{cos}\left(\theta_{1}\right) + \ell_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right)\right) \ddot{\theta}_{1}\end{matrix}\right]$$

$$Centripetal:\left[\begin{matrix}- \ell_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) \dot{\theta}_{2}^{2} + \left(- \ell_{1} \operatorname{cos}\left(\theta_{1}\right) - \ell_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right)\right) \dot{\theta}_{1}^{2}\\- \ell_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) \dot{\theta}_{2}^{2} + \left(- \ell_{1} \operatorname{sin}\left(\theta_{1}\right) - \ell_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right)\right) \dot{\theta}_{1}^{2}\end{matrix}\right]$$

$$Coriolis:\;\;\;\;\:\left[\begin{matrix}- 2 \ell_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) \dot{\theta}_{1} \dot{\theta}_{2}\\- 2 \ell_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) \dot{\theta}_{1} \dot{\theta}_{2}\end{matrix}\right]$$

This new term is called the [Coriolis acceleration](http://en.wikipedia.org/wiki/Coriolis_effect); it is 'felt' by the endpoint when its distance to the instantaneous center of rotation varies, due to the links' constraints, and as consequence the endpoint motion is deflected (its direction is perpendicular to the relative linear velocity of the endpoint with respect to the linear velocity at the second joint, $\mathbf{v} - \mathbf{v}_{joint2}$.

Let's now deduce the Jacobian for this planar two-link chain:

$$
\mathbf{J} =
\large
\begin{bmatrix}
\frac{\partial x_P}{\partial \theta_{1}} & \frac{\partial x_P}{\partial \theta_{2}} \\
\frac{\partial y_P}{\partial \theta_{1}} & \frac{\partial y_P}{\partial \theta_{2}} \\
\end{bmatrix}
$$

We could manually run:
```python
J = Matrix([[r2[0].diff(theta1), r2[0].diff(theta2)], [r2[1].diff(theta1), r2[1].diff(theta2)]])
```
But it's shorter with the Sympy Jacobian function:

```python
J2 = r2.jacobian([theta1, theta2])
J2
```

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

Using the Jacobian, the linear velocity of the endpoint is:

$$ \mathbf{v_J} = \mathbf{J} \cdot \mathbf{\omega} $$

Where:

```python
w2 = Matrix((theta1, theta2)).diff(t)
w2
```

$$\left[\begin{matrix}\dot{\theta}_{1}\\\dot{\theta}_{2}\end{matrix}\right]$$

Then:

```python
vel2J = J2*w2
vel2J
```

$$\left[\begin{matrix}- \ell_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) \dot{\theta}_{2} + \left(- \ell_{1} \operatorname{sin}\left(\theta_{1}\right) - \ell_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right)\right) \dot{\theta}_{1}\\\ell_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) \dot{\theta}_{2} + \left(\ell_{1} \operatorname{cos}\left(\theta_{1}\right) + \ell_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right)\right) \dot{\theta}_{1}\end{matrix}\right]$$

This expression derived with the Jacobian is the same as the first-order derivative  of the equation for the endpoint position. We can show this equality by comparing the two expressions with Sympy:

```python
vel2.expand() == vel2J.expand()
```

    True

Once again, the linear acceleration of the endpoint is given by the derivative of the product between the Jacobian and the angular velocity:

$$ \mathbf{a} = \dot{\mathbf{J}} \cdot \mathbf{\omega} + \mathbf{J} \cdot \dot{\mathbf{\omega}} $$

Let's calculate this derivative:

```python
acc2J = J2.diff(t)*w2 + J2*w2.diff(t)
acc2J
```

$$\left[\begin{matrix}- \ell_{2} \left(\dot{\theta}_{1} + \dot{\theta}_{2}\right) \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) \dot{\theta}_{2} - \ell_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) \ddot{\theta}_{2} + \left(- \ell_{1} \operatorname{sin}\left(\theta_{1}\right) - \ell_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right)\right) \ddot{\theta}_{1} + \left(- \ell_{1} \operatorname{cos}\left(\theta_{1}\right) \dot{\theta}_{1} - \ell_{2} \left(\dot{\theta}_{1} + \dot{\theta}_{2}\right) \operatorname{cos}\left(\theta_{1} + \theta_{2}\right)\right) \dot{\theta}_{1}\\- \ell_{2} \left(\dot{\theta}_{1} + \dot{\theta}_{2}\right) \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) \dot{\theta}_{2} + \ell_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) \ddot{\theta}_{2} + \left(\ell_{1} \operatorname{cos}\left(\theta_{1}\right) + \ell_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right)\right) \ddot{\theta}_{1} + \left(- \ell_{1} \operatorname{sin}\left(\theta_{1}\right) \dot{\theta}_{1} - \ell_{2} \left(\dot{\theta}_{1} + \dot{\theta}_{2}\right) \operatorname{sin}\left(\theta_{1} + \theta_{2}\right)\right) \dot{\theta}_{1}\end{matrix}\right]$$

Once again, the expression above is the same as the second-order derivative of the equation for the endpoint position:

```python
acc2.expand() == acc2J.expand()
```

    True

Let's plot some simulated data to have an idea of the two-link kinematics.
Consider $\ell_1=\ell_2=0.5m, \theta_1(0)=\theta_2(0)=0$, 1 s of movement duration, $\theta_1(1)=\theta_2(1)=90^o$, and that it is a [minimum-jerk movement](http://nbviewer.ipython.org/github/demotu/BMC/blob/master/notebooks/MinimumJerkHypothesis.ipynb) of the endpoint.

First, the simulated trajectories:

```python
t, p0, pf, d, rx, ry = symbols('t p0 pf d rx ry')

# minimum jerk kinematics
mjt = p0 + (pf - p0)*(10*(t/d)**3 - 15*(t/d)**4 + 6*(t/d)**5)
rfu = lambdify((t, p0, pf, d), mjt, 'numpy')
vfu = lambdify((t, p0, pf, d), diff(mjt, t, 1), 'numpy')
afu = lambdify((t, p0, pf, d), diff(mjt, t, 2), 'numpy')
jfu = lambdify((t, p0, pf, d), diff(mjt, t, 3), 'numpy')

#initial values:
p0, pf, d, L1, L2 = [1, 0], [0.5, .5], 1, .5, .5
ts = np.arange(0.01, 1.01, .01)

#endpoint kinematics
x  = rfu(ts, p0[0], pf[0], d)
y  = rfu(ts, p0[1], pf[1], d)
vx = vfu(ts, p0[0], pf[0], d)
vy = vfu(ts, p0[1], pf[1], d)
ax = afu(ts, p0[0], pf[0], d)
ay = afu(ts, p0[1], pf[1], d)
jx = jfu(ts, p0[0], pf[0], d)
jy = jfu(ts, p0[1], pf[1], d)

#inverse kinematics
ang2b = np.arccos((x**2 + y**2 - L1**2 - L2**2)/(2*L1*L2))
ang1b = np.arctan2(y, x) - (np.arctan2(L2*np.sin(ang2b), (L1+L2*np.cos(ang2b))))

ang2 = acos((rx**2 + ry**2 - l1**2 - l2**2)/(2*l1*l2))
ang2fu = lambdify((rx ,ry, l1, l2), ang2, 'numpy');
ang2 = ang2fu(x, y, L1, L2)
ang1 = atan2(ry, rx) - (atan(l2*sin(acos((rx**2 + ry**2 - l1**2 - l2**2)/(2*l1*l2)))/ \
                            (l1+l2*cos(acos((rx**2 + ry**2 - l1**2 - l2**2)/(2*l1*l2))))))
ang1fu = lambdify((rx, ry, l1, l2), ang1, 'numpy');
ang1 = ang1fu(x, y, L1, L2)

rx = rx(t)
ry = ry(t)
ang2b = acos((rx**2 + ry**2 - l1**2 - l2**2)/(2*l1*l2))
ang1b = atan2(ry, rx) - (atan(l2*sin(acos((rx**2 + ry**2 - l1**2 - l2**2)/(2*l1*l2)))/ \
                            (l1 + l2*cos(acos((rx**2 + ry**2-l1**2 - l2**2)/(2*l1*l2))))))
X, Y, Xd, Yd, Xdd, Ydd, Xddd, Yddd = symbols('X Y Xd Yd Xdd Ydd Xddd Yddd')
dicti = {rx:X, ry:Y, rx.diff(t, 1):Xd, ry.diff(t, 1):Yd, \
        rx.diff(t, 2):Xdd, ry.diff(t, 2):Ydd, rx.diff(t, 3):Xddd, ry.diff(t, 3):Yddd, l1:L1, l2:L2}
vang1 = diff(ang1b, t, 1)
vang1 = vang1.subs(dicti)
vang1fu = lambdify((X, Y, Xd, Yd, l1, l2), vang1, 'numpy')
vang1 = vang1fu(x, y, vx, vy, L1, L2)
vang2 = diff(ang2b, t, 1)
vang2 = vang2.subs(dicti)
vang2fu = lambdify((X, Y, Xd, Yd, l1, l2), vang2, 'numpy')
vang2 = vang2fu(x, y, vx, vy, L1, L2)

aang1 = diff(ang1b, t, 2)
aang1 = aang1.subs(dicti)
aang1fu = lambdify((X, Y, Xd, Yd, Xdd, Ydd, l1, l2), aang1, 'numpy')
aang1 = aang1fu(x, y, vx, vy, ax, ay, L1, L2)
aang2 = diff(ang2b, t, 2)
aang2 = aang2.subs(dicti)
aang2fu = lambdify((X, Y, Xd, Yd, Xdd, Ydd, l1, l2), aang2, 'numpy')
aang2 = aang2fu(x, y, vx, vy, ax, ay, L1, L2)

jang1 = diff(ang1b, t, 3)
jang1 = jang1.subs(dicti)
jang1fu = lambdify((X, Y, Xd, Yd, Xdd, Ydd, Xddd, Yddd, l1, l2), jang1, 'numpy')
jang1 = jang1fu(x, y, vx, vy, ax, ay, jx, jy, L1, L2)
jang2 = diff(ang2b, t, 3)
jang2 = jang2.subs(dicti)
jang2fu = lambdify((X, Y, Xd, Yd, Xdd, Ydd, Xddd, Yddd, l1, l2), jang2, 'numpy')
jang2 = jang2fu(x, y, vx, vy, ax, ay, jx, jy, L1, L2)
```

And the plots for the trajectories:

```python
fig, hax = plt.subplots(2, 4, sharex = True, figsize=(14, 7))
hax[0, 0].plot(ts, x, 'r', linewidth=3, label = 'x')
hax[0, 0].plot(ts, y, 'k', linewidth=3, label = 'y')
hax[0, 0].set_title('Linear displacement [$m$]')
hax[0, 0].legend(loc='best').get_frame().set_alpha(0.8)
hax[0, 0].set_ylabel('Endpoint')
hax[0, 1].plot(ts, vx, 'r', linewidth=3)
hax[0, 1].plot(ts, vy, 'k', linewidth=3)
hax[0, 1].set_title('Linear velocity [$m/s$]')
hax[0, 2].plot(ts, ax, 'r', linewidth=3)
hax[0, 2].plot(ts, ay, 'k', linewidth=3)
hax[0, 2].set_title('Linear acceleration [$m/s^2$]')
hax[0, 3].plot(ts, jx, 'r', linewidth=3)
hax[0, 3].plot(ts, jy, 'k', linewidth=3)
hax[0, 3].set_title('Linear jerk [$m/s^3$]')
hax[1, 0].plot(ts,ang1*180/np.pi, 'b', linewidth=3, label = 'Ang1')
hax[1, 0].plot(ts,ang2*180/np.pi, 'g', linewidth=3, label = 'Ang2')
hax[1, 0].set_xlabel('Time [$s$]')
hax[1, 0].set_title('Angular displacement [ $^o$]')
hax[1, 0].legend(loc='best').get_frame().set_alpha(0.8)
hax[1, 0].set_ylabel('Joint')
hax[1, 1].plot(ts,vang1*180/np.pi, 'b', linewidth=3)
hax[1, 1].plot(ts,vang2*180/np.pi, 'g', linewidth=3)
hax[1, 1].set_xlabel('Time [$s$]')
hax[1, 1].set_title('Angular velocity [ $^o/s$]')
hax[1, 2].plot(ts,aang1*180/np.pi, 'b', linewidth=3)
hax[1, 2].plot(ts,aang2*180/np.pi, 'g', linewidth=3)
hax[1, 2].set_xlabel('Time [$s$]')
hax[1, 2].set_title('Angular acceleration [ $^o/s^2$]')
hax[1, 3].plot(ts,jang1*180/np.pi, 'b', linewidth=3)
hax[1, 3].plot(ts,jang2*180/np.pi, 'g', linewidth=3)
hax[1, 3].set_xlabel('Time [$s$]')
hax[1, 3].set_title('Angular jerk [ $^o/s^3$]')
tit = fig.suptitle('Minimum jerk trajectory kinematics of a two-link chain', fontsize=20)
for hax2 in hax.flat:
    hax2.locator_params(nbins=5)
    hax2.grid(True)
plt.subplots_adjust(hspace=0.15, wspace=0.25) #plt.tight_layout()

fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(12, 3))
ax1.plot(x, y, 'r', linewidth=3)
ax1.set_xlabel('Displacement in x [$m$]')
ax1.set_ylabel('Displacement in y [$m$]')
ax1.set_title('Endpoint space', fontsize=14)
ax1.grid(True)
ax2.plot(ang1*180/np.pi, ang2*180/np.pi, 'b', linewidth=3)
ax2.set_xlabel('Displacement in joint 1 [ $^o$]')
ax2.set_ylabel('Displacement in joint 2 [ $^o$]')
ax2.set_title('Joint sapace', fontsize=14)
plt.subplots_adjust(left=0.3, wspace=0.3)
ax2.grid(True)
```

## Problems

1. For the numerical example of the two-link chain plotted above, calculate and plot the values for the each type of acceleration (tangential, centripetal and Coriolis). See solution below.

2. For the two-link chain, calculate and interpret the Jacobian and the expressions for the position, velocity, and acceleration of the endpoint for the following cases:
 a) When the first joint (the joint at the base) is fixed at $0^o$.
 b) When the second joint is fixed at $0^o$.

3. For the two-link chain, a special case of movement occurs when the endpoint moves along a line passing through the first joint (the joint at the base). A system with this behavior is known as a polar manipulator (Mussa-Ivaldi, 1986). For simplicity, consider that the lengths of the two links are equal to $\ell$. In this case, the two joint angles are related by: $2\theta_1+\theta_2=\pi$.
 a) Calculate the Jacobian for this polar manipulator and compare it with the Jacobian for the  standard two-link chain. Note the difference between the off-diagonal terms.
 b) Calculate the expressions for the endpoint position, velocity, and acceleration.
 c) For the endpoint acceleration of the polar manipulator, identify the tangential, centrifugal, and Coriolis components and compare them with the expressions for the standard two-link chain.

4. Deduce the equations for the kinematics of a two-link pendulum with the angles in relation to the vertical.

5. Deduce the equations for the kinematics of a two-link system using segment angles and compare with the deduction employing joint angles.

6. Calculate the Jacobian matrix for the following function:
$$
f(x, y) = \begin{bmatrix}
x^2 y \\
5 x + \sin y \end{bmatrix}
$$

#### Calculation of each type of acceleration of the endpoint for the numerical example of the two-link system

```python
# tangential acceleration
A1, A2, A1d, A2d, A1dd, A2dd = symbols('A1 A2 A1d A2d A1dd A2dd')
dicti = {theta1:A1, theta2:A2, theta1.diff(t,1):A1d, theta2.diff(t,1):A2d, \
         theta1.diff(t,2):A1dd, theta2.diff(t,2):A2dd, l1:L1, l2:L2}
tg2 = tg.subs(dicti)
tg2fu = lambdify((A1, A2, A1dd, A2dd), tg2, 'numpy');
tg2n = tg2fu(ang1, ang2, aang1, aang2)
tg2n = tg2n.reshape((2, 100)).T
# centripetal acceleration
ct2 = ct.subs(dicti)
ct2fu = lambdify((A1, A2, A1d, A2d), ct2, 'numpy');
ct2n = ct2fu(ang1, ang2, vang1, vang2)
ct2n = ct2n.reshape((2, 100)).T
# coriolis acceleration
co2 = co.subs(dicti)
co2fu = lambdify((A1, A2, A1d, A2d), co2, 'numpy');
co2n = co2fu(ang1, ang2, vang1, vang2)
co2n = co2n.reshape((2, 100)).T
# total acceleration (it has to be the same calculated before)
acc_tot = tg2n + ct2n + co2n
```

#### And the corresponding plots

```python
fig, hax = plt.subplots(1, 3, sharex = True, sharey = True, figsize=(12, 5))
hax[0].plot(ts, acc_tot[:, 0], color=(1,0,0,.3), linewidth=5, label = 'x total')
hax[0].plot(ts, acc_tot[:, 1], color=(0,0,0,.3), linewidth=5, label = 'y total')
hax[0].plot(ts, tg2n[:, 0], 'r', linewidth=2, label = 'x')
hax[0].plot(ts, tg2n[:, 1], 'k', linewidth=2, label = 'y')
hax[0].set_title('Tangential')
hax[0].set_ylabel('Endpoint acceleration [$m/s^2$]')
hax[0].set_xlabel('Time [$s$]')
hax[1].plot(ts, acc_tot[:, 0], color=(1,0,0,.3), linewidth=5, label = 'x total')
hax[1].plot(ts, acc_tot[:, 1], color=(0,0,0,.3), linewidth=5, label = 'y total')
hax[1].plot(ts, ct2n[:, 0], 'r', linewidth=2, label = 'x')
hax[1].plot(ts, ct2n[:, 1], 'k', linewidth=2, label = 'y')
hax[1].set_title('Centripetal')
hax[1].set_xlabel('Time [$s$]')
hax[1].legend(loc='best').get_frame().set_alpha(0.8)
hax[2].plot(ts, acc_tot[:, 0], color=(1,0,0,.3), linewidth=5, label = 'x total')
hax[2].plot(ts, acc_tot[:, 1], color=(0,0,0,.3), linewidth=5, label = 'y total')
hax[2].plot(ts, co2n[:, 0], 'r', linewidth=2, label = 'x')
hax[2].plot(ts, co2n[:, 1], 'k', linewidth=2, label = 'y')
hax[2].set_title('Coriolis')
hax[2].set_xlabel('Time [$s$]')
tit = fig.suptitle('Acceleration terms for the minimum jerk trajectory of a two-link chain', fontsize=16)
for hax2 in hax:
    hax2.locator_params(nbins=5)
    hax2.grid(True)
# plt.subplots_adjust(hspace=0.15, wspace=0.25) #plt.tight_layout()
```

## References

- Mussa-Ivaldi FA (1986) Compliance.  In: Morasso P Tagliasco V (eds), [Human Movement Understanding: from computational geometry to artificial Intelligence](http://books.google.com.br/books?id=ZlZyLKNoAtEC). North-Holland, Amsterdam.
- Ruina A, Rudra P (2015) [Introduction to Statics and Dynamics](http://ruina.tam.cornell.edu/Book/index.html). Oxford University Press.
- Siciliano B et al. (2009) [Robotics - Modelling, Planning and Control](http://books.google.com.br/books/about/Robotics.html?hl=pt-BR&id=jPCAFmE-logC). Springer-Verlag London.
- Zatsiorsky VM (1998) [Kinematics of Human Motions](http://books.google.com.br/books/about/Kinematics_of_Human_Motion.html?id=Pql_xXdbrMcC&redir_esc=y). Champaign, Human Kinetics.