#**Chapter 4 – Training Models**

     Open the original notebook

Let's begin like in the last notebook: importing a few common modules, ensuring MatplotLib plots figures inline and preparing a function to save the figures. We also check that Python 3.5 or later is installed (although Python 2.x may work, it is deprecated so once again we strongly recommend you use Python 3 instead), as well as Scikit-Learn ≥0.20.

You don't need to worry about understanding everything that is written in this section.

```python
# Python ≥3.5 is required
import sys
assert sys.version_info >= (3, 5)

# Is this notebook running on Colab or Kaggle?
IS_COLAB = "google.colab" in sys.modules

# Scikit-Learn ≥0.20 is required
import sklearn
assert sklearn.__version__ >= "0.20"

# Common imports
import numpy as np
import os

# To make this notebook's output stable across runs
rnd_seed = 42
rnd_gen = np.random.default_rng(rnd_seed)

# To plot pretty figures
%matplotlib inline
import matplotlib as mpl
import matplotlib.pyplot as plt
mpl.rc('axes', labelsize=14)
mpl.rc('xtick', labelsize=12)
mpl.rc('ytick', labelsize=12)

# Where to save the figures
PROJECT_ROOT_DIR = "."
CHAPTER_ID = "classification"
IMAGES_PATH = os.path.join(PROJECT_ROOT_DIR, "images", CHAPTER_ID)
os.makedirs(IMAGES_PATH, exist_ok=True)

def save_fig(fig_id, tight_layout=True, fig_extension="png", resolution=300):
    path = os.path.join(IMAGES_PATH, fig_id + "." + fig_extension)
    print("Saving figure", fig_id)
    if tight_layout:
        plt.tight_layout()
    plt.savefig(path, format=fig_extension, dpi=resolution)
```

In this notebook we will be working with the [*Iris Flower Dataset*](https://en.wikipedia.org/wiki/Iris_flower_data_set), in which the length and width of both the sepals and petals of three types of Iris flowes were recorded. For reference, these are pictures of the three flowers:

 In order: Iris Setosa,  Iris Versicolor, and Iris Virginica

Photo Credits:[Kosaciec szczecinkowaty Iris setosa](https://en.wikipedia.org/wiki/File:Kosaciec_szczecinkowaty_Iris_setosa.jpg) by [Radomil Binek](https://commons.wikimedia.org/wiki/User:Radomil) licensed under [CC BY-SA 3.0](https://creativecommons.org/licenses/by-sa/3.0/deed.en); [Blue flag flower close-up (Iris versicolor)](https://en.wikipedia.org/wiki/File:Iris_versicolor_3.jpg)by Danielle Langlois licensed under [CC BY-SA 3.0](https://creativecommons.org/licenses/by-sa/3.0/deed.en); [image of Iris virginica shrevei](https://en.wikipedia.org/wiki/File:Iris_virginica.jpg) by [Frank Mayfield](https://www.flickr.com/photos/33397993@N05) licensed under [CC BY-SA 2.0](https://creativecommons.org/licenses/by-sa/2.0/deed.en).

As you can imagine, this dataset is normally used to train *multiclass*/*multinomial* classification algorithms and not *binary* classification algorithms, since there *are* more than 2 classes.

"*Three classes, even!*" - an observant TA

For this exercise, however, we will be implemented the binary classification algorithm referred to as the *logistic regression* algorithm (also called logit regression).

```python
# Let's load the Iris Dataset
from sklearn import datasets
iris = datasets.load_iris()

# Print out some information about the data
print(f'Keys in Iris dictionary: \n{list(iris.keys())}\n\n')
print(iris.DESCR)

# And load the petal lengths and widths as our input data
X = iris["data"][:, (2, 3)]  # petal length, petal width
y = iris["target"]

print(iris['data_module'])

# The target data labels Setosa as 0, Versicolor as 1, and Virginica as 2. For
# this exercise we will be using only the Versicolor and Virgina sets.
bin_indices = np.logical_or(y==1,y==2)
bin_X = X[bin_indices]
bin_y = (y[bin_indices]==2).astype(np.uint8) # convert to binary
```

We now have a set of binary classification data we can use to train an algorithm.

As we saw during our reading, we need to define three things in order to train our algorithm: the type of algorithm we will train, the cost function (which will tell us how close our prediction is to the truth), and a method for updating the parameters in our model according to the value of the cost function (e.g., the gradient descent method).

Let's begin by defining the type of algorithm we will use. We will train a logistic regression model to differentiate between two classes. A reminder of how the logistic regression algorithm works is given below.

The logistic regression algorithm will thus take an input $t$ that is a linear combination of the features:

 $t_{\small{n}} = \beta_{\small{0}} + \beta_{\small{1}} \cdot X_{1,n} + \beta_{\small{2}} \cdot X_{2,n}$

where
* $n$ is the ID of the sample
* $X_{\small{0}}$ represents the petal length
* $X_{\small{1}}$ represents the petal width

This input is then fed into the logistic function, $\sigma$:
\begin{align}
\sigma: t\mapsto \dfrac{1}{1+e^ {-t}}
\end{align}

Let's plot it below to remember the shape of the function

```python
t = np.arange(-4,4,.1)
def logistic(in_val):
    # Return the value of the logistic function
    return 1/(1 + np.exp(- in_val))

fig, ax = plt.subplots()

ax.axvline(0, c='black', alpha=1)
ax.axhline(0, c='black', alpha=1)

[ax.axhline(y_val, c='black', alpha=0.5, linestyle='dotted') for y_val in (0.5,1)]

plt.autoscale(axis='x', tight=True)

ax.plot(t, logistic(t));
ax.set_xlabel('$t$')
ax.set_ylabel('$\\sigma\\  \\left(t\\right)$')
fig.tight_layout()
```

With the logistic function, we define inputs resulting in $\sigma\geq.5$ as belonging to the ***one*** class, and any value below that is considered to belong to the ***zero*** class.

We now have a function which lets us map the value of the petal length and width to the class to which the observation belongs (i.e., whether the length and width correspond to Iris Versicolor or Iris Virginica). However, there is a parameter vector **$\theta$** with a number of parameters that we do not have a value for:
 $\theta = [ \beta_{\small{0}}, \beta_{\small{1}}$, $\beta_{\small{2}} ]$

**Q1) Set up an array of random numbers between 0 and 1 representing the $\theta$ vector.**

Hint:  Use `rnd_gen`! If you're not sure how to use it, consult the `default_rng` documentation [at this link](https://numpy.org/doc/stable/reference/random/generator.html). For instance, you may use the `random` method of `rnd_gen`.

```python
# Write your code here
```

In order to determine whether a set of $\beta$ values is better than the other, we need to quantify well the values are able to predict the class. This is where the cost function comes in.

The cost function, $c$, will return a value close to zero when the prediction, $\hat{p}$, is correct and a large value when it is wrong. In a binary classification problem, we can use the log loss function. For a single prediction and truth value, it is given by:
\begin{align}
        \text{c}(\hat{p},y) = \left\{
        \begin{array}{cl}
        -\log(\hat{p})& \text{if}\; y=1\\
        -\log(1-\hat{p}) & \text{if}\; y=0
        \end{array}
        \right.
    \end{align}

However, we want to apply the cost function to an n-dimensional set of predictions and truth values. Thankfully, we can find the average value of the log loss function $J$ for an an-dimensional set of $\hat{y}$ & $y$ as follows:

\begin{align}
        \text{J}(\mathbf{\hat{p}},y) = - \dfrac{1}{n} \sum_{i=1}^{n}
        \left[ y_i\cdot \log\left( \hat{p}_i \right) \right] +
        \left[ \left( 1 - y_i \right) \cdot \log\left( 1-\hat{p}_i \right) \right]
    \end{align}

We now have a formula that can be used to calculate the average cost over the training set of data.

**Q2) Define a log_loss function that takes in an arbitrarily large set of prediction and truths**

Hint 1: You need to encode the function $J$ above, for which Numpy's functions may be quite convenient (e.g., [`log`](https://numpy.org/doc/stable/reference/generated/numpy.log.html), [`mean`](https://numpy.org/doc/stable/reference/generated/numpy.mean.html), etc.)

Hint 2: Asserting the dimensions of the vector is a good way to check that your function is working correctly. [Here's a tutorial on how to use `assert`](https://swcarpentry.github.io/python-novice-inflammation/10-defensive/index.html#assertions). For instance, to assert that two vectors `X` and `Y` have the same dimension, you may use:
```
assert X.shape==Y.shape
```

Now let's code 💻

```python
def log_loss(p_hat, y, epsilon=1e-7):
  # Write your code here.
  # We can also run into problems if p_hat = 0, so add an _epsilon_ term
  # when evaluating log(p_hat).
  log_p_hat = epsilon + ___________

  J = _____________________________

  # After calculating J, assert that J has the same shape as p_hat and y
  assert __________
  assert __________

  return
```

We now have a way of quantifying how good our predictions are. The final thing needed for us to train our algorithm is figuring out a way to update the parameters in a way that improves the average quality of our predictions.

**Warning**: we'll go into a bit of math below

Let's look at the change in a single parameter within $\theta$: $\beta_1$ (given $X_{1,i} = X_1$, $\;\hat{p}_{i} = \hat{p}$, $\;y_{i} = y$). If we want to know what the effect of changing the value of $\beta_1$ will have on the log loss function we can find this with the partial derivative:
$
        \dfrac{\partial J}{\partial \beta_1}
$

This may not seem very helpful by itself - after all, $\beta_1$ isn't even in the expression of $J$. But if we use the chain rule, we can rewrite the expression as:

        $\dfrac{\partial J}{\partial \hat{p}} \cdot
        \dfrac{\partial \hat{p}}{\partial \theta} \cdot
        \dfrac{\partial \theta}{\partial \beta_1}$

We'll spare you the math (feel free to verify it youself, however!):

$\dfrac{\partial J}{\partial \hat{p}} =  \dfrac{\hat{p} - y}{\hat{p}(1-\hat{p})}, \quad
        \dfrac{\partial \hat{p}}{\partial \theta} = \hat{p} (1-\hat{p}), \quad
        \dfrac{\partial \theta}{\partial \beta_1} = X_1 $

and thus
$
        \dfrac{\partial J}{\partial \beta_1} = (\hat{p} - y) \cdot X_1
$

We can calculate the partial derivative for each parameter in $\theta$ which, as you may have realized, is simply the $\theta$ gradient of $J$: $\nabla_{\theta}(J)$

With all of this information, we can now write $\nabla_{\theta} J$ in terms of the error, the feature vector, and the number of samples we're training on!

$\nabla_{\mathbf{\theta}^{(k)}} \, J(\mathbf{\theta^{(k)}}) = \dfrac{1}{n} \sum\limits_{i=1}^{n}{ \left ( \hat{p}^{(k)}_{i} - y_{i} \right ) \mathbf{X}_{i}}$

Note that here $k$ represents the iteration of the parameters we are currently on.

We now have a gradient we can calculate and use in the batch gradient descent method! The updated parameters will thus be:

\begin{align}
{\mathbf{\theta}^{(k+1)}} = {\mathbf{\theta}^{(k)}} - \eta\,\nabla_{\theta^{(k)}}J(\theta^{(k)})
\end{align}

Where $\eta$ is the learning rate parameter. It's also worth pointing out that $\;\hat{p}^{(k)}_i = \sigma\left(\theta^{(k)}, X_i\right) $

In order to easily calculate the input to the logistic regression, we'll multiply the $\theta$ vector with the X data, and as we have a non-zero bias  $\beta_0$ we'd like to have an X matrix whose first column is filled with ones.

\begin{align}
    X_{\small{with\ bias}} = \begin{pmatrix}
        1 & X_{1,0} & X_{2,0}\\
        1 & X_{1,1} & X_{2,1}\\
        &...&\\
        1 & X_{1,n} & X_{2,n}
        \end{pmatrix}
\end{align}

**Q3) Prepare the `X_with_bias` matrix (remember to use the `bin_X` data and not just `X`). Write a function called `predict` that takes in the parameter vector $\theta$ and the `X_with_bias` matrix and evaluates the logistic function for each of the samples.**

Hint 1: You recently learned how to initialize arrays in the `Numpy` notebook [at this link](https://nbviewer.org/github/tbeucler/2022_ML_Earth_Env_Sci/blob/main/Lab_Notebooks/S1_2_Numpy.ipynb). There are many ways to add a columns of 1 to `bin_X`, for instance using [`np.concatenate`](https://numpy.org/doc/stable/reference/generated/numpy.concatenate.html) or [`np.append`](https://numpy.org/doc/stable/reference/generated/numpy.append.html).

Hint 2:  To clarify, the function `predict` calculates $\hat{p}$ from $\beta$ and $\boldsymbol{X}$.

Hint 3: In practice, to calculate the logistic function for each sample, you may follow the equations [higher up in the notebook](#logit) and (1) calculate $t$ from $\beta$ and $\boldsymbol{X_{\mathrm{with\ bias}}}$ before (2) applying the logistic function $\sigma$ to $t$.

```python
# Prepare the X_with_bias matrix
```

```python
# Write your function predict here
```

**Q4) Now that you have a `predict` function, write a `gradient_calc` function that calculates the gradient for the logistic function.**

Hint 1: You'll have to feed `theta`, `X`, and `y` to the `gradient_calc` function.

Hint 2: You can use [this equation](#grad_eq) to calculate the gradient of the cost function.

```python
# Write your code here
```

We can now write a function that will train a logistic regression algorithm!

Your `logistic_regression` function needs to:
* Take in a set of training input/output data, validation input/output data, a number of iterations to train for, a set of initial parameters $\theta$, and a learning rate $\eta$
* At each iteration:
 * Generate a set of predictions on the training data. Hint: You may use your function `predict` on inputs `X_train` from the training set.
 * Calculate and store the loss function for the training data at each iteration. Hint: You may use your function `log_loss` on inputs `X_train` and outputs `y_train` from the training set.
 * Calculate the gradient. Hint: You may use your function `grad_calc`.
 * Update the $\theta$ parameters. Hint: You need to implement [this equation](#grad_descent).
 * Generate a set of predictions on the validation data using the updated parameters. Hint: You may use your function `predict` on inputs `X_valid` from the validation set.
 * Calculate and store the loss function for the validation data. Hint: You may use your function `log_loss` on inputs `X_valid` and outputs `y_valid` from the validation set.
 * Bonus: Calculate and store the accuracy of the model on the training and validation data as a metric!
* Return the final set of parameters $\theta$ & the stored training/validation loss function values (and the accuracy, if you did the bonus)

**Q5) Write the `logistic_regression` function**

```python
# Write your code here
```

**¡¡¡Important Note!!!**

The notebook assumes that you will return
1. a Losses list, where Losses[0] is the training loss and Losses[1] is the validation loss
2. a tuple with the 3 final coefficients ($\beta_0$, $\beta_1$, $\beta_2$)

The code for visualizing the bonus accuracy is not included - but it should be simple enough to do in a way similar to that which is done with the losses.

---------------------

Now that we have our logistic regression function, we're all set to train our algorithm! Or are we?

There's an important data step that we've neglected up to this point - we need to split the data into the train, validation, and test datasets.

```python
test_ratio = 0.2
validation_ratio = 0.2
total_size = len(X_with_bias)

test_size = int(total_size * test_ratio)
validation_size = int(total_size * validation_ratio)
train_size = total_size - test_size - validation_size

rnd_indices = rnd_gen.permutation(total_size)

X_train = X_with_bias[rnd_indices[:train_size]]
y_train = bin_y[rnd_indices[:train_size]]
X_valid = X_with_bias[rnd_indices[train_size:-test_size]]
y_valid = bin_y[rnd_indices[train_size:-test_size]]
X_test = X_with_bias[rnd_indices[-test_size:]]
y_test = bin_y[rnd_indices[-test_size:]]
```

Now we're ready!

**Q6) Train your logistic regression algorithm. Use 5000 iterations, $\eta$=0.1**

Hint: It's time to use the `logistic_regression` function you defined in Q5.

```python
# Complete the code
losses, coeffs =
```

Let's see how our model did while learning!

```python
# Produce the Loss Function Visualization Graphs
fig, ax = plt.subplots(figsize=(18,8))
ax.plot(losses[0], color='blue', label='Training', linewidth=3);
ax.plot(losses[1], color='orange', label='Validation', linewidth=3);
ax.legend();
ax.set_ylabel('Log Loss')
ax.set_xlabel('Iterations')
ax.set_title('Loss Function Graph')
ax.autoscale(axis='x', tight=True)
fig.tight_layout();

# Let's get predictions from our model for the training, validation, and testing
# datasets
y_hat_train = (predict(X_train, coeffs)>=.5).astype(int)
y_hat_valid = (predict(X_valid, coeffs)>=.5).astype(int)
y_hat_test = (predict(X_test, coeffs)>=.5).astype(int)

y_sets = [ [y_hat_train, y_train],
           [y_hat_valid, y_valid],
           [y_hat_test, y_test] ]

def accuracy_score(y_hat, y):
    assert(y_hat.size==y.size)
    return (y_hat == y).sum()/y.size

[accuracies.append(accuracy_score(y_set[0],y_set[1])) for y_set in y_sets]

printout= (f'Training Accuracy:{accuracies[0]:.1%} \n'
           f'Validation Accuracy:{accuracies[1]:.1%} \n')

# Add the testing accuracy only once you're sure that your model works!

print(printout)
```

Congratulations on training a logistic regression algorithm from scratch! Once you're done with the upcoming environmental science applications notebook, feel free to come back to take a look at the challenges 😀

## Challenges

* **C1)** Add L2 Regularization to training function

* **C2)** Add early stopping to the training algorithm! Stop training when the accuracy is >=90%

* **C3)** Implement a softmax regression model (It's multiclass logistic regression 🙂)


# Multibody dynamics of simple biomechanical models

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

The human body is composed of multiple interconnected segments (which can be modeled as rigid or flexible) and each segment may have translational and rotational movement. The part of mechanics for the study of movement and forces of interconnected bodies is called [multibody system](http://en.wikipedia.org/wiki/Multibody_system) or multibody dynamics.

There are different approaches to deduce the kinematics and dynamics of such bodies, the most common are the [Newton-Euler](http://en.wikipedia.org/wiki/Newton%E2%80%93Euler_equations) and the [Langrangian](http://en.wikipedia.org/wiki/Lagrangian_mechanics) formalisms. The Newton-Euler formalism is based on the well known Newton-Euler equations. The Langrangian formalism uses the [principle of least action](http://en.wikipedia.org/wiki/Principle_of_least_action) and describes the movement based on [generalized coordinates](http://en.wikipedia.org/wiki/Generalized_coordinates), a set of parameters (typically, a convenient minimal set) to describe the configuration of the system taking into account its constraints. For a system with multiple bodies and several constraints, e.g., the human body, it is easier to describe the dynamics of such system using the Langrangian formalism.

Next, we will study two simple problems of multibody systems in the context of biomechanics which we can handle well using the Newton-Euler approach. First a planar one-link system (which is not a multibody!), which can represent the movement of one limb of the body or the entire body as a single inverted pendulum. Second, a planar two-link system, which can represent the movement of two segments of the body, e.g., upper arm and forearm. Zajac and Gordon (1989) and Zajac (1993) offer excellent discussions about applying multibody system concepts to understanding human body movement.

## Newton-Euler equations

For a two-dimensional movement in the $XY$ plane, the Newton-Euler equations are:

\begin{equation}
\left\{ \begin{array}{l l}
\sum F_X = m \ddot{x}_{cm} \\
\\
\sum F_Y = m \ddot{y}_{cm} \\
\\
\sum M_Z = I_{cm} \ddot{\alpha}_Z
\end{array} \right.
\label{}
\end{equation}

Where the movement is described around the body center of mass ($cm$). $(F_X,\,F_Y)$ and $M_Z$ are, respectively, the forces and moment of forces (torques) acting on the body, $(\ddot{x}_{cm},\,\ddot{y}_{cm})$ and $\ddot{\alpha}_Z$ are, respectively, the linear and angular accelerations, and $I_{cm}$ is the body moment of inertia around the $Z$ axis passing through the body center of mass.

Let's use Sympy to derive some of the characteristics of the systems.

```python
from sympy import Symbol, symbols, cos, sin, Matrix, simplify
from sympy.physics.mechanics import dynamicsymbols, mlatex, init_vprinting
init_vprinting()
from IPython.display import display, Math
```

## One-link system

Let's study the dynamics of a planar inverted pendulum as a model for the movement of a human body segment with an external force acting on the segment (see Figure 1).

Figure. Planar inverted pendulum with joint actuators (muscles) and corresponding free body diagram. See text for notation convention.

The following notation convention will be used for this problem:

 - $L$ is the length of the segment.
 - $d$ is the distance from the joint of the segment to its center of mass position.
 - $m$ is the mass of the segment.
 - $g$ is the gravitational acceleration (+).
 - $\alpha$ is the angular position of the joint w.r.t. horizontal, $\ddot{\alpha_i}$ is the corresponding angular acceleration.
 - $I$ is the moment of inertia of the segment around its center of mass position.
 - $F_{r}$ is the joint reaction force.
 - $F_{e}$ is the external force acting on the segment.
 - $T$ is the joint moment of force (torque).

Muscles responsible for the movement of the segment are represented as a single pair of antagonistic joint actuators (e.g., flexors and extensors). We will consider that all joint torques are generated only by these muscles (we will disregard the torques generated by ligaments and other tissues) and the total or net joint torque will be the sum of the torques generated by the two muscles:

\begin{equation}
T = T_{net} = T_{extension} - T_{flexion} $$

Where we considered the extensor torque as positive. In what follows, we will determine only the net torque, we will be unable to decompose the net torque in its components.

### Kinetics

From the free body diagram, the Newton-Euler equations for the planar inverted pendulum are:

\begin{equation}
\begin{array}{l l}
F_{r,x} + F_{e,x} = m\ddot{x} \\
\\
F_{r,y} - mg + F_{e,y} = m\ddot{y} \\
\\
T + dF_{r,x}\sin\alpha - dF_{r,y}\cos\alpha - (L-d)F_{e,x}\sin\alpha + (L-d)F_{e,y}\cos\alpha = I\ddot{\alpha}
\end{array}
\label{}
\end{equation}

However, manually placing the terms in the Newton-Euler equations as we did above where we calculated the signs of the cross products is error prone. We can avoid this manual placing by treating the quantities as vectors and express them in matricial form:

\begin{equation}
\begin{array}{l l}
\mathbf{F}_r + \mathbf{F}_g + \mathbf{F}_e = m\ddot{\mathbf{r}} \\
\\
\mathbf{T} + \mathbf{r}_{cm,j} \times \mathbf{F}_r + \mathbf{r}_{cm,e} \times \mathbf{F}_e = I\ddot{\mathbf{\alpha}}
\end{array}
\label{}
\end{equation}

Where:

\begin{equation}
\begin{bmatrix} F_{rx} \\ F_{ry} \\ 0 \end{bmatrix}  + \begin{bmatrix} 0 \\ -g \\ 0 \end{bmatrix}  + \begin{bmatrix} F_{ex} \\ F_{ey} \\ 0 \end{bmatrix} = m\begin{bmatrix} \ddot{x} \\ \ddot{y} \\ 0 \end{bmatrix} , \quad \begin{bmatrix} \hat{i} \\ \hat{j} \\ \hat{k} \end{bmatrix}
\label{}
\end{equation}

\begin{equation}
\begin{bmatrix} 0 \\ 0 \\ T_z \end{bmatrix}  + \begin{bmatrix} -d\cos\alpha \\ -d\sin\alpha \\ 0 \end{bmatrix}  \times \begin{bmatrix} F_{rx} \\ F_{ry} \\ 0 \end{bmatrix} + \begin{bmatrix} (L-d)\cos\alpha \\ (L-d)\sin\alpha \\ 0 \end{bmatrix}  \times \begin{bmatrix} F_{ex} \\ F_{ey} \\ 0 \end{bmatrix} = I_z\begin{bmatrix} 0 \\ 0 \\ \ddot{\alpha} \end{bmatrix} , \quad \begin{bmatrix} \hat{i} \\ \hat{j} \\ \hat{k} \end{bmatrix} $$

Note that $\times$ represents the cross product, not matrix multiplication. Then, both in symbolic or numeric manipulation we would use the cross product function to perform part of the calculations. There are different computational tools that can be used for the formulation of the equations of motion. For instance, Sympy has a module, [Classical Mechanics](http://docs.sympy.org/dev/modules/physics/mechanics/), and see [this list](http://real.uwaterloo.ca/~mbody/#Software) for other software. Let's continue with the manual formulation of the equations hence they are not complex.

We can rewrite the equation for the moments of force in a form that doesn't explicitly involve the joint reaction force expressing the moments of force around the joint center:

\begin{equation}
T - mgd\cos\alpha - LF_{e,x}\sin\alpha + LF_{e,y}\cos\alpha = I_o\ddot{\alpha}
\label{}
\end{equation}

Where $I_o$ is the moment of inertia around the joint, $I_o=I_{cm}+md^2$, using the parallel axis theorem.

The torque due to the joint reaction force does not appear on this equation; this torque is null because by the definition the reaction force acts on the joint. If we want to determine the joint torque and we know the kinematics, we perform inverse dynamics:

\begin{equation}
T = I_o\ddot{\alpha} + mgd \cos \alpha + LF_{e,x}\sin\alpha - LF_{e,y}\cos\alpha
\label{}
\end{equation}

If we want to determine the kinematics and we know the joint torque, we perform direct dynamics:

\begin{equation}
\ddot{\alpha} = I_o^{-1}[T - mgd \cos \alpha - LF_{e,x}\sin\alpha + LF_{e,y}\cos\alpha ]
\label{}
\end{equation}

The expression above is a second-order differential equation which typically is solved numerically. So, unless we are explicitly interested in estimating the joint reaction forces, we don't need to use them for calculating the joint torque or simulate movement. Anyway, let's look at the kinematics of this problem to introduce some important concepts which will be needed later.

### Kinematics

A single planar inverted pendulum has one degree of freedom, the rotation movement of the segment around the pin joint. In this case, if the angular position $\alpha(t)$ is known, the coordinates $x(t)$ and $y(t)$ of the center of mass and their derivatives can be readily determined (a process referred as forward kinematics):

```python
t = Symbol('t')
d, L = symbols('d L', positive=True)
a = dynamicsymbols('alpha')
```

```python
x, y = d*cos(a), d*sin(a)
xd, yd = x.diff(t), y.diff(t)
xdd, ydd = xd.diff(t), yd.diff(t)

display(Math(r'x=' + mlatex(x)))
display(Math(r'\dot{x}=' + mlatex(xd)))
display(Math(r'\ddot{x}=' + mlatex(xdd)))
display(Math(r'y=' + mlatex(y)))
display(Math(r'\dot{y}=' + mlatex(yd)))
display(Math(r'\ddot{y}=' + mlatex(ydd)))
```

$$x=d \operatorname{cos}\left(\alpha\right)$$

$$\dot{x}=- d \operatorname{sin}\left(\alpha\right) \dot{\alpha}$$

$$\ddot{x}=- d \operatorname{sin}\left(\alpha\right) \ddot{\alpha} - d \operatorname{cos}\left(\alpha\right) \dot{\alpha}^{2}$$

$$y=d \operatorname{sin}\left(\alpha\right)$$

$$\dot{y}=d \operatorname{cos}\left(\alpha\right) \dot{\alpha}$$

$$\ddot{y}=- d \operatorname{sin}\left(\alpha\right) \dot{\alpha}^{2} + d \operatorname{cos}\left(\alpha\right) \ddot{\alpha}$$

The terms in $\ddot{x}$ and $\ddot{y}$ proportional to $\dot{\alpha}^2$ are components of the centripetal acceleration on the body. As the name suggests, the [centripetal](http://en.wikipedia.org/wiki/Centripetal_force) acceleration is always directed to the center (towards the joint) when the segment is rotating. See the notebook [Kinematic chain](http://nbviewer.ipython.org/github/demotu/BMC/blob/master/notebooks/KinematicChain.ipynb) for more on that.

As an exercise, let's go back to the Newton-Euler equation for the sum of torques around the center of mass where the torques due to the joint reaction forces are explicit. From the equation for the the sum of forces, hence we have expressions for the linear accelerations, we can isolate the reaction forces and substitute them on the equation for the torques. With a little help from Sympy:

```python
m, I, g = symbols('m I g', positive=True)
Fex, Fey = symbols('F_ex F_ey')
add = a.diff(t, 2)
```

```python
Frx = m*xdd - Fex
Fry = m*ydd + m*g - Fey
display(Math(r'F_{rx}=' + mlatex(Frx)))
display(Math(r'F_{ry}=' + mlatex(Fry)))
```

$$F_{rx}=- F_{ex} + m \left(- d \operatorname{sin}\left(\alpha\right) \ddot{\alpha} - d \operatorname{cos}\left(\alpha\right) \dot{\alpha}^{2}\right)$$

$$F_{ry}=- F_{ey} + g m + m \left(- d \operatorname{sin}\left(\alpha\right) \dot{\alpha}^{2} + d \operatorname{cos}\left(\alpha\right) \ddot{\alpha}\right)$$

```python
T = I*add - d*sin(a)*Frx + d*cos(a)*Fry + (L-d)*sin(a)*Fex - (L-d)*cos(a)*Fey
display(Math(r'T\quad=\quad ' + mlatex(T)))
```

$$T\quad=\quad F_{ex} \left(L - d\right) \operatorname{sin}\left(\alpha\right) - F_{ey} \left(L - d\right) \operatorname{cos}\left(\alpha\right) + I \ddot{\alpha} - d \left(- F_{ex} + m \left(- d \operatorname{sin}\left(\alpha\right) \ddot{\alpha} - d \operatorname{cos}\left(\alpha\right) \dot{\alpha}^{2}\right)\right) \operatorname{sin}\left(\alpha\right) + d \left(- F_{ey} + g m + m \left(- d \operatorname{sin}\left(\alpha\right) \dot{\alpha}^{2} + d \operatorname{cos}\left(\alpha\right) \ddot{\alpha}\right)\right) \operatorname{cos}\left(\alpha\right)$$

This equation for the torques around the center of mass of only one rotating segment seems too complicated. The equation we derived before for the torques around the joint was much simpler. However, if we look at the terms on this last equation, we can simplify most of them. Let's use Sympy to simplify this equation:

```python
T = simplify(T)
display(Math(r'T=' + mlatex(T)))
```

$$T=F_{ex} L \operatorname{sin}\left(\alpha\right) - F_{ey} L \operatorname{cos}\left(\alpha\right) + I \ddot{\alpha} + d^{2} m \ddot{\alpha} + d g m \operatorname{cos}\left(\alpha\right)$$

And we are back to the more simple equation we've seen before. The first two terms on the right side are the torque due to the external force, the third and fourth are the moment of inertia around the joint (use the theorem of parallel axis) times the acceleration, and the last term is the gravitational torque.

But what happened with all the other terms in the equation?

First, the terms proportional to the angular acceleration were just components from each direction of the 'inertial' torque that when summed resulted in $md^2\ddot{\alpha}$.
Second, the terms proportional to $\dot{\alpha}^2$ are components of the torque due to the centripetal force (acceleration). But the centripetal force passes through the joint as well as through the center of mass, i.e., it has zero lever arm and this torque should be zero. Indeed, when summed these terms are canceled out.

Now let's study a two-link system which can rotate independently around each joint. We will see that now the torque due to the centripetal force most of the times will not cancel out and a new torque component will appear.

### The Jacobian matrix

Another way to deduce the velocity and acceleration of a point at the rotating link is to use the [Jacobian matrix](http://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant) (see [Kinematic chain](http://nbviewer.ipython.org/github/demotu/BMC/blob/master/notebooks/KinematicChain.ipynb)). Remember that 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. For the planar one-link case, this means that the Jacobian matrix is:

\begin{equation}
\mathbf{J}=
\begin{bmatrix}
\dfrac{\partial x}{\partial \alpha} \\
\dfrac{\partial y}{\partial \alpha} \\
\end{bmatrix}
\label{}
\end{equation}

```python
r = Matrix((x, y))
J = r.diff(a)
display(Math(r'\mathbf{J}=' + mlatex(J)))
```

$$\mathbf{J}=\left[\begin{matrix}- d \operatorname{sin}\left(\alpha\right)\\d \operatorname{cos}\left(\alpha\right)\end{matrix}\right]$$

And Sympy has a function to calculate the Jacobian:

```python
J = r.jacobian([a])
display(Math(r'\mathbf{J}=' + mlatex(J)))
```

$$\mathbf{J}=\left[\begin{matrix}- d \operatorname{sin}\left(\alpha\right)\\d \operatorname{cos}\left(\alpha\right)\end{matrix}\right]$$

The linear velocity of a point in the link will be given by the product between the Jacobian of the kinematic link and its angular velocity:

\begin{equation}
\mathbf{v} = \mathbf{J} \dot{\alpha}
\label{}
\end{equation}

Using Sympy:

```python
vel = J*a.diff(t)
display(Math(r'\begin{bmatrix} \dot{x} \\ \dot{y} \end{bmatrix}=' + mlatex(vel)))
```

$$\begin{bmatrix} \dot{x} \\ \dot{y} \end{bmatrix}=\left[\begin{matrix}- d \operatorname{sin}\left(\alpha\right) \dot{\alpha}\\d \operatorname{cos}\left(\alpha\right) \dot{\alpha}\end{matrix}\right]$$

And the linear acceleration will be given by the derivative of this last expression:

\begin{equation}
\mathbf{a} = \dot{\mathbf{J}} \dot{\alpha} + \mathbf{J} \ddot{\alpha}
\label{}
\end{equation}

And using Sympy again:

```python
acc = (J*a.diff(t)).diff(t)
display(Math(r'\begin{bmatrix} \ddot{x} \\ \ddot{y} \end{bmatrix}=' + mlatex(acc)))
```

$$\begin{bmatrix} \ddot{x} \\ \ddot{y} \end{bmatrix}=\left[\begin{matrix}- d \operatorname{sin}\left(\alpha\right) \ddot{\alpha} - d \operatorname{cos}\left(\alpha\right) \dot{\alpha}^{2}\\- d \operatorname{sin}\left(\alpha\right) \dot{\alpha}^{2} + d \operatorname{cos}\left(\alpha\right) \ddot{\alpha}\end{matrix}\right]$$

Same expressions as before.

We can also use the Jacobian matrix to calculate the torque due to a force on the link:

\begin{equation}
T = \mathbf{J}^T \begin{bmatrix} F_{ex} \\ F_{ey} \end{bmatrix}
\label{}
\end{equation}

```python
Te = J.T*Matrix((Fex, Fey))
display(Math(r'T_e=' + mlatex(Te[0])))
```

$$T_e=- F_{ex} d \operatorname{sin}\left(\alpha\right) + F_{ey} d \operatorname{cos}\left(\alpha\right)$$

## Two-link system

Let's study the dynamics of a planar double inverted pendulum (see Figure 2) as a model of two interconnected segments in the human body with an external force acting on the distal segment. Once again, we will consider that there are muscles around each joint and they generate torques.

Figure. Planar double inverted pendulum  with joint actuators (muscles) and corresponding free body diagrams. See text for notation convention.

The following notation convention will be used for this problem:
 - Subscript $i$ runs 1 or 2 meaning first (most proximal) or second joint when referring to angles, joint moments, or joint reaction forces, or meaning first or second segment when referring to everything else.
 - $L_i$ is the length of segment $i$.
 - $d_i$ is the distance from the proximal joint of segment $i$ to its center of mass position.
 - $m_i$ is the mass of segment $i$.
 - $g$ is the gravitational acceleration (+).
 - $\alpha_i$ is the angular position of joint $i$ in the joint space, $\ddot{\alpha_i}$ is the corresponding angular acceleration.
 - $\theta_i$ is the angular position of joint $i$ in the segmental space w.r.t. horizontal, $\theta_1=\alpha_1$ and $\theta_2=\alpha_1+\alpha_2$.
 - $I_i$ is the moment of inertia of segment $i$ around its center of mass position.
 - $F_{ri}$ is the reaction force of joint $i$.
 - $F_{e}$ is the external force acting on the distal segment.
 - $T_i$ is the moment of force (torque) of joint $i$.

Hence we know we will need the linear accelerations for solving the Newton-Euler equations, let's deduce them first.

### Kinematics

Once again, if the angular positions $\alpha_1(t)$ and $\alpha_2(t)$ are known, the coordinates $(x_1(t), y_1(t))$ and $(x_2(t), y_2(t))$ and their derivatives can be readily determined (by forward kinematics):

#### Link 1

```python
t = Symbol('t')
d1, d2, L1, L2 = symbols('d1, d2, L_1 L_2', positive=True)
a1, a2 = dynamicsymbols('alpha1 alpha2')
a1d, a2d = a1.diff(t), a2.diff(t)
a1dd, a2dd = a1.diff(t, 2), a2.diff(t, 2)
```

```python
x1, y1 = d1*cos(a1), d1*sin(a1)
x1d, y1d = x1.diff(t), y1.diff(t)
x1dd, y1dd = x1d.diff(t), y1d.diff(t)

display(Math(r'x_1=' + mlatex(x1)))
display(Math(r'\dot{x}_1=' + mlatex(x1d)))
display(Math(r'\ddot{x}_1=' + mlatex(x1dd)))
display(Math(r'y_1=' + mlatex(y1)))
display(Math(r'\dot{y}_1=' + mlatex(y1d)))
display(Math(r'\ddot{y}_1=' + mlatex(y1dd)))
```

$$x_1=d_{1} \operatorname{cos}\left(\alpha_{1}\right)$$

$$\dot{x}_1=- d_{1} \operatorname{sin}\left(\alpha_{1}\right) \dot{\alpha}_{1}$$

$$\ddot{x}_1=- d_{1} \operatorname{sin}\left(\alpha_{1}\right) \ddot{\alpha}_{1} - d_{1} \operatorname{cos}\left(\alpha_{1}\right) \dot{\alpha}_{1}^{2}$$

$$y_1=d_{1} \operatorname{sin}\left(\alpha_{1}\right)$$

$$\dot{y}_1=d_{1} \operatorname{cos}\left(\alpha_{1}\right) \dot{\alpha}_{1}$$

$$\ddot{y}_1=- d_{1} \operatorname{sin}\left(\alpha_{1}\right) \dot{\alpha}_{1}^{2} + d_{1} \operatorname{cos}\left(\alpha_{1}\right) \ddot{\alpha}_{1}$$

#### Link 2

```python
x2, y2 = L1*cos(a1) + d2*cos(a1+a2), L1*sin(a1) + d2*sin(a1+a2)
x2d, y2d = x2.diff(t), y2.diff(t)
x2dd, y2dd = x2d.diff(t), y2d.diff(t)

display(Math(r'x_2=' + mlatex(x2)))
display(Math(r'\dot{x}_2=' + mlatex(x2d)))
display(Math(r'\ddot{x}_2=' + mlatex(x2dd)))
display(Math(r'y_2=' + mlatex(y2)))
display(Math(r'\dot{y}_2=' + mlatex(y2d)))
display(Math(r'\ddot{y}_2=' + mlatex(y2dd)))
```

$$x_2=L_{1} \operatorname{cos}\left(\alpha_{1}\right) + d_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)$$

$$\dot{x}_2=- L_{1} \operatorname{sin}\left(\alpha_{1}\right) \dot{\alpha}_{1} - d_{2} \left(\dot{\alpha}_{1} + \dot{\alpha}_{2}\right) \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right)$$

$$\ddot{x}_2=- L_{1} \operatorname{sin}\left(\alpha_{1}\right) \ddot{\alpha}_{1} - L_{1} \operatorname{cos}\left(\alpha_{1}\right) \dot{\alpha}_{1}^{2} - d_{2} \dot{\alpha}_{1} + \dot{\alpha}_{2}^{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right) - d_{2} \left(\ddot{\alpha}_{1} + \ddot{\alpha}_{2}\right) \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right)$$

$$y_2=L_{1} \operatorname{sin}\left(\alpha_{1}\right) + d_{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right)$$

$$\dot{y}_2=L_{1} \operatorname{cos}\left(\alpha_{1}\right) \dot{\alpha}_{1} + d_{2} \left(\dot{\alpha}_{1} + \dot{\alpha}_{2}\right) \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)$$

$$\ddot{y}_2=- L_{1} \operatorname{sin}\left(\alpha_{1}\right) \dot{\alpha}_{1}^{2} + L_{1} \operatorname{cos}\left(\alpha_{1}\right) \ddot{\alpha}_{1} - d_{2} \dot{\alpha}_{1} + \dot{\alpha}_{2}^{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right) + d_{2} \left(\ddot{\alpha}_{1} + \ddot{\alpha}_{2}\right) \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)$$

Inspecting the equations above, we see a new kind of acceleration, proportional to $\dot{\alpha_1}\dot{\alpha_2}$. This acceleration is due to the [Coriolis effect](http://en.wikipedia.org/wiki/Coriolis_effect) and is  present only when there are movement in the two joints.

### Kinetics

From the free body diagrams, the Newton-Euler equations for the planar double inverted pendulum are:

#### Link 2

\begin{equation}
\begin{array}{l l}
F_{r2x} + F_{e,x} = m_2\ddot{x}_{2} \\
\\
F_{r2y} - m_2g + F_{e,y} = m_2\ddot{y}_{2} \\
\\
T_2 + d_2F_{r2x}\sin(\alpha_1+\alpha_2) - d_2F_{r2y}\cos(\alpha_1+\alpha_2) - (L_2-d_2)F_{e,x}\sin(\alpha_1+\alpha_2) - (L_2-d_2)F_{e,y}\cos(\alpha_1+\alpha_2) = I_{2}(\ddot{\alpha}_1+\ddot{\alpha}_2)
\end{array}
\label{}
\end{equation}

#### Link 1

\begin{equation}
\begin{array}{l l}
F_{r1x} - F_{r2x} = m_1\ddot{x}_{1} \\
\\
F_{r1y} - F_{r2y} - m_1g = m_1\ddot{y}_{1} \\
\\
T_1 - T_2 + d_1F_{r1x}\sin\alpha_1 - d_1F_{r1y}\cos\alpha_1 + (L_1-d_1)F_{r2x}\sin\alpha_1 - (L_1-d_1)F_{r2y}\cos\alpha_1 = I_{1}\ddot{\alpha}_1
\end{array}
\label{}
\end{equation}

If we want to determine the joint torques and we know the kinematics of the links, the inverse dynamics approach, we isolate the joint torques in the equations above, start solving for link 2 and then link 1. To determine the kinematics knowing the joint torques, the direct dynamics approach, we isolate the joint angular accelerations in the equations above and solve the ordinary differential equations.

Let's express the equations for the torques substituting the terms we know:

```python
m1, m2, I1, I2, g = symbols('m_1, m_2, I_1 I_2 g', positive=True)
```

```python
# link 2
Fr2x = m2*x2dd - Fex
Fr2y = m2*y2dd + m2*g - Fey
T2 = I2*(a1dd+a2dd) - d2*Fr2x*sin(a1+a2) + d2*Fr2y*cos(a1+a2) + (L2-d2)*Fex*sin(a1+a2) - (L2-d2)*Fey*cos(a1+a2)
T2 = simplify(T2)
# link 1
Fr1x = m1*x1dd + Fr2x
Fr1y = m1*y1dd + Fr2y + m1*g
T1 = I1*a1dd + T2 - d1*Fr1x*sin(a1) + d1*Fr1y*cos(a1) - (L1-d1)*Fr2x*sin(a1) + (L1-d1)*Fr2y*cos(a1)
T1 = simplify(T1)
```

The expressions for the joint moments of force are:

```python
display(Math(r'T_1\quad = \quad ' + mlatex(T1)))
display(Math(r'T_2\quad = \quad ' + mlatex(T2)))
```

$$T_1\quad = \quad F_{ex} L_{1} \operatorname{sin}\left(\alpha_{1}\right) + F_{ex} L_{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right) - F_{ey} L_{1} \operatorname{cos}\left(\alpha_{1}\right) - F_{ey} L_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right) + I_{1} \ddot{\alpha}_{1} + I_{2} \ddot{\alpha}_{1} + I_{2} \ddot{\alpha}_{2} + L_{1}^{2} m_{2} \ddot{\alpha}_{1} - 2 L_{1} d_{2} m_{2} \operatorname{sin}\left(\alpha_{2}\right) \dot{\alpha}_{1} \dot{\alpha}_{2} - L_{1} d_{2} m_{2} \operatorname{sin}\left(\alpha_{2}\right) \dot{\alpha}_{2}^{2} + 2 L_{1} d_{2} m_{2} \operatorname{cos}\left(\alpha_{2}\right) \ddot{\alpha}_{1} + L_{1} d_{2} m_{2} \operatorname{cos}\left(\alpha_{2}\right) \ddot{\alpha}_{2} + L_{1} g m_{2} \operatorname{cos}\left(\alpha_{1}\right) + d_{1}^{2} m_{1} \ddot{\alpha}_{1} + d_{1} g m_{1} \operatorname{cos}\left(\alpha_{1}\right) + d_{2}^{2} m_{2} \ddot{\alpha}_{1} + d_{2}^{2} m_{2} \ddot{\alpha}_{2} + d_{2} g m_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)$$

$$T_2\quad = \quad F_{ex} L_{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right) - F_{ey} L_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right) + I_{2} \ddot{\alpha}_{1} + I_{2} \ddot{\alpha}_{2} + L_{1} d_{2} m_{2} \operatorname{sin}\left(\alpha_{2}\right) \dot{\alpha}_{1}^{2} + L_{1} d_{2} m_{2} \operatorname{cos}\left(\alpha_{2}\right) \ddot{\alpha}_{1} + d_{2}^{2} m_{2} \ddot{\alpha}_{1} + d_{2}^{2} m_{2} \ddot{\alpha}_{2} + d_{2} g m_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)$$

There is an elegant form to display the equations for the torques using generalized coordinates, $q=[\alpha_1, \alpha_2]^T$ and grouping the terms proportional to common quantities in matrices, see for example, Craig (2005, page 180), Pandy (2001), and Zatsiorsky (2002, page 383):

\begin{equation}
\begin{array}{l l}
\tau = M(q)\ddot{q} + C(q,\dot{q}) + G(q) + E(q,\dot{q})
\end{array}
\label{}
\end{equation}

Where $\tau$ is a matrix (2x1) of joint torques; $M$ is the mass or inertia matrix (2x2); $\ddot{q}$ is a matrix (2x1) of angular accelerations; $C$ is a matrix (2x1) of [centipetal](http://en.wikipedia.org/wiki/Centripetal_force) and [Coriolis](http://en.wikipedia.org/wiki/Coriolis_effect) torques; $G$ is a matrix (2x1) of  gravitational torques; and $E$ is a matrix (2x1) of external torques.
Let's use Sympy to display the equations in this new form:

```python
T1, T2 = T1.expand(), T2.expand()
q1, q2 = dynamicsymbols('q_1 q_2')
q1d, q2d = q1.diff(t), q2.diff(t)
q1dd, q2dd = q1.diff(t, 2), q2.diff(t, 2)
T1 = T1.subs({a1:q1, a2:q2, a1d:q1d, a2d:q2d, a1dd:q1dd, a2dd:q2dd})
T2 = T2.subs({a1:q1, a2:q2, a1d:q1d, a2d:q2d, a1dd:q1dd, a2dd:q2dd})
```

```python
M = Matrix(((simplify(T1.coeff(q1dd)), simplify(T1.coeff(q2dd))),
            (simplify(T2.coeff(q1dd)), simplify(T2.coeff(q2dd)))))
C = Matrix((simplify(T1.coeff(q1d**2)*q1d**2 + T1.coeff(q2d**2)*q2d**2 + T1.coeff(q1d*q2d)*q1d*q2d),
            simplify(T2.coeff(q1d**2)*q1d**2 + T2.coeff(q2d**2)*q2d**2 + T2.coeff(q1d*q2d)*q1d*q2d)))
G = Matrix((simplify(T1.coeff(g)*g),
            simplify(T2.coeff(g)*g)))
E = Matrix((simplify(T1.coeff(Fex)*Fex + T1.coeff(Fey)*Fey),
            simplify(T2.coeff(Fex)*Fex + T2.coeff(Fey)*Fey)))

display(Math(r'\begin{eqnarray}\tau&=&\begin{bmatrix}\tau_1\\ \tau_2\\ \end{bmatrix} \\' +
             r'M(q)&=&' + mlatex(M) + r'\\' +
             r'\ddot{q}&=&' + mlatex(Matrix((q1dd, q2dd))) + r'\\' +
             r'C(q,\dot{q})&=&' + mlatex(C) + r'\\' +
             r'G(q)&=&' + mlatex(G) + r'\\' +
             r'E(q,\dot{q})&=&' + mlatex(E) + r'\end{eqnarray}'))
```

$$\begin{eqnarray}\tau&=&\begin{bmatrix}\tau_1\\ \tau_2\\ \end{bmatrix} \\M(q)&=&\left[\begin{matrix}I_{1} + I_{2} + L_{1}^{2} m_{2} + 2 L_{1} d_{2} m_{2} \operatorname{cos}\left(q_{2}\right) + d_{1}^{2} m_{1} + d_{2}^{2} m_{2} & I_{2} + L_{1} d_{2} m_{2} \operatorname{cos}\left(q_{2}\right) + d_{2}^{2} m_{2}\\I_{2} + L_{1} d_{2} m_{2} \operatorname{cos}\left(q_{2}\right) + d_{2}^{2} m_{2} & I_{2} + d_{2}^{2} m_{2}\end{matrix}\right]\\\ddot{q}&=&\left[\begin{matrix}\ddot{q}_{1}\\\ddot{q}_{2}\end{matrix}\right]\\C(q,\dot{q})&=&\left[\begin{matrix}- L_{1} d_{2} m_{2} \left(2 \dot{q}_{1} + \dot{q}_{2}\right) \operatorname{sin}\left(q_{2}\right) \dot{q}_{2}\\L_{1} d_{2} m_{2} \operatorname{sin}\left(q_{2}\right) \dot{q}_{1}^{2}\end{matrix}\right]\\G(q)&=&\left[\begin{matrix}g \left(L_{1} m_{2} \operatorname{cos}\left(q_{1}\right) + d_{1} m_{1} \operatorname{cos}\left(q_{1}\right) + d_{2} m_{2} \operatorname{cos}\left(q_{1} + q_{2}\right)\right)\\d_{2} g m_{2} \operatorname{cos}\left(q_{1} + q_{2}\right)\end{matrix}\right]\\E(q,\dot{q})&=&\left[\begin{matrix}F_{ex} \left(L_{1} \operatorname{sin}\left(q_{1}\right) + L_{2} \operatorname{sin}\left(q_{1} + q_{2}\right)\right) - F_{ey} \left(L_{1} \operatorname{cos}\left(q_{1}\right) + L_{2} \operatorname{cos}\left(q_{1} + q_{2}\right)\right)\\L_{2} \left(F_{ex} \operatorname{sin}\left(q_{1} + q_{2}\right) - F_{ey} \operatorname{cos}\left(q_{1} + q_{2}\right)\right)\end{matrix}\right]\end{eqnarray}$$

With this convention, to perform inverse dynamics we would calculate:

\begin{equation}
\tau = M(q)\ddot{q} + C(q,\dot{q}) + G(q) + E(q,\dot{q})
\label{}
\end{equation}

And for direct dynamics we would solve the differential equation:

\begin{equation}
\ddot{q} = M(q)^{-1} \left[\tau - C(q,\dot{q}) - G(q) - E(q,\dot{q}) \right]
\label{}
\end{equation}

The advantage of calculating analytically the derivatives of the position vector as function of the joint angles and using the notation above is that each term that contributes to each joint torque or acceleration can be easily identified.

#### Coupling (or interaction) effects

The terms off the main diagonal in the inertia matrix (which are the same) and the centripetal and Coriolis terms represent the effects of the movement (nonzero velocity) of one joint over the other. These torques are referred as coupling or interaction effects (see for example Hollerbach and Flash (1982) for an application of this concept in the study of the motor control of the upper limb movement).

#### Planar double pendulum

Using the same equations above, one can represent a planar double pendulum (hanging from the top, not inverted) considering the angles $\alpha_1$ and $\alpha_2$ negative, e.g., at $\alpha_1=-90^o$ and $\alpha_2=0$ the pendulum is hanging vertical.

#### WARNING: $F_r$ is not the actual joint reaction force!

For these two examples, in the Newton-Euler equations based on the free body diagrams we represented the consequences of all possible muscle forces on a joint as a net muscle torque and all forces acting on a joint as a resultant joint reaction force. That is, all forces between segments were represented as a resultant force that doesn't generate torque and a force couple (or free moment) that only generates torque. This is an important principle in mechanics of rigid bodies, see for example [this text](http://nbviewer.ipython.org/github/demotu/BMC/blob/master/notebooks/FreeBodyDiagram.ipynb). However, this principle creates the unrealistic notion that the sum of forces is applied directly on the joint (which has no further implication for a rigid body), but it is inaccurate for the understanding of the local effects on the joint. So, if we are trying to understand the stress on the joint or mechanisms of joint injury, the forces acting on the joint and on the rest of the segment must be considered individually.

#### Determination of muscle force

The torque $T$ exerted by a muscle is given by the product between the muscle-tendon moment arm $r$ and its force $F$. For the human body, there is more than one muscle crossing a joint and several joints. In such case, the torques due to the muscles are expressed in the following matrix form considering $n$ joints and $m$ muscles:

\begin{eqnarray}
\begin{bmatrix} T_1 \\ \vdots \\ T_n \end{bmatrix} = \begin{bmatrix} r_{11} & \cdots & r_{1m} \\ \vdots & \ddots & \vdots \\ r_{n1} & \cdots & r_{nm} \end{bmatrix} \begin{bmatrix} F_1 \\ \vdots \\ F_m \end{bmatrix}
\label{}
\end{eqnarray}

Where $r_{nm}$ is the moment arm about joint $n$ of the muscle $m$.
In the example of the two-link system, we sketched two uniarticular muscles for each of the two joints, consequently:

\begin{eqnarray}
\begin{bmatrix} T_1 \\ T_2 \end{bmatrix} = \begin{bmatrix} r_{1,ext} & r_{1,flex} & 0 & 0 \\ 0 & 0 & r_{1,ext} & r_{1,flex} \end{bmatrix} \begin{bmatrix} F_{1,ext} \\ -F_{1,flex} \\ F_{2,ext} \\ -F_{2,flex} \end{bmatrix}
\label{}
\end{eqnarray}

The moment arm of a muscle varies with the motion of the joints it crosses. In this case, using the [virtual work principle](http://en.wikipedia.org/wiki/Virtual_work) the moment arm can be given by (Sherman et al., 2013; Nigg and Herzog, 2006, page 634):

\begin{equation}
r(q) = \dfrac{\partial L_{MT}(q)}{\partial q}
\label{}
\end{equation}

Where $L_{MT}(q)$ is the length of the muscle-tendon unit expressed as a function of angle $q$.

For the simulation of human movement, muscles can be modeled as [Hill-type muscles](http://nbviewer.ipython.org/github/demotu/BMC/blob/master/notebooks/MuscleSimulation.ipynb), the torques they generate are given by the matrix above, and this matrix is entered in the ODE for a multibody system dynamics we deduced before:

\begin{equation}
\ddot{q} = M(q)^{-1} \left[R_{MT}(q)F_{MT}(a,L_{MT},\dot{L}_{MT}) - C(q,\dot{q}) - G(q) - E(q,\dot{q}) \right]
\label{}
\end{equation}

Where $R_{MT}$ and $F_{MT}$ are matrices for the moment arms and muscle-tendon forces, respectively.
This ODE is then solved numerically given initial values; but this problem is far from trivial for a simulation with several segments and muscles.

## Numerical simulation

Let's simulate a voluntary movement of the upper limb using the planar two-link system as a model in order to visualize the contribution of each torque term. We will ignore the muscle dynamics and we will calculate the joint torques necessary to move the upper limb from one point to another under the assumption that the movement is performed with the smoothest trajectory possible. I.e., the movement is performed with a [minimum-jerk trajectory](http://nbviewer.ipython.org/github/demotu/BMC/blob/master/notebooks/MinimumJerkHypothesis.ipynb), a hypothesis about control of voluntary movements proposed by Flash and Hogan (1985).

Once we determine the desired trajectory, we can calculate the velocity and acceleration of the segments and combine with anthropometric measures to calculate the joint torques necessary to move the segments. This means we will perform inverse dynamics.

Let's simulate a slow (4 s) and a fast (0.5 s) movement of the upper limb starting at the anatomical neutral position (upper limb at the side of the trunk) and ending with the upper arm forward at horizontal and elbow flexed at 90 degrees.

First, let's import the necessary Python libraries and customize the environment:

```python
import numpy as np
%matplotlib inline
import matplotlib
import matplotlib.pyplot as plt
matplotlib.rcParams['lines.linewidth'] = 3
matplotlib.rcParams['font.size'] = 13
matplotlib.rcParams['lines.markersize'] = 5
matplotlib.rc('axes', grid=False, labelsize=14, titlesize=16, ymargin=0.05)
matplotlib.rc('legend', numpoints=1, fontsize=11)
import sys
sys.path.insert(1, r'./../functions')  # add to pythonpath
```

Let's take the anthropometric data from Dempster's model (see [Body segment parameters](http://nbviewer.ipython.org/github/demotu/BMC/blob/master/notebooks/BodySegmentParameters.ipynb)):

```python
height, mass = 1.70,               70  # m, kg
L1n, L2n     = 0.188*height,       0.253*height
d1n, d2n     = 0.436*L1n,          0.682*L2n
m1n, m2n     = 0.0280*mass,        0.0220*mass
rg1n, rg2n   = 0.322,              0.468
I1n, I2n     = m1n*(rg1n*L1n)**2,  m2n*(rg2n*L2n)**2
```

Considering these lengths, the initial and final positions of the endpoint (finger tip) for the simulated movement will be:

```python
xi, yi = 0, -L1n-L2n
xf, yf = L1n, L2n
gn = 9.81  # gravity acceleration m/s2
```

### Slow movement

```python
duration = 4  # seconds
```

The endpoint minimum jerk trajectory will be (see [Kinematic chain in a plane (2D)](http://nbviewer.ipython.org/github/demotu/BMC/blob/master/notebooks/KinematicChain.ipynb)):

```python
from minjerk import minjerk
```

```python
time, rlin, vlin, alin, jlin = minjerk([xi, yi], [xf, yf], duration=duration)
```

Let's find the joint angles to produce this minimum-jerk trajectory (inverse kinematics):

```python
from invkin2_2d import invkin
```

```python
rang = invkin(time, rlin, L1=L1n, L2=L2n)
```

For the joint torques, we need to calculate the angular velocity and acceleration. Let's do that using numerical differentiation:

```python
def diff_c(ang, duration):
    """Numerical differentiations using the central difference for the angular data.
    """
    # central difference (f(x+h)-f(x-h))/(2*h)
    dt = duration/(ang.shape[0]-1)
    vang = np.empty_like(rang)
    aang = np.empty_like(rang)
    vang[:, 0] = np.gradient(rang[:, 0], dt)
    vang[:, 1] = np.gradient(rang[:, 1], dt)
    aang[:, 0] = np.gradient(vang[:, 0], dt)
    aang[:, 1] = np.gradient(vang[:, 1], dt)

    _, ax = plt.subplots(1, 3, sharex=True, figsize=(10, 3))
    ax[0].plot(time, rang*180/np.pi)
    ax[0].legend(['Ang 1', 'Ang 2'], framealpha=.5, loc='best')
    ax[1].plot(time, vang*180/np.pi)
    ax[2].plot(time, aang*180/np.pi)
    ylabel = [r'Displacement [$\mathrm{^o}$]', r'Velocity [$\mathrm{^o/s}$]', r'Acceleration [$\mathrm{^o/s^2}$]']
    for i, axi in enumerate(ax):
        axi.set_xlabel('Time [$s$]')
        axi.set_ylabel(ylabel[i])
        axi.xaxis.set_major_locator(plt.MaxNLocator(4))
        axi.yaxis.set_major_locator(plt.MaxNLocator(4))
    plt.tight_layout()
    plt.show()

    return vang, aang

vang, aang = diff_c(rang, duration)
```

```python
def dyna(time, L1n, L2n, d1n, d2n, m1n, m2n, gn, I1n, I2n, q1, q2, rang, vang, aang, Fexn, Feyn, M, C, G, E):
    """Numerical calculation and plot for the torques of a planar two-link system.
    """
    from sympy import lambdify, symbols
    Mfun  = lambdify((I1, I2, L1, L2, d1, d2, m1, m2, q1, q2), M, 'numpy')
    Mn    = Mfun(I1n, I2n, L1n, L2n, d1n, d2n, m1n, m2n, rang[:, 0], rang[:, 1])
    M00   = Mn[0, 0][:, 0]*aang[:, 0]
    M01   = Mn[0, 1][:, 0]*aang[:, 1]
    M10   = Mn[1, 0][:, 0]*aang[:, 0]
    M11   = Mn[1, 1]*aang[:, 1]
    Q1d, Q2d = symbols('Q1d Q2d')
    dicti = {q1.diff(t, 1):Q1d, q2.diff(t, 1):Q2d}
    C0fun = lambdify((L1, d2, m2, q2, Q1d, Q2d), C[0].subs(dicti), 'numpy')
    C0    = C0fun(L1n, d2n, m2n, rang[:, 1], vang[:, 0], vang[:, 1])
    C1fun = lambdify((L1, d2, m2, q2, Q1d, Q2d), C[1].subs(dicti), 'numpy')
    C1    = C1fun(L1n, d2n, m2n, rang[:, 1], vang[:, 0], vang[:, 1])
    G0fun = lambdify((L1, d1, d2, m1, m2, g, q1, q2), G[0], 'numpy')
    G0    = G0fun(L1n, d1n, d2n, m1n, m2n, gn, rang[:, 0], rang[:, 1])
    G1fun = lambdify((L1, d1, d2, m1, m2, g, q1, q2), G[1], 'numpy')
    G1    = G1fun(L1n, d1n, d2n, m1n, m2n, gn, rang[:, 0], rang[:, 1])
    E0fun = lambdify((L1, L2, q1, q2, Fex, Fey), E[0], 'numpy')
    E0    = E0fun(L1n, L2n, rang[:, 0], rang[:, 1], 0, 0)
    E1fun = lambdify((L1, L2, q1, q2, Fex, Fey), E[1], 'numpy')
    E1    = E1fun(L1n, L2n, rang[:, 0], rang[:, 1], Fexn, Feyn)

    _, ax = plt.subplots(1, 2, sharex=True, squeeze=True, figsize=(10, 4))
    ax[0].plot(time, M00+M01)
    ax[0].plot(time, C0)
    ax[0].plot(time, G0)
    ax[0].plot(time, E0)
    ax[0].plot(time, M00+M01+C0+G0, 'k--', linewidth=4)
    ax[0].set_ylabel(r'Torque [Nm]')
    ax[0].set_title('Joint 1')
    ax[1].plot(time, M10+M11, label='Mass/Inertia')
    ax[1].plot(time, C1, label='Centripetal/Coriolis')
    ax[1].plot(time, G1, label='Gravitational')
    ax[1].plot(time, E1, label='External')
    ax[1].plot(time, M10+M11+C1+G1, 'k--', linewidth=4, label='Muscular (sum)')
    ax[1].set_title('Joint 2')
    ax[1].legend(framealpha=.5, loc='upper right', bbox_to_anchor=(1.6, 1))
    for i, axi in enumerate(ax):
        axi.set_xlabel('Time [$s$]')
        axi.xaxis.set_major_locator(plt.MaxNLocator(4))
        axi.yaxis.set_major_locator(plt.MaxNLocator(4))
    plt.tight_layout()
    plt.show()

    return M00, M01, M10, M11, C0, C1, G0, G1, E0, E1
```

```python
Fexn, Feyn = 0, 0
M00, M01, M10, M11, C0, C1, G0, G1, E0, E1 = dyna(time, L1n, L2n, d1n, d2n, m1n, m2n, gn, I1n, I2n,
                                                  q1, q2, rang, vang, aang, Fexn, Feyn, M, C, G, E)
```

The joint torques essentially compensate the gravitational torque.

### Fast movement

Let's see what is changed for a fast movement:

```python
duration = 0.5  # seconds
time, rlin, vlin, alin, jlin = minjerk([xi, yi], [xf, yf], duration=duration)
rang = invkin(time, rlin, L1=L1n, L2=L2n)
vang, aang = diff_c(rang, duration)
M00, M01, M10, M11, C0, C1, G0, G1, E0, E1 = dyna(time, L1n, L2n, d1n, d2n, m1n, m2n, gn, I1n, I2n,
                                                  q1, q2, rang, vang, aang, Fexn, Feyn, M, C, G, E)
```

The interaction torques are larger than the gravitational torques for most part of the movement.

### Fast movement in the horizontal plane

Let's simulate a fast movement in the horizontal plane:

```python
gn = 0  # gravity acceleration m/s2
M00, M01, M10, M11, C0, C1, G0, G1, E0, E1 = dyna(time, L1n, L2n, d1n, d2n, m1n, m2n, gn, I1n, I2n,
                                                  q1, q2, rang, vang, aang, Fexn, Feyn, M, C, G, E)
```

## Exercises

1. Derive the equations of motion for a single pendulum (not inverted).
2. Derive the equations of motion for a double pendulum (not inverted).
3. Consider the double pendulum moving in the horizontal plane and with no external force. Find out the type of movement and which torque terms are changed when:
  a) $\dot{\alpha}_1=0^o$
  b) $\alpha_2=0^o$
  c) $\dot{\alpha}_2=0^o$
  d) $2\alpha_1+\alpha_2=180^o$ (hint: a two-link system with this configuration is called polar manipulator)
4. Derive the equations of motion and the torque terms using angles in the segmental space $(\theta_1,\,\theta_2)$.
5. Run the numerical simulations for the torques with different parameters.

## References

- Craig JJ (2005) [Introduction to Robotics: Mechanics and Control](http://books.google.com.br/books?id=MqMeAQAAIAAJ). 3rd Edition. Prentice Hall.
- Hollerbach JM, Flash T (1982) [Dynamic interactions between limb segments during planar arm movement](http://link.springer.com/article/10.1007%2FBF00353957). Biological Cybernetics, 44, 67-77.
- Nigg BM and Herzog W (2006) [Biomechanics of the Musculo-skeletal System](https://books.google.com.br/books?id=hOIeAQAAIAAJ&dq=editions:ISBN0470017678). 3rd Edition. Wiley.
- Pandy MG (2001) [Computer modeling and simulation](https://drive.google.com/open?id=0BxbW72zV7WmUbXZBR2VRMnF5UTA&authuser=0). Annu. Rev. Biomed. Eng., 3, 245–73.
- Sherman MA, Seth A, Delp SL (2013) [What is a moment arm? Calculating muscle effectiveness in biomechanical models using generalized coordinates](http://simtk-confluence.stanford.edu:8080/download/attachments/3376330/ShermanSethDelp-2013-WhatIsMuscleMomentArm-Final2-DETC2013-13633.pdf?version=1&modificationDate=1369103515834) in Proc. ASME Int. Design Engineering Technical Conferences (IDETC), Portland, OR, USA.
- Zajac FE (1993) [Muscle coordination of movement: a perspective](http://e.guigon.free.fr/rsc/article/Zajac93.pdf). J Biomech., 26, Suppl 1:109-24.
- Zajac FE, Gordon ME (1989) [Determining muscle's force and action in multi-articular movement](https://drive.google.com/open?id=0BxbW72zV7WmUcC1zSGpEOUxhWXM&authuser=0). Exercise and Sport Sciences Reviews, 17, 187-230.
- Zatsiorsky VM (2002) [Kinetics of human motion](http://books.google.com.br/books?id=wp3zt7oF8a0C). Human Kinetics.


```python
%run ../../common/import_all.py

from common.setup_notebook import set_css_style, setup_matplotlib, config_ipython

from sklearn.cluster import KMeans
from sklearn.preprocessing import scale

from common.kmeans_helpers import (retrieve_cluster_points,
                                   compute_wcss,
                                   compute_gap_statistic,
                                   evaluate_gap_statistic_best_k)

dataset = '../../datasets/oldfaithful.txt'

config_ipython()
setup_matplotlib()
set_css_style()
```

# K-means

The k-means is a clustering algorithm. Not to be confused with kNN!

## How it works

The $K$-means clustering algorithm needs you, the user, to choose $K$, which is the number of clusters you want to end up with. It is the fastest of the clustering methods and it always converges, though it may do to a local minimum.

 Given a set of $n$ observations, $(\mathbf{x_1}, \mathbf{x_2}, \ldots, \mathbf{x_n})$, each living in a $d$-dimensional space ($\mathbf{x} \in \mathbb{R}^d$), where $d$ are the features, the algorithms wants to partition them into $K \leq n$ clusters $\{\mathcal{C}_1, \mathcal{C}_2, \ldots, \mathcal{C}_n\}$  so to minimise the within-cluster-sum-of-squares (*WCSS*, $W$):

$$
\boxed{W = \sum_{k=1}^{K} \sum_{\mathbf{x} \in \mathcal{C}_k} || \mathbf{x} - \mathbb{\mu}_k ||^2} \ ,
$$

where $\mathbb{\mu}_k$ is the mean of the points falling in cluster $C_k$ (the so-called *centroid* of the cluster), itself a vector in $\mathbb{R}^d$, each of its coordinates being the mean for that feature in the cluster.

In this horrible figure I did here, $K$ is two and the red crosses indicate the centroids.

## The standard algorithm

The standard $K$-means algorithm is [[that of Lloyds]](#lloyds) (dated 1957, was only published outside of Bell Labs in 1982).

In brief, the algorithms starts by initialising the $K$ centroids with points picked randomly from the sample. It then proceeds iteratively, until convergence, through steps called

1. *assignment*
2. *update*

The *assignment* step consists in assigning each sample point to the cluster whose centroid is the nearest; this procedure is equivalent to partitioning the samples according to the Voronoi diagram generated by the centroids. The voronoi diagram is a way to partition a plane in regions based on the distance to points.
The *update* step calculates the new centroids with the new cluster configuration.

These steps are executed one after the other until the difference between new and old centroids does not exceed a chosen threshold.

## Further metrics

On top of the *WCSS*, we can use other metrics to assess and monitor the quality and the nature for our clustering.

### Compactness

We can assess how "compact" our clustering is via summing up all the intra-cluster distances for each cluster, divided by twice the number of points in each of them:

$$
\boxed{c = \sum_{k=1}^K \frac{1}{2n_k} d_k} \ .
$$

To expand on the above, $n_k$ is the number of points in cluster $\mathcal{C}_k$ identified by index $k$, with $k \in \{1, \ldots, K\}$, and $d_k$ is the sum of distances within the cluster:

$$
d_k = \sum_{\mathbf{x} \in \mathcal{C}_k} \sum_{\mathbf{y} \in \mathcal{C}_k} || \mathbf{x} - \mathbf{y} ||^2 \ ,
$$

and it can be written as ($\mathbf{\mu}_k$ being the centroid of the cluster)

$$
2 n_k \sum_{\mathbf{x} \in C_k} || \mathbf{x} - \mathbf{\mu}_k ||^2 \ .
$$

__Proof__

By definition, we have ($\mu_k$ is the centroid of $\mathcal{C}_k$)

$$
\mathbf{\mu}_k = \frac{\sum_{\mathbf{x} \in \mathcal{C}_k} \mathbf{x}}{n_k}
$$

because each component of $\mathbf{\mu}_k$ is the mean of the corresponding components of the points in the cluster.

The first member is (we use the fact, in the second line, that $\sum_{\mathbf{x}} 1 = n_k$):

$$
\begin{align}
\sum_{\mathbf{x}} \sum_{\mathbf{y}} (||\mathbf{x}||^2 + ||\mathbf{y}||^2 - 2 \mathbf{x} \cdot \mathbf{y}) &=
\sum_{\mathbf{x}} \sum_{\mathbf{y}} ||\mathbf{x}||^2 + \sum_{\mathbf{x}} \sum_{\mathbf{y}} ||\mathbf{y}||^2 - 2 \sum_{\mathbf{x}} \sum_{\mathbf{y}} \mathbf{x} \cdot \mathbf{y} \\
&= n_k \sum_{\mathbf{x}} ||\mathbf{x}||^2 + n_k \sum_{\mathbf{y}} ||\mathbf{y}||^2 -2 n_k \sum_{\mathbf{x}} \mathbf{x} \cdot \mathbf{\mu}_k \\
&= 2 n_k \sum_{\mathbf{x}} ||\mathbf{x}||^2 - 2n_k^2 ||\mu_k||^2
\end{align}
$$

The second member is

$$
\begin{align}
2 n_k \big[ \sum_{\mathbf{x}} ||\mathbf{x}||^2 + \sum_{\mathbf{x}} ||\mu_k||^2 -2 \sum_{\mathbf{x}} \mathbf{x} \cdot \mathbf{\mu}_k \big] &= 2 n_k \big[ \sum_{\mathbf{x}} ||\mathbf{x}||^2 + n_k ||\mu_k||^2 - 2 n_k ||\mu_k||^2 \big] \\
&= 2 n_k \big[ \sum_{\mathbf{x}} ||\mathbf{x}||^2 - n_k ||\mu_k||^2 \big] \\
&= 2 n_k \sum_{\mathbf{x}} ||\mathbf{x}||^2 -  2 n_k^2 ||\mu_k||^2 \ ,
\end{align}
$$

so the two things are the same.

## How to choose the best $K$?

Clearly, if I choose a too large $K$, I end up with a too-fine clustering which fails in showing me similarities in the data. On the flip side, If I choose a too small $K$, I also fail in separating similar points for the reverse reason, as I'm putting too much stuff together. How should I go with choosing the $K$ that best separates my points?

There can be (at least) three ways: one rule of thumb, one heuristic and one mathematical.

### The rule of thumb

The *rule of thumb* method suggests choosing $k \sim \sqrt{n/2}$. This is a (very) hand-waving method, and gives an "approximate" clustering (it typically gives too many clusters). The justification behind it comes from the fact that it runs in linear time so it really should only be taken as an indication when wanting to reduce dataset size.

### The elbow method

The *elbow method* is a heuristic method consisting in looking at when adding a new cluster does not model the data any better.

By plotting $W$ as a function of $k$, it should be clear where the contribution of another cluster gives minimal further gain (the "elbow" point of the curve). It is clear that the choice of this point is kind of subjective anyway, as it depends on the situation and on the what we, in that situation, consider a minimal further gain not worth pursuing.

See [[3]](#3) for an computational answer.

### The Gap Statistic method

It is a mathematical presented in the original paper in [[5]](#5), you can read an explanation plus implementation in [[4]](#4). The authors define the gap statistic as

$$
G(K) = \mathbb{E}[\log W^*_K] - \log W_K \ ,
$$

where the asterisk refers to a null reference dataset. It is demonstrated that the best $K$ clustering the data is the smallest $K$ such that

$$
G(K) \geq G(K+1) - s_{K+1} \ ,
$$

where $s_K$ is the simulation error calculated from the standard deviation $\sigma_K$ of B Monte Carlo replicates $\log(W^*_K)$, as

$$
s_K = \sqrt{1 + \frac{1}{B}} \sigma_K \ ,
$$

and

$$
\mathbb{E}[\log W^*_K] = \frac{1}{B} \sum_{b=1}^{B} \log W^*_{Kb} \ ,
$$

$W^*_{Kb}$ given by the clustering of $B$ reference datasets.

The reference distribution is obtained by generating each reference feature randomly (uniformly) within the range values for that feature, that is, within the bounding box for that feature. Then the expected value is estimated by an average of $B$ copies $\log W^*_K$, each computed from a Monte Carlo sample $x^*_1, \ldots, x^*_n$, drawn from a reference distribution (x is the dataset matrix).

Because

$$
\mathbb{E}[\log W^*_K] = \frac{1}{B} \log \Pi_b W^*_{Kb} \ ,
$$

then

$$
G(K) = \log \left[ \frac{(\Pi_b W^*_{Kb})^{\frac{1}{B}}}{W_K} \right]
$$

In the logarithm, at the denominator, we got the geometric mean of $W^*_K$. This last equation is saying that the gap statistics is the logarithm of the ratio of the geometric mean of $W^*_K$ to $W_K$.

The standard deviation is

$$
\sigma_K = \sqrt{\frac{1}{B} \sum_{b=1}^B (\log W^*_{Kb} - \mathbb{E}[\log W^*_{Kb}])^2 }
$$

There is an implementation of this in this repo anyway, which we will use later on.

## Trying this!

### Let's use the "old faithful" dataset

```python
# Let's read the Old Faithful dataset into a Pandas Dataframe, choosing the columns

data = pd.read_csv(dataset, delimiter=' ')[['eruptions', 'waiting']]

data.head()
```

      eruptions
      waiting

      0
      3.600
      79

      1
      1.800
      54

      2
      3.333
      74

      3
      2.283
      62

      4
      4.533
      85

### Scale the data so that each column is in the same scale

```python
scaled_data = scale(data)
```

### First, see how it looks like

It looks that 2 groups are naturally emerging from the data.

```python
data.plot.scatter('eruptions', 'waiting')
plt.title('Scatter of the data in the "old faithful" dataset')
plt.show();
```

### What does the rule of thumb say?

```python
thumb_k = np.sqrt(data.shape[0]/2)
print(' * Rule of thumb asserts optimal k =', round(thumb_k, 2))
```

     * Rule of thumb asserts optimal k = 11.66

### Using the elbow method

You'll see the elbow seems to appear quite soon.

```python
k_range = range(1, 21)            # range of K (decide arbitrarily how many Ks to test)
inertia_k = dict()                # inertia for each k
wcss_k = dict()                   # WCSS for each k
p_k = dict()                      # percentage of variance explained for each k

# Loop over the value of K
for k in k_range:
    print('k = ', k)

    # Fit the model
    fit = KMeans(n_clusters=k).fit(scaled_data)

    # Retrieve centroids for fitted model
    # A centroid is the cluster center, given as the vector of coordinates over the n_features dimensions
    # So there will be K number of n_features-dimensional centroids
    centroids = fit.cluster_centers_

    # Retrieve cluster labels for fitted model
    labels = fit.labels_

    # Retrieve the inertia of the fitted model
    inertia_k[k] = fit.inertia_

    # Compute the WCSS of the fitted model
    wcss_k[k] = compute_wcss(centroids, labels, scaled_data)

plt.title('WCSS vs. K')
plt.plot(k_range, list(wcss_k.values()), marker='o')
plt.xlabel('K')
plt.xticks(k_range)
plt.ylabel('WCSS')
plt.show();
```

### Run a clustering

```python
k = 5                          # choose K

fit = KMeans(n_clusters=k).fit(scaled_data)
centroids = fit.cluster_centers_
labels = fit.labels_

labels
```

    array([2, 1, 2, 3, 0, 3, 0, 0, 1, 0, 1, 0, 4, 1, 0, 1, 3, 0, 1, 4, 1, 1, 2,
           2, 4, 2, 1, 2, 2, 4, 2, 4, 2, 4, 2, 1, 1, 4, 3, 0, 4, 3, 0, 3, 4, 2,
           2, 1, 4, 3, 4, 0, 1, 4, 1, 4, 2, 3, 4, 4, 3, 0, 1, 4, 3, 0, 4, 4, 3,
           4, 4, 3, 4, 2, 3, 4, 3, 4, 2, 2, 2, 4, 2, 3, 2, 0, 2, 4, 1, 0, 3, 0,
           1, 4, 3, 2, 0, 2, 1, 4, 3, 0, 1, 0, 4, 1, 0, 1, 0, 2, 4, 3, 0, 4, 3,
           4, 1, 0, 3, 0, 1, 2, 4, 3, 0, 2, 1, 4, 3, 0, 1, 0, 3, 0, 1, 4, 1, 0,
           1, 2, 4, 3, 4, 4, 4, 3, 4, 1, 0, 1, 4, 2, 3, 4, 2, 2, 4, 0, 1, 0, 1,
           0, 3, 2, 2, 4, 3, 0, 1, 0, 1, 3, 4, 2, 4, 4, 4, 1, 0, 2, 1, 4, 0, 2,
           1, 4, 0, 1, 0, 3, 4, 3, 4, 0, 2, 4, 0, 4, 1, 4, 3, 4, 0, 1, 4, 1, 4,
           0, 1, 0, 3, 4, 1, 2, 2, 4, 1, 0, 1, 2, 1, 4, 1, 4, 2, 4, 2, 4, 2, 4,
           2, 1, 0, 1, 0, 1, 1, 4, 2, 3, 2, 1, 0, 3, 0, 2, 3, 4, 3, 4, 1, 0, 2,
           4, 0, 2, 2, 0, 3, 4, 4, 0, 3, 0, 1, 3, 4, 4, 1, 0, 1, 4], dtype=int32)

### Compute the gap statistic

```python
k_gap = {}
for k in k_range:
    gap = compute_gap_statistic(scaled_data, k, scale_data=False)
    k_gap[k] = gap

k_gapdiff = {}
for k in range(min(k_range), max(k_range)-1):
    k_gapdiff[k] = k_gap[k][0] - k_gap[k+1][0] - k_gap[k + 1][2]

'The best k according to the gap statistic is: ', evaluate_gap_statistic_best_k(scaled_data)
```

    ('The best k according to the gap statistic is: ', 2)

```python
plt.plot(list(k_gap.keys()), [value[0] for value in k_gap.values()])
plt.title('Gap statistic')
plt.ylabel('$G$')
plt.xlabel('$K$')
#plt.grid()
plt.show();
```

```python
plt.bar(list(k_gapdiff.keys()), k_gapdiff.values())
plt.title('Gap diff')
plt.ylabel('$G_K - G_{K+1} + s_{K+1}$')
plt.xlabel('$K$')
plt.ylim(-0.8, 0.1)
plt.show();
```

### Scatter plot of two given features with the respective cluster colour and centroid

```python
# Choose feature indices from the header
f1 = 1
f2 = 2

k = 5

# List of colours: if len is < k, add some
colours = {
    'red': 'r',
    'black': 'k',
    'blue': 'b',
    'green': 'g',
    'grey': '#929090',
    'pink': '#FFB6C1',
    'light_blue': '#00BFFF',
    'light_green': '#29E191'
}

plt.title('Scatter plot of two features, k clusters', fontweight='bold', fontsize=16)
plt.xlabel('eruptions')
plt.ylabel('waiting')
plt.grid()

# Note: plot displays the scaled data
for cluster_index in range(k):
    cluster_color = colours[list(colours)[cluster_index]]
    cluster_points = retrieve_cluster_points(cluster_index, labels, scaled_data)
    plt.scatter([point[f1-1] for point in cluster_points],
                [point[f2-1] for point in cluster_points], color=cluster_color)
    plt.scatter(centroids[cluster_index,f1-1], centroids[cluster_index, f2-1], marker='x', s=200, color=cluster_color)
plt.show();
```

## References

1.  S Lloyds, [Least squares quantization in PCM](http://www.cs.nyu.edu/~roweis/csc2515-2006/readings/lloyd57.pdf), *IEEE transactions on information theory*, 28.2, 1982
2.  [Stack Overflow on the elbow method](http://stackoverflow.com/questions/6645895/calculating-the-percentage-of-variance-measure-for-k-means)
3.  [This great blog on the gap statistic, with a Python implementation and examples](https://datasciencelab.wordpress.com/2013/12/27/finding-the-k-in-k-means-clustering/)
4.  Tibshirani, Walther, Hastie, [Estimating the number of clusters in a data set via the gap statistic](http://web.stanford.edu/~hastie/Papers/gap.pdf), *J R Statistic Society*, 63:2, 2001
5.  [K-means is no free lunch](http://varianceexplained.org/r/kmeans-free-lunch/)

```python

```


# 低次の多変量テイラー展開式を導出

```python
import sympy as sy
from sympy import diff
from sympy.printing.numpy import NumPyPrinter

import original_kinematics

from pathlib import Path
import os
```

必要なシンボリック変数を宣言

```python
N = 3  # テイラー展開の次数

# アクチュエータベクトル
l1, l2, l3 = sy.symbols("l1, l2, l3")
q = sy.Matrix([[l1, l2, l3]]).T

# ディスク位置を表すスカラ変数
xi = sy.Symbol("xi")

l1a, l2a, l3a = sy.symbols("l1a, l2a, l3a")  # 平衡点
```

テイラー展開する対象の式を導出

```python
kinema = original_kinematics.Local()

P = kinema.P(q, xi)
R = kinema.R(q, xi)

```

テイラー展開式を導出

```python
a, b, c = sy.symbols("a, b, c")
((a+b+c)**2).expand()
```

$\displaystyle a^{2} + 2 a b + 2 a c + b^{2} + 2 b c + c^{2}$

```python
((a+b+c)**3).expand()
```

$\displaystyle a^{3} + 3 a^{2} b + 3 a^{2} c + 3 a b^{2} + 6 a b c + 3 a c^{2} + b^{3} + 3 b^{2} c + 3 b c^{2} + c^{3}$

テイラー展開（マクローリン展開）を実行

```python

def multi_maclaurin_2(f):
    """a周りの2次の多変数テイラー展開"""
    f0 = f.subs([(l1, l1a), (l2, l2a), (l3, l3a)])

    f1 = diff(f, l1).subs([(l1, l1a), (l2, l2a), (l3, l3a)])*(l1-l1a) +\
        diff(f, l2).subs([(l1, l1a), (l2, l2a), (l3, l3a)])*(l2-l2a) +\
            diff(f, l3).subs([(l1, l1a), (l2, l2a), (l3, l3a)])*(l3-l3a)

    f2 = 1/2*(
        diff(diff(f, l1), l1).subs([(l1, l1a), (l2, l2a), (l3, l3a)])*(l1-l1a)**2 +\
            diff(diff(f, l2), l2).subs([(l1, l1a), (l2, l2a), (l3, l3a)])*(l2-l2a)**2 +\
                diff(diff(f, l3), l3).subs([(l1, l1a), (l2, l2a), (l3, l3a)])*(l3-l3a)**2 +\
                    2*diff(diff(f, l1), l2).subs([(l1, l1a), (l2, l2a), (l3, l3a)])*(l1-l1a)*(l2-l2a) +\
                        2*diff(diff(f, l2), l3).subs([(l1, l1a), (l2, l2a), (l3, l3a)])*(l2-l2a)*(l3-l3a) +\
                            2*diff(diff(f, l3), l1).subs([(l1, l1a), (l2, l2a), (l3, l3a)])*(l3-l3a)*(l1-l1a)
    )

    return f0 + f1 + f2

P_series = []
for i in range(3):
    P_series.append(
        multi_maclaurin_2(P[i, 0])
    )

R_series = [[]]*3
for i in range(3):
    for j in range(3):
        R_series[i].append(
            multi_maclaurin_2(R[i, j])
        )

```

```python
# 微小値をlaに代入
for i in range(3):
    P_series[i] = P_series[i].subs([(l1a, 0.0001), (l2a, 0.002), (l3a, 0.0015)])

for i in range(3):
    for j in range(3):
        R_series[i][j] = R_series[i][j].subs([(l1a, 0.0001), (l2a, 0.002), (l3a, 0.0015)])
```

## 結果を出力

最後に自分で微小項や発散するを削除する必要あり

```python
cwd = str(Path().resolve())
base = cwd + "/maclaurin_2"

dir_name = base
os.makedirs(dir_name, exist_ok=True)

numpy_word = "import numpy\ndef f(q, xi):\n    l1, l2, l3 = q[0,0], q[1,0], q[2,0]\n\n    return "

for i in range(3):
    f = open(dir_name + "/P_" + str(i) + ".py", 'w')
    f.write(numpy_word)
    f.write(NumPyPrinter().doprint(P_series[i]))
    f.close()

for i in range(3):
    for j in range(3):
        f = open(dir_name + "/R_" + str(i) + "_" + str(j) + ".py", 'w')
        f.write(numpy_word)
        f.write(NumPyPrinter().doprint(R_series[i][j]))
        f.close()
```

```python
R_series[0][0]
```

$\displaystyle 0.5 \left(l_{1} - 0.0001\right)^{2} \left(- 6.07837243596557 \cdot 10^{-5} \xi^{2} \cos{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} - 1.4723987711447 \xi \sin{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} - 121438.163869571 \cos{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} + 121438.163869571\right) + 0.5 \left(l_{1} - 0.0001\right) \left(l_{2} - 0.002\right) \left(8.8412689977681 \cdot 10^{-5} \xi^{2} \cos{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} - 4.09059149510746 \xi \sin{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} - 387780.362371481 \cos{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} + 387780.362371481\right) + 0.5 \left(l_{1} - 0.0001\right) \left(l_{3} - 0.0015\right) \left(3.31547587416304 \cdot 10^{-5} \xi^{2} \cos{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} + 7.03538903739686 \xi \sin{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} + 630656.690110624 \cos{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} - 630656.690110624\right) + \left(l_{1} - 0.0001\right) \left(0.00754103754629469 \xi \sin{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} - 73.0683388245297 \cos{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} + 73.0683388245297\right) + 0.5 \left(l_{2} - 0.002\right)^{2} \left(- 3.21500690827931 \cdot 10^{-5} \xi^{2} \cos{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} + 0.0899501794735671 \xi \sin{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} - 133709.809902707 \cos{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} + 133709.809902707\right) + 0.5 \left(l_{2} - 0.002\right) \left(l_{3} - 0.0015\right) \left(- 2.41125518120948 \cdot 10^{-5} \xi^{2} \cos{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} + 3.91069113616032 \xi \sin{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} + 655199.982176895 \cos{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} - 655199.982176895\right) + \left(l_{2} - 0.002\right) \left(- 0.00548439094275978 \xi \sin{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} - 204.591348708683 \cos{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} + 204.591348708683\right) + 0.5 \left(l_{3} - 0.0015\right)^{2} \left(- 4.52110346476778 \cdot 10^{-6} \xi^{2} \cos{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} - 5.47304008677859 \xi \sin{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} - 642928.33614376 \cos{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} + 642928.336143759\right) + \left(l_{3} - 0.0015\right) \left(- 0.00205664660353492 \xi \sin{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} + 277.659687533213 \cos{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} - 277.659687533213\right) + 0.935567010309278 \cos{\left(1.42156017576933 \cdot 10^{-5} \xi \right)} + 0.0644329896907217$