# Gradient Descent--Huber Loss
### Author: Jayanth Raman

**Date: 10 Dec 2019** v2

HomeWork2 of Machine Learning Basics, Tokyo DataScience

# Huber Loss
Point-wise Huber loss is defined piecewise as:
$$
L_\gamma (e_i) =
\begin{cases}
 e_i^2                   & \text{for } |e_i| \le \gamma, \quad \gamma > 0\\
 \gamma (2 |e_i| - \gamma), & \text{otherwise.}
\end{cases}
$$
where, $\gamma > 0$ is a threshold and it defines the point where the Huber loss changes from a quadratic to a linear function or vice versa.  And $e_i$ are the individual data points.

The overall Huber loss of $M$ data points is the average:
$$
L_\gamma (e) = \frac{1}{M} \sum_{i=1}^{M} L_\gamma (e_i)
$$
for the $M$ points of $e_i$.

When $e$ is the error, $e = \hat{y} - y$, then the point-wise loss is
$$
L_\gamma (y_i, \hat{y_i}) =
\begin{cases}
 (\hat{y_i} - y_i)^2                   & \text{for } |\hat{y_i} - y_i| \le \gamma, \quad \gamma > 0\\
 \gamma (2 |\hat{y_i} - y_i| - \gamma), & \text{otherwise.}
\end{cases}
$$
where, $\hat{y}$ is the prediction of $y$.

And the overall Huber loss is the average
$$
L_\gamma (y, \hat{y}) = \frac{1}{M} \sum_{i=1}^{M} L_\gamma (y_i, \hat{y_i})
$$

[From [Wikipedia](https://en.wikipedia.org/wiki/Huber_loss)]

For a derivation of the linear term so that it is continuous with the quadratic term, see [Appendix A](#Appendix-A----Huber-Loss-Derivation).

## Gradients
A polynomial model is defined as
$$
\hat{y}_i = \sum_{d=0}^{N} w_d x^d_i
$$
where $\hat{y}_i$ is the predicted value of $y_i$ at $x_i$ and $w_d$ are the weights.

For the Huber-loss model, the first gradient component (in the $w_0$ direction) is
$$
\frac{\partial L_\gamma}{\partial w_0} =
\begin{cases}
\frac{2}{M} \sum_{i=1}^{M} (\hat{y_i} - y_i)  & |\hat{y_i} - y_i| < \gamma \\
\frac{2\gamma}{M} \sum_{i=1}^{M} \text{sgn}(\hat{y_i} - y_i) & \text{otherwise}
\end{cases}
$$
where $\text{sgn}$ is the signum or sign function.

Let $e_i = \hat{y_i} - y_i$.  Notice that, when $e_i < \gamma$, then each term in
the summation is $\text{sgn}(e_i)\,|e_i|$.  And, when $e_i \ge \gamma$, then each term is
$\text{sgn}(e_i)\,\gamma$.  Hence, each term in the summation is $\text{sgn}(e_i)\,\min(|e_i|, \gamma)$.

Hence the first component of the gradient can be simplified to:
$$
\frac{\partial L_\gamma}{\partial w_0} =
\frac{2}{M} \sum_{i=1}^{M} \text{sgn}(e_i) \, \min \left( |e_i|, \gamma \right)
$$
where $e_i = \hat{y_i} - y_i$.  And the above expression can be computed as an average.

The other gradient components are defined recursively as
$$
\frac{\partial L_\gamma}{\partial w_d} = x \frac{\partial L_\gamma}{\partial w_{d - 1}} \quad d=1, 2, ..., N
$$

For a derivation of the above, please see [Appendix B](#Appendix-B----Gradient-for-Huber-Loss).

# Summary of Findings
Synthetic data was generated with a few outliers.  The data, $(x_i, y_i)$, is linear:
$$
y_i = w_0 + w_1 x_i
$$

Then, two models were fit:
 * One with an MSE-loss function.
 * Another with a Huber-loss function.  The parameter, $\gamma$, was chosen to be
   twice the standard deviation of the error in the MSE-loss model.

The models were compared using MSE loss (which may be an advantage to the MSE-loss model when
all of the data is considered).  MSE loss was computed for all of the data and also
just for the unaffected data (i.e. data without the outliers).

With all of the data in consideration, both models performed about the same with the MSE-loss model slightly outperforming the Huber-loss model.

But, when only the unaffected data was considered, the Huber-loss model outperformed the MSE-loss model.  The MSE-loss for the two models is shown in the table below.  The MSE loss for the Huber-loss model is about 38% lower than that for the MSE-loss model, when only the unaffected data is considered.  For more details, see [here](#Comparison-of-MSE-Loss).

The models were also trained on the same synthetic data, but with the outliers suppressed.  Both models were similar and both had an MSE-loss of approximately 0.1170.

| Model            | Loss (unaffected data) | Loss (all data) | Loss (no outliers) |
|------------------|-----------------------:|----------------:|-------------------:|
| MSE-loss Model   |                 0.2468 |          1.9935 |             0.1170 |
| Huber-loss Model |                 0.1516 |          2.0246 |             0.1171 |

The code below is easily applicable to quadratic data or any higher degree polynomials.

# Calculations and Observations

What follows is the source code for performing the calculations and the comparisons.

```python
# Imports
import bokeh
import bokeh.plotting as bkp
import functools
import numpy as np
import pandas as pd   # for summary statistics
#
bkp.reset_output()
bkp.output_notebook()
```

    Loading BokehJS ...

The function to compute $y$ as a polynomial of $x$ is given below.

```python
# Function to generate `y` given `x` and a set of weights.
def polynomial_function(weights, x):
    '''
    weights = [a0, a1, a2, ...]
    output = a0 + a1 * x + a2 * x ** 2 + ...
    '''
    if isinstance(x, list):
        x = np.array(x)
    out = weights[0]
    xn = x
    for w in weights[1:]:
        out = out + w * xn
        xn = xn * x
    return out

def test_polynomial_function():
    x1 = 3.1415
    tmp1 = 4 + 3.2 * x1 + 2.1 * x1 * x1 + 8.53 * x1 * x1 * x1
    assert polynomial_function([6.24, 4.14], x1) == 6.24 + 4.14 * x1
    assert polynomial_function([4, 3.2, 2.1, 8.53], x1) == tmp1
    assert all(polynomial_function([4, 3.2, 2.1, 8.53], [x1, x1]) == np.array([tmp1, tmp1]))
test_polynomial_function()
```

A helper function to generate training data is given below.  Outliers are added using samples from a binomial distribution.  Gaussian noise is added to all data.  The function outputs $x$, $y$ with no outlier noise, and outlier noise.  In order to generate $y$ with outliers, the outlier noise vector is added.

```python
def gen_training_data(wt_true, npoints=50, gauss_noise_sd=0.4, binomp={'p': 0.03, 'mag': 6.0}, seed=414243):
    '''Generate (simulate) training data'''
    np.random.seed(seed)
    xtrain = np.random.random(npoints)
    # e.g. 6 * binomial(1, 0.03, size=n) - 6 * binomial(1, 0.03, size=n)
    binomial = np.random.binomial
    ntrials = 1
    outlier_noise = binomial(ntrials, binomp['p'], size=npoints) - binomial(ntrials, binomp['p'], size=npoints)
    outlier_noise = outlier_noise * binomp['mag']
    ytrain = polynomial_function(wt_true, xtrain) + gauss_noise_sd * np.random.randn(npoints)
    return xtrain, ytrain, outlier_noise
```

We define two update functions, one using an MSE loss function, and another using a Huber loss function.
We will refer to the former as the MSE-loss model and the latter as the Huber-loss model.

```python
def update_weights_mse(weights, x, y, learning_rate):
    y_predicted = polynomial_function(weights, x)
    derivative_of_loss = 2 * (y_predicted - y)
    weights[0] -= learning_rate * derivative_of_loss.mean()
    m1 = derivative_of_loss.mean()
    for ii in range(1, len(weights)):
        derivative_of_loss = x * derivative_of_loss
        weights[ii] -= learning_rate * derivative_of_loss.mean()
    return None

def update_weights_huber(weights, x, y, learning_rate, gamma):
    y_predicted = polynomial_function(weights, x)
    err = y_predicted - y
    derivative_of_loss = 2.0 * np.sign(err) * np.minimum(gamma, np.abs(err))
    weights[0] -= learning_rate * derivative_of_loss.mean()
    m1 = derivative_of_loss.mean()
    for ii in range(1, len(weights)):
        derivative_of_loss = x * derivative_of_loss
        weights[ii] -= learning_rate * derivative_of_loss.mean()
    return None
```

We also define helper functions to compute the MSE loss and the Huber loss.

```python
def mse_loss(y, yhat):
    return np.mean((y - yhat) ** 2)

def huber_loss(y, yhat, gamma):
    errabs = np.abs(y - yhat)
    loss = np.where(errabs <= gamma, (errabs ** 2), gamma * (2.0 * errabs - gamma))
    return np.mean(loss)

# test
def test():
    wt_true = [3.0, 6.0]
    xtrain, ytrain, outlier_noise = gen_training_data(wt_true, npoints=70)
    _ytrue = polynomial_function(wt_true, xtrain)
    ytrain = ytrain + outlier_noise
    m1, m2 = mse_loss(_ytrue, ytrain), mse_loss(ytrain, _ytrue)
    print('MSE loss:', m1, m2)
    assert m1 == m2
    h1, h2 = huber_loss(_ytrue, ytrain, np.inf), huber_loss(ytrain, _ytrue, np.inf)
    print('Huber loss, gamma=np.inf:', h1, h2)
    assert h1 == h2
    assert h1 == m1
    gamma = np.std(ytrain)
    h1, h2 = huber_loss(_ytrue, ytrain, gamma), huber_loss(ytrain, _ytrue, gamma)
    print(f'Huber loss, gamma={gamma:.4f}: {h1}, {h2}')
    assert h1 == h2
test()
```

    MSE loss: 2.1804075712825233 2.1804075712825233
    Huber loss, gamma=np.inf: 2.1804075712825233 2.1804075712825233
    Huber loss, gamma=1.9832: 1.252087574809613, 1.252087574809613

A helper function to do the training is defined below.  It takes as parameters the $x$ (`xtrain`) and $y$ (`ytrain`) data along with the learning rate and update functions and the number of iterations to run.  An optional loss function, if provided, results in the loss being computed and returned.  This is useful in checking if the training has progressed properly.  The iteration interval when the loss is sampled can also be provided as `loss_iter`.  Optional initial weights can also be provided.  This is useful in continuing a previous training run.  If not provided, random weights are generated.

```python
# Train
def train(xtrain, ytrain, nweights, update_func, niter, learning_rate=0.02,
          loss_func=None, init_weights=None, loss_iter=20):
    # initialize weights
    weights = np.random.randn(nweights) if init_weights is None else init_weights
    loss_values = []
    for ii in range(niter):
        update_func(weights, xtrain, ytrain, learning_rate)
        if loss_func and ii % loss_iter == (loss_iter - 1):
            loss_values.append(loss_func(polynomial_function(weights, xtrain), ytrain))
    return weights, loss_values
```

Training data is generated below using weights that define a linear relationship.  The number of outliers that were generated is shown below.

```python
wt_true = [3.0, 6.0]
xtrain, ytrain, outlier_noise = gen_training_data(wt_true, npoints=70)
print('Number of outliers:', sum(np.where(outlier_noise, 1, 0)))
```

    Number of outliers: 4

Let's look at the summary statistics of the generated data, with and without outliers.

```python
display(pd.DataFrame({'y (no outliers)': ytrain}).describe())
print()
display(pd.DataFrame({'y (with outliers)': ytrain + outlier_noise}).describe())
```

      y (no outliers)

      count
      70.000000

      mean
      5.993224

      std
      1.806728

      min
      2.545869

      25%
      4.440494

      50%
      6.017799

      75%
      7.658137

      max
      8.887732

      y (with outliers)

      count
      70.000000

      mean
      5.993224

      std
      1.997503

      min
      0.680286

      25%
      4.440494

      50%
      6.017799

      75%
      7.735788

      max
      9.859972

As expected, the standard deviation of the data with outliers is higher than the one without.  The minimum and maximum values are also wider.  Otherwise the mean and median are unchanged since the positive and negative outliers are balanced in the data that was produced.

We plot the data below.  The outliers are plotted in a different color.  The dashed line corresponds to the true weights that generated the data.

```python
p = bkp.figure(title='Training Data With Outliers', y_range=[-1, 11], width=600, height=400)
xin, xout = xtrain[outlier_noise == 0], xtrain[outlier_noise != 0]
yin, yout = ytrain[outlier_noise == 0], (ytrain + outlier_noise)[outlier_noise != 0]
p.circle(xin, yin, radius=0.01, legend_label='unaffected data')
p.circle(xout, yout, radius=0.01, color='darkorange', legend_label='outliers')
line = bokeh.models.Slope(gradient=wt_true[1], y_intercept=wt_true[0],
                          line_color='gray', line_dash='dashed', line_width=3.5)
p.add_layout(line)
p.legend.location = 'bottom_left'
p.legend.border_line_alpha = 0.4
p.legend.border_line_color = 'black'
bkp.show(p)
```

## Predictions Without Outliers
First, we compare predictions using the MSE-loss and Huber-loss models using data without outliers i.e. we do not yet add the noise that will move some of the points and make them outliers.

### MSE-loss Model
First, we fit the MSE-loss model.

```python
# No outliers - Predict using MSE Loss
np.random.seed(424242)
weights_mse, loss_values_mse = train(xtrain, ytrain, nweights=2, update_func=update_weights_mse,
                                     loss_func=mse_loss, niter=250 * 20)
```

```python
print('loss values (last few):', loss_values_mse[-4:])
print('True weights:', wt_true)
print('Predicted weights:', weights_mse)
yhat = polynomial_function(weights_mse, xtrain)
print('MSE loss:', mse_loss(yhat, ytrain))
```

    loss values (last few): [0.11702782079848476, 0.11702782079843213, 0.11702782079838533, 0.11702782079834366]
    True weights: [3.0, 6.0]
    Predicted weights: [3.07659014 5.79207763]
    MSE loss: 0.11702782079834366

Notice that the loss values have stabilized.  Later, we also plot the loss against the iteration steps to drive home this point.

The predicted weights are close to the true weights.  And the MSE loss of the predictions is small compared to either the average value or the median value of $y$.

The error distribution is plotted below.  The two dashed lines are plotted at $\pm \gamma$.  The axis limits are set so that we can later directly compare it to the error generated with the outliers added.

In the Huber-loss model, $\gamma$ will be set to twice the standard deviation of the error.

```python
# Plot the errors
p = bkp.figure(title='Error', x_range=[-7, 7], y_range=[0.5, 0.8], width=600, height=200,
               x_axis_label='error=(yhat - y)')
err = polynomial_function(weights_mse, xtrain) - ytrain
p.circle(err, 0.6, fill_color='blue')
gamma = 2 * np.std(err)
p.line([-gamma, -gamma], [0, 1], line_dash='dashed', line_width=2)
p.line([gamma, gamma], [0, 1], line_dash='dashed', line_width=2)
p.yaxis.visible = False
bkp.show(p)
```

The error data is bunched up near the origin (again, the x-axis scale is set to account for the outliers which come later).  The lines at $\pm \gamma$ are at the data edges and there are really no outliers to be separated here.

### Huber-loss Model
Next we predict using the Huber-loss model.  We use twice the standard deviation of the error from the MSE-loss model as the $\gamma$ value.

```python
# No outliers - Predict using Huber Loss
np.random.seed(424242)
# gamma = np.std(ytrain + outlier_noise)
gamma = 2 * np.std(polynomial_function(weights_mse, xtrain) - ytrain)
print('gamma:', gamma)
update_func = functools.partial(update_weights_huber, gamma=gamma)
loss_func = functools.partial(huber_loss, gamma=gamma)
weights_huber, loss_values_huber = train(xtrain, ytrain, nweights=2, update_func=update_func,
                                         loss_func=loss_func, niter=250 * 20)
```

    gamma: 0.6841865850726339

```python
print('gamma:', gamma)
print('loss values (last few):', loss_values_huber[-4:])
print('True weights:', wt_true)
print('Predicted weights:', weights_huber)
yhat = polynomial_function(weights_huber, xtrain)
print('MSE loss:', mse_loss(yhat, ytrain))
```

    gamma: 0.6841865850726339
    loss values (last few): [0.11154368969877192, 0.11154368969833425, 0.11154368969794404, 0.11154368969759612]
    True weights: [3.0, 6.0]
    Predicted weights: [3.07095171 5.78240498]
    MSE loss: 0.11714691181634618

Again, the loss values have stabilized.

The predicted weights are close to the true weights and are *practically the same as that from the MSE-loss model.*

Next, we plot the predictions of both models against the data.

```python
p = bkp.figure(title='Predictions -- Data Without Outliers', y_range=[-1, 11], width=600, height=400)
p.circle(xtrain, ytrain, radius=0.01)
xtmp = np.linspace(0, 1)
ytrue = polynomial_function(wt_true, xtmp)
ymse = polynomial_function(weights_mse, xtmp)
yhuber = polynomial_function(weights_huber, xtmp)
p.line(xtmp, ytrue, color='gray', line_width=3, line_dash='dashed', legend_label='true weight')
p.line(xtmp, ymse, color='blue', line_width=3, legend_label='MSE')
p.line(xtmp, yhuber, color='darkorange', line_width=3, legend_label='Huber')
p.legend.location = 'bottom_right'
bkp.show(p)
```

From the above figure, the three predictions are practically indistinguishable.

Both the models yielded the same **MSE loss of 0.117**.

The true weights and the weights from the predictions are below:

```python
print(f'True weights      : {wt_true}')
print(f'MSE-loss weights  : {weights_mse}')
print(f'Huber-loss weights: {weights_huber}')
```

    True weights      : [3.0, 6.0]
    MSE-loss weights  : [3.07659014 5.79207763]
    Huber-loss weights: [3.07095171 5.78240498]

### Training Loss
For completeness, we plot the training loss against the training iterations for both MSE and Huber losses.  The training loss decreases with the number of steps as expected.  The Huber-loss model converges a bit more slowly than the MSE-loss model.

```python
p = bkp.figure(title='Training Loss vs Iterations (no outliers)', width=600, height=400)
p.line(np.arange(len(loss_values_mse)) * 20, loss_values_mse, line_width=2, legend_label='MSE')
p.line(np.arange(len(loss_values_huber)) * 20, loss_values_huber, line_width=2,
       color='darkorange', legend_label='Huber')
p.xaxis.axis_label = 'Training Steps'
p.yaxis.axis_label = 'Loss'
bkp.show(p)
```

## Predictions With Outliers
We now add the outlier noise, thus creating the outliers, and then train the linear model using both the MSE-loss and Huber-loss models.

### MSE-loss Model
We train the MSE-loss model.

```python
# With outliers - Predict using MSE Loss
np.random.seed(424242)
weights_mse_ol, loss_values_mse_ol = train(xtrain, ytrain + outlier_noise, nweights=2,
                                           update_func=update_weights_mse,
                                           loss_func=mse_loss, niter=250 * 20)
```

```python
print('loss values (last few):', loss_values_mse_ol[-4:])
print('True weights:', wt_true)
print('Predicted weights:', weights_mse_ol)
yhat = polynomial_function(weights_mse_ol, xtrain)
print('MSE loss (all data):', mse_loss(yhat, ytrain + outlier_noise))
print('MSE loss (unaffected data):', mse_loss(yhat[outlier_noise == 0], ytrain[outlier_noise == 0]))
```

    loss values (last few): [1.9935106591383374, 1.993510659138311, 1.993510659138289, 1.9935106591382687]
    True weights: [3.0, 6.0]
    Predicted weights: [3.68645222 4.58096621]
    MSE loss (all data): 1.9935106591382687
    MSE loss (unaffected data): 0.24677995511981501

Notice that
 * the loss values have stabilized.
 * the predicted weights have diverged from the true weights, compared to the model build on the data without outliers.
 * the MSE loss for all of the data has increased to almost 2.0 from 0.117.
 * the MSE loss for the unaffected data has increased to 0.2468.

#### Error characteristics
Let's look at the error characteristics.  Ideally, one may want to base the value of the Huber-loss parameter, $\gamma$, based on the standard deviation of the error.

```python
yhat = polynomial_function(weights_mse_ol, xtrain)
err_mse_ol = yhat - ytrain - outlier_noise
pd.DataFrame({'error': err_mse_ol}).describe()
```

      error

      count
      7.000000e+01

      mean
      5.191414e-08

      std
      1.422112e+00

      min
      -5.657756e+00

      25%
      -2.994260e-01

      50%
      -1.110702e-02

      75%
      3.095180e-01

      max
      5.935515e+00

#### Choice of $\gamma$
For the Huber-loss function, we will choose $\gamma$ to be twice the standard deviation of the error.

The plot below shows the distribution of the error along with dashed lines at plus or minus one standard deviation of the error.  Observe that the lines at $\pm \gamma$ clearly separate the outliers from the unaffected data.  Compare this plot to the earlier plot of the error for when there were no outliers in the data.

```python
# Plot the errors
p = bkp.figure(title='Error', x_range=[-7, 7], y_range=[0.5, 1.0], width=600, height=200,
               x_axis_label='error=(yhat - y)')
p.circle(err_mse_ol[outlier_noise == 0], 0.6, fill_color='blue', legend_label='unaffected data')
p.circle(err_mse_ol[outlier_noise != 0], 0.6 + 0.01 * np.random.randn(4), color='darkorange',
         radius=0.1, legend_label='outlier')
estd = 2 * np.std(err_mse_ol)
p.line([-estd, -estd], [0, 1], line_dash='dashed', line_width=2)
p.line([estd, estd], [0, 1], line_dash='dashed', line_width=2)
p.legend.location = 'top_right'
p.yaxis.visible = False
bkp.show(p)
```

### Huber-loss model
Next, we fit the Huber-loss model.

```python
# With outliers - Predict using Huber Loss
np.random.seed(424242)
gamma = 2 * np.std(err_mse_ol)
update_func = functools.partial(update_weights_huber, gamma=gamma)
loss_func = functools.partial(huber_loss, gamma=gamma)
weights_huber_ol, loss_values_huber_ol = train(xtrain, ytrain + outlier_noise, nweights=2, update_func=update_func,
                                               loss_func=loss_func, niter=250 * 20)
```

```python
print('gamma:', gamma)
print('loss values (last few):', loss_values_huber_ol[-4:])
print('True weights:', wt_true)
print('Predicted weights:', weights_huber_ol)
yhat = polynomial_function(weights_huber_ol, xtrain)
print('MSE loss (all data):', mse_loss(yhat, ytrain + outlier_noise))
yhat = polynomial_function(weights_huber_ol, xtrain[outlier_noise == 0])
print('MSE loss (unaffected data):', mse_loss(yhat, ytrain[outlier_noise == 0]))
```

    gamma: 2.8238347395966823
    loss values (last few): [1.5358121300535852, 1.5358121300532634, 1.5358121300529748, 1.5358121300527157]
    True weights: [3.0, 6.0]
    Predicted weights: [3.38589048 5.1598805 ]
    MSE loss (all data): 2.02456696435184
    MSE loss (unaffected data): 0.15162498018067325

Notice that
 * the loss values have stabilized.
 * the predicted weights have diverged from the true weights, but not as much as the MSE-loss model.
 * the MSE loss for all of the data has increased to over 2.0 from 0.117.
 * the MSE loss for the unaffected data has increased to 0.1516.  But this is lower than the loss for the MSE-loss model.

```python
p = bkp.figure(title='Predictions -- Huber-loss -- Data With Outliers', y_range=[-1, 11], width=600, height=400)
p.circle(xtrain, ytrain + outlier_noise, radius=0.01)
xtmp = np.linspace(0, 1)
ytrue = polynomial_function(wt_true, xtmp)
ymse = polynomial_function(weights_mse_ol, xtmp)
yhuber = polynomial_function(weights_huber_ol, xtmp)
p.line(xtmp, ytrue, color='gray', line_width=3, line_dash='dashed', legend_label='true weight')
p.line(xtmp, ymse, color='blue', line_width=3, legend_label='MSE')
p.line(xtmp, yhuber, color='darkorange', line_width=3, legend_label='Huber')
p.legend.location = 'bottom_right'
bkp.show(p)
```

Observe that the Huber-loss model line is closer to the true line than the MSE-loss model line.

### Training Loss
For completeness, we plot the training loss against the training iterations for both MSE and Huber losses.  The training loss decreases with the number of steps as expected.  We can also observe that the training loss for the MSE-loss model is higher than that of the Huber-loss model.

```python
p = bkp.figure(title='Training Loss vs Iterations (data with outliers)', width=600, height=400)
p.line(np.arange(len(loss_values_mse_ol)) * 20, loss_values_mse_ol, line_width=2, legend_label='MSE')
p.line(np.arange(len(loss_values_huber_ol)) * 20, loss_values_huber_ol, line_width=2,
       color='darkorange', legend_label='Huber')
p.xaxis.axis_label = 'Training Steps'
p.yaxis.axis_label = 'Loss'
bkp.show(p)
```

## Comparison of Error Characteristics

We compare the characteristics of the error for the two models for all the data (including outliers) and for just the unaffected data.

```python
# Compute error for both models, with and without outliers, and display their statistics
y_mse_ol = polynomial_function(weights_mse_ol, xtrain)
err_mse_ol = (y_mse_ol - ytrain - outlier_noise)
#
y_huber_ol = polynomial_function(weights_huber_ol, xtrain)
err_huber_ol = (y_huber_ol - ytrain - outlier_noise)
#
tmp = pd.DataFrame({
    ('All Data', 'MSE-loss Model'): err_mse_ol,
    ('All Data', 'Huber-loss Model'): err_huber_ol,
    ('Unaffected Data', 'MSE-loss Model'): np.concatenate((err_mse_ol[outlier_noise == 0], np.nan * np.ones(4))),
    ('Unaffected Data', 'Huber-loss Model'): np.concatenate((err_huber_ol[outlier_noise == 0], np.nan * np.ones(4))),
}).describe()
display(tmp)
```

      All Data
      Unaffected Data

      MSE-loss Model
      Huber-loss Model
      MSE-loss Model
      Huber-loss Model

      count
      7.000000e+01
      70.000000
      66.000000
      6.600000e+01

      mean
      5.191414e-08
      -0.009046
      0.006265
      1.993456e-07

      std
      1.422112e+00
      1.433117
      0.500536
      3.923744e-01

      min
      -5.657756e+00
      -5.893139
      -1.359935
      -1.332695e+00

      25%
      -2.994260e-01
      -0.216800
      -0.280476
      -2.026042e-01

      50%
      -1.110702e-02
      0.004658
      -0.011107
      4.657694e-03

      75%
      3.095180e-01
      0.277028
      0.306297
      2.218965e-01

      max
      5.935515e+00
      6.005147
      1.203468
      9.108528e-01

**Observations**
 * "All Data" refers to all the data including outliers, and "Unaffected Data" refers to
   data excluding the outliers.
 * *Mean*: Notice that the mean error is close to zero for both models, with and without outliers.
   This is probably because we have two outliers on either side of the unaffected data.
 * *Standard deviation--All Data*: The standard deviation for all the data is slightly higher for the Huber-loss
   model than for the MSE-loss model.  By this measure it would seem that both the models are similar,
   with the MSE-model being slightly better.
 * *Standard deviation--Unaffected Data*: But, when we look at the standard deviation for the
   unaffected data, then the Huber-loss model really shines.  It's error `std` is 0.037 compared to 0.5
   for the MSE-loss and is an order of magnitude better.
 * *Min-Max All Data*: Again, the minimum and maximum error values are slightly worse for the
   Huber-loss model--implying that it underperforms for the outliers themselves.  This makes sense
   given the position of the outliers in our data.  The outliers cause the Huber-loss line to be closer
   to the actual line which causes the errors at the outliers to be larger.
 * *Min-Max Unaffected Data*: For the unaffected data, the roles are reversed--the Huber-loss model
   slightly outperforms the MSE-loss model.  For example, the maximum error is 0.82 compared to 1.2.
   Again, for our data, this makes sense since the Huber-loss line is closer to the actual line.

Plots of the errors for the two models are shown below.  Also, the outliers are shown in
a different color than the unaffected data.

```python
# Plot the errors
p = bkp.figure(title='Predictions -- Data Without Outliers', x_range=[-7, 7], y_range=[0, 1], width=600, height=400)
p.circle(err_mse_ol[outlier_noise == 0], 0.6, fill_color='blue', legend_label='unaffected data')
p.circle(err_mse_ol[outlier_noise != 0], 0.6 + 0.01 * np.random.randn(4), color='darkorange',
         radius=0.1, legend_label='outlier')
p.add_layout(bokeh.models.Label(x=0, y=0.7, text='MSE-Loss Model', text_align='center', text_baseline='top'))
p.circle(err_huber_ol[outlier_noise == 0], 0.3, fill_color='blue', legend_label='unaffected data')
p.circle(err_huber_ol[outlier_noise != 0], 0.3 + 0.01 * np.random.randn(4), color='darkorange',
         radius=0.1, legend_label='outlier')
p.add_layout(bokeh.models.Label(x=0, y=0.2, text='Huber-Loss Model', text_align='center'))
bkp.show(p)
```

## Comparison of MSE Loss
The MSE loss using the weights of the following models is shown in the table below:
 1. Model using MSE loss, and
 2. Model using Huber loss

| Model            | Loss (unaffected data) | Loss (all data) | Loss (no outliers) |
|------------------|-----------------------:|----------------:|-------------------:|
| MSE-loss Model   |                 0.2468 |          1.9935 |             0.1170 |
| Huber-loss Model |                 0.1516 |          2.0246 |             0.1171 |

Legend:
 * **Loss (unaffected data)**: The fitted data contains outliers, but the outliers
   were left out when computing the MSE loss.
 * **Loss (all data)**: The fitted data contains outliers, and the MSE loss was
   computed on all the data including the outliers.
 * **Loss (no outliers)**: The fitted data does not contain outliers i.e. the
   outlier noise was not added to the data.  The MSE loss is for all of the data.

Observations:
 * When there are no outliers, there is practically no difference between the two models.
 * When the model is fit on data with outliers, and when the MSE loss is computed on the
   entire dataset, the MSE-loss model slightly outperforms the Huber-loss model.
 * When the model is fit on data with outliers, but the MSE loss is computed on the data
   excluding the outliers, then the Huber-loss model significantly outperforms the
   MSE-loss model.
 * Since the comparison is done using MSE-loss it is but natural that the MSE-loss model
   outperforms the Huber-loss model when all of the data is considered.

# Appendix A -- Huber Loss Derivation

Define the error, $e_i$, as
$$
e_i = \hat{y}_i - y_i
$$

The total L2 loss for all the $M$ datapoints is
$$
L_2 = \frac{1}{M} \sum_{i=1}^{M} e_i^2
$$

The quadratic L2 loss for each data point is
$$
L_2^{(i)} = e_i^2
$$

For the Huber loss function, we wish to convert from a quadratic to a linear function.
Hence, we define the L1 loss as
$$
L_1^{(i)} = c_1 + c_2 |e_i|
$$
for some constants $c_1$ and $c_2$.

For clarity we temporarily drop the $i$ subscript.  And we require that the Huber loss function
be continuous at the change point $\gamma > 0$ and also that the first derivative be continuous.
$$
\begin{align}
L_2 & = L_1 \\
\frac{\partial L_2}{\partial e} & = \frac{\partial L_1}{\partial e}
\end{align}
$$

Substituting for $L_1$ and $L_2$ at $e=\gamma$, we get
$$
\begin{align}
\gamma^2 & = c_1 + c_2 \gamma \\
2 \gamma & = c_2
\end{align}
$$

Which yields, $c_2 = 2 \gamma$ and $c_1 = -\gamma^2$.

Hence, the linear part of the Huber loss is
$$
\begin{align}
L_1^{(i)} & = c_1 + c_2 |e_i| \\
  & = -\gamma^2 + 2 \gamma |e_i| \\
  & = \gamma (2|e_i| - \gamma)
\end{align}
$$

And the point-wise Huber loss function is then
$$
L_\gamma (e_i) =
\begin{cases}
 e_i^2                   & \text{for } |e_i| \le \gamma, \quad \gamma > 0\\
 \gamma (2 |e_i| - \gamma), & \text{otherwise.}
\end{cases}
$$

# Appendix B -- Gradient for Huber Loss

Define the point-wise error, $e_i$, as $e_i = \hat{y_i} - y_i$.

Where, for an $N$-degree polynomial model, the prediction, $\hat{y_i}$ for input $x_i$, is given by
$$
\hat{y_i} = w_0 + w_1 x + w_2 x^2 + ... + w_N x^N = \sum_{d=0}^{N} w_i x^i
$$

The point-wise Huber loss is defined as
$$
L_i = L_\gamma (e_i) =
\begin{cases}
 e_i^2                   & \text{for } |e_i| \le \gamma, \quad \gamma > 0\\
 \gamma (2 |e_i| - \gamma), & \text{otherwise.}
\end{cases}
$$

To derive the gradient, we apply the following chain rule
$$
\frac{\partial L_i}{\partial w_d} = \frac{\partial L_i}{\partial e_i} \frac{\partial e_i}{\partial w_d}
$$

And,
$$
\frac{\partial e_i}{\partial w_d} = x^i
$$

Let's derive the gradients for the linear and quadratic terms separately.

For the quadratic term,
$$
\frac{\partial L_i}{e_i} = \frac{\partial e_i^2}{e_i} = 2 e_i \quad |e_i| \le \gamma
$$

For the linear term,
$$
\frac{\partial L_i}{e_i} = \frac{\partial \gamma (2 |e_i| - \gamma)}{e_i}
= 2 \gamma \text{sgn}(e_i) \quad |e_i| > \gamma
$$

The above two can be combined into one expression
$$
\frac{\partial L_i}{\partial e_i} = 2 \text{sgn}(e_i) \, \min(|e_i|, \gamma)
$$

By the chain rule and substituting for $\frac{\partial e_i}{w_i}$,
$$
\frac{\partial L_i}{\partial w_d} = 2 \text{sgn}(e_i) \, \min(|e_i|, \gamma) x^i
$$

Which can be defined recursively as follows.  This definition is suitable to
compute the gradients sequentially along each of the weights, $w_d$.
$$
\begin{align}
\frac{\partial L_i}{\partial w_0} & = 2 \, \text{sgn}(e_i) \, \min(|e_i|, \gamma) \\
\frac{\partial L_i}{\partial w_d} & = x \frac{\partial L_i}{\partial w_{d - 1}} \quad d = 1, 2, ..., N
\end{align}
$$

# Appendix C -- Variation of $\gamma$

```python
# different gamma
np.random.seed(424242)
gamma = 6.0
update_func = functools.partial(update_weights_huber, gamma=gamma)
loss_func = functools.partial(huber_loss, gamma=gamma)
weights, loss_values = train(xtrain, ytrain + outlier_noise, nweights=2, update_func=update_func,
                             loss_func=loss_func, niter=250 * 20)
print(loss_values[-10:])
print(wt_true)
print(weights)
```

    [1.9935106591385687, 1.993510659138518, 1.993510659138473, 1.9935106591384324, 1.993510659138397, 1.993510659138365, 1.9935106591383362, 1.993510659138311, 1.9935106591382883, 1.9935106591382683]
    [3.0, 6.0]
    [3.68645222 4.58096621]

```python

```


```python
from sympy.interactive import printing
printing.init_printing(use_latex=True)
from sympy import *
import sympy as sp

x = sp.symbols('x')
f = sp.Function('f')(x)
diffeq = Eq(f.diff(x,x)-5*f,10)
display(diffeq)

dsolve(diffeq,f)
```

```python

```


[](https://pythonista.io)

# Introducción a ```sympy```.

El proyecto [sympy](https://www.sympy.org/en/index.html) comprende una biblioteca de herramientas que permiten realziar operaciones de matemáticas simbólicas.

En este sentido, es posible utilizar algunos de sus componentes para realizar operaciones que en lugar de regresar valores numéricos regresan representaciones simbólicas.

```python
!pip install sympy
```

```python
import sympy
```

## La función *sympy.symbols()*.

Esta función permite crear objetos de la clase *sympy.core.symbol.Symbol* que pueden ser utulizadso como símbolos algebraicos.

```
sympy.symbols('<símbolo>')
```

```python
x = sympy.symbols('x')
```

```python
type(x)
```

```python
x + 1
```

```python
2/3 + x
```

```python
x ** 2
```

```python
x ** (1/2)
```

## La función *sympy.Rational()*

```python
sympy.Rational(2, 3)
```

```python

```

```python
x, y, z = sympy.symbols("x, y, z")
```

```python
f = sympy.Function("f")
```

```python
f(x)
```

```python
f = sympy.Function('f')(x)
```

```python
f
```

```python
expr = x**4 + x**3 + x**2 + x + 1
```

```python
expr
```

```python
expr.diff()
```

```python
expr.integrate()
```

```python
expresion = x + sympy.sin(x)
```

```python
expresion
```

```python
expresion.integrate(x, x)
```

```python
expresion.diff(x, x, x)
```

```python
expr.diff(x)
```

Esta obra está bajo una Licencia Creative Commons Atribución 4.0 Internacional.

© José Luis Chiquete Valdivieso. 2019.


```python
from sympy import *
init_printing()
```

```python
q1, q2, q3 = symbols('q_1 q_2 q_3',
                     real=True,
                     positive=True,
                     finite=True)

# Define payoff functions for solvable system
payoff1 = Rational(1,4)*(q1**2) - q1 * q2 + Rational(1,3) * q1 * q3
payoff2 = Rational(1,4) * q1 - Rational(1,4)*q2**2 + q2 * q3
payoff3 = Rational(1,8) * q3 * (q1 + q2) - q3

# Define payoff functions for unsolvable system
# payoff1_ns
# payoff2_ns
# payoff3_ns

payoff1, payoff2, payoff3
```

```python
D1 = payoff1.diff(q1)
D2 = payoff2.diff(q2)
D3 = payoff3.diff(q3)
D = [D1, D2, D3]
D1, D2, D3
```

```python
# Solve the FOC system
solve([D1, D2, D3], [q1, q2, q3], dict=True)
```

```python
A = Matrix([
    [Rational(1,2), -1, Rational(1,3)],
    [0, -Rational(1,2), 1],
    [Rational(1,8), Rational(1,8), 0]
])
A
b = Matrix([[0],
            [0],
            [1]])

A, b
```

```python
A.rref()
```

```python
A.inv()
```

```python
A.det()
```

```python
A.inv() * b
```


# Kinematic chain in a plane (2D)

Marcos Duarte

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)).

Let's analyze the kinematics of such a chain, more precisely, the case for a planar two-link kinematic chain. There are different ways to describe the kinematics of a chain, for now we will use simple trigonometry and calculus and the Cartesian coordinate system. 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. In Sympy, we could even use the [mechanics module](http://docs.sympy.org/latest/modules/physics/mechanics/index.html), a specific module for creation of symbolic equations of motion for complicated multibody systems, but let's deduce everythig manually to understand all 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 and three angles to completely define the configuration of the rigid body. For a link attached to a fixed body by a revolute 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 revolute 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 revolute 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()
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.physics.mechanics import dynamicsymbols, mlatex, init_vprinting
init_vprinting()

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

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

```python
t = Symbol('t')
l = Symbol('ell', positive=True)
a = dynamicsymbols('alpha')  # it's already alpha(t)
```

Using simple trigonometry, the equations for the endpoint position in terms of the joint angle are:

```python
rx = l*cos(a)
display(Math(latex(r'r_x=') + latex(rx)))
ry = l*sin(a)
display(Math(latex(r'r_y=') + latex(ry)))
```

$$r_x=\ell \cos{\left (\alpha{\left (t \right )} \right )}$$

$$r_y=\ell \sin{\left (\alpha{\left (t \right )} \right )}$$

Computing the configuration of a link or a chain (including the endpoint location) from the the joint parameters (joint angles and link lengths) as we have done is called forward or direct kinematics.

The inverse kinematics for this system is to specify the joint angle in terms of the endpoint position. For the one-link system:

$$\alpha = arctan\left(\frac{r_y}{r_x}\right)$$

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

```python
r = Matrix((rx, ry))
Math(latex(r'\mathbf{r}=\begin{bmatrix} r_x \\ r_y \\ \end{bmatrix}=' + mlatex(r)))
```

$$\mathbf{r}=\begin{bmatrix} r_x \\ r_y \\ \end{bmatrix}=\left[\begin{matrix}\ell \operatorname{cos}\left(\alpha\right)\\\ell \operatorname{sin}\left(\alpha\right)\end{matrix}\right]$$

## 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](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.

The velocity of the endpoint can be obtained by the first-order [derivative](http://en.wikipedia.org/wiki/Derivative) of the position vector. The derivative of a vector is obtained by differentiating each vector component:

$$ \frac{\mathrm{d}\mathbf{r}}{\mathrm{d}t} = \begin{bmatrix}
\frac{\mathrm{d}r_x}{\mathrm{d}t} \\
\frac{\mathrm{d}r_y}{\mathrm{d}t} \\
\end{bmatrix} $$

Note that the derivative is with respect to time but $r_x$ and $r_y$ depend explicitly on $\alpha$ and it's $\alpha$ that depends on $t$ ($r_x$ and $r_y$ 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](http://en.wikipedia.org/wiki/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](http://en.wikipedia.org/wiki/Notation_for_differentiation)):

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

Or using what is known as [Leibniz's notation](http://en.wikipedia.org/wiki/Notation_for_differentiation):

$$\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](http://en.wikipedia.org/wiki/Total_derivative) and $\partial f / \partial x$ represents the [partial derivative](http://en.wikipedia.org/wiki/Partial_derivative) of a function.

[**Product rule**](http://en.wikipedia.org/wiki/Product_rule)
The derivative of the product of two functions is:

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

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

```python
v = r.diff(t)
Math(mlatex('\mathbf{v}=') + mlatex(v))
```

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

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

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

```python
Math(mlatex(r'||\mathbf{v}||=') + mlatex(simplify(sqrt(v[0]**2+v[1]**2))))
```

$$||\mathbf{v}||=\ell \sqrt{\left(\dot{\alpha}\right)^{2}}$$

Which is $\ell\dot{\alpha}$ (we could have used the function norm of Sympy, v.norm(), but the output does not symplify nicely).

The direction of $\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 revolute joint in a plane.

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 = r.diff(t, 2)
Math(mlatex(r'\mathbf{a}=') + mlatex(acc))
```

$$\mathbf{a}=\left[\begin{matrix}- \ell \left(\operatorname{sin}\left(\alpha\right) \ddot{\alpha} + \operatorname{cos}\left(\alpha\right) \left(\dot{\alpha}\right)^{2}\right)\\\ell \left(- \operatorname{sin}\left(\alpha\right) \left(\dot{\alpha}\right)^{2} + \operatorname{cos}\left(\alpha\right) \ddot{\alpha}\right)\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{\alpha}$ and other term proportional to the square of the angular velocity $\dot{\alpha}^{2}$.
The term proportional to angular acceleration is always tangent to the trajectory of the endpoint (see figure below) and it's magnitude or Euclidean norm is:

```python
A = a.diff(t, 2)
Math(mlatex(r'||\mathbf{a}_t||=') +
     mlatex(simplify(sqrt(expand(acc[0]).coeff(A)**2+expand(acc[1]).coeff(A)**2)*A)))
```

$$||\mathbf{a}_t||=\ell \ddot{\alpha}$$

The term proportional to angular velocity 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 = a.diff(t)**2
Math(mlatex(r'||\mathbf{a}_c||=') +
     mlatex(simplify(sqrt(expand(acc[0]).coeff(A)**2+expand(acc[1]).coeff(A)**2)*A)))
```

$$||\mathbf{a}_c||=\ell \left(\dot{\alpha}\right)^{2}$$

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 acelleration).

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

Let's plot some simulated data to have an idea of the one-link kinematics. Consider $\ell=1\:m,\alpha_0=0^o,\alpha_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
a0, af, d = 0, np.pi/2, 1
ts = np.arange(0, 1.01, .01)
mjt  = a0 + (af - a0)*(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, a:b, a.diff(t, 1):c, a.diff(t, 2):d, a.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=(12, 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](http://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant)**
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 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 is:

$$ \mathbf{J}=
\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.

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

$$ \mathbf{J}=
\begin{bmatrix}
\frac{\partial \:r_x}{\partial \alpha} \\
\frac{\partial \:r_y}{\partial \alpha} \\
\end{bmatrix} $$

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

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

And Sympy has a function to calculate the Jacobian:

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

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

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

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

Where:

```python
w = Matrix([a.diff(t)])
Math(mlatex(r'\mathbf{\omega}=') + mlatex(w))
```

$$\mathbf{\omega}=\left[\begin{matrix}\dot{\alpha}\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 vector z coming out of the screen (paper).

Then:

```python
velJ = J*w
Math(mlatex(r'\mathbf{v_J}=') + mlatex(velJ))
```

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

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

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

Let's calculate this derivative:

```python
accJ = J.diff(t)*w + J*w.diff(t)
Math(mlatex(r'\mathbf{a_J}=') + mlatex(accJ))
```

$$\mathbf{a_J}=\left[\begin{matrix}- \ell \operatorname{sin}\left(\alpha\right) \ddot{\alpha} - \ell \operatorname{cos}\left(\alpha\right) \left(\dot{\alpha}\right)^{2}\\- \ell \operatorname{sin}\left(\alpha\right) \left(\dot{\alpha}\right)^{2} + \ell \operatorname{cos}\left(\alpha\right) \ddot{\alpha}\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 revolute joints in a plane.

We need to define the symbolic variables $t,\:\ell_1,\:\ell_2,\:\alpha_1,\:\alpha_2$ (and make $\alpha_1$ and $\alpha_2$ function of time):

```python
t = Symbol('t')
l1, l2 = symbols('ell_1 ell_2', positive=True)
#a1, a2 = Function(r'\alpha_1')(t), Function(r'\alpha_2')(t)
a1, a2 = dynamicsymbols('alpha1 alpha2')  # alpha1(t) alpha2(t)
```

The position of the endpoint in terms of the joint angle is:

```python
r2 = Matrix((l1*cos(a1)+l2*cos(a1+a2),l1*sin(a1)+l2*sin(a1+a2)))
Math(mlatex(r'\mathbf{r}=\begin{bmatrix} r_x \\ r_y \end{bmatrix}=') + mlatex(r2))
```

$$\mathbf{r}=\begin{bmatrix} r_x \\ r_y \end{bmatrix}=\left[\begin{matrix}\ell_{1} \operatorname{cos}\left(\alpha_{1}\right) + \ell_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)\\\ell_{1} \operatorname{sin}\left(\alpha_{1}\right) + \ell_{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right)\end{matrix}\right]$$

Note that $\alpha_2$ is a joint angle (referred as measured in the joint space); the angle of the segment 2 with respect to the horizontal is $\alpha_1+\alpha_2$ and is referred as an angle in the segmental space.

The inverse kinematics of this system is to specify the joint angles in terms of the endpoint position. Using the [cosine rule](http://en.wikipedia.org/wiki/Law_of_cosines), the angle $\alpha_2$ is:

$$ r_x^2+r_y^2=\ell_1^2+\ell_2^2-2\ell_1 \ell_2 cos(\pi-\alpha_2) $$

$$ \alpha_2=arccos\left(\frac{r_x^2+r_y^2-\ell_1^2-\ell_2^2}{2\ell_1 \ell_2}\;\;\right) $$

To find the angle $\alpha_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(\alpha_2)}{\ell_1+\ell_2 cos(\alpha_2)}\right) $$

And the angle of its hypotenuse with the horizontal is:

$$ \alpha_1+\phi=arctan\left(\frac{r_y}{r_x}\right) $$

Then, the angle $\alpha_1$ is:

$$ \alpha_1=arctan\left(\frac{r_y}{r_x}\right) - arctan\left(\frac{\ell_2 sin(\alpha_2)}{\ell_1+\ell_2 cos(\alpha_2)}\right) $$

Note that there are two possible sets of $(\alpha_1,\alpha_2)$ angles for the same $(r_x,r_y)$ 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 $\alpha_2'=2\pi-\alpha_2$, but $sin(\alpha_2')=-sin(\alpha_{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 revolute joints in a plane.

## Differential kinematics

The linear velocity of the endpoint is:

```python
vel2 = r2.diff(t)
Math(mlatex(r'\mathbf{v}=') + mlatex(vel2))
```

$$\mathbf{v}=\left[\begin{matrix}- \ell_{1} \operatorname{sin}\left(\alpha_{1}\right) \dot{\alpha}_{1} - \ell_{2} \left(\dot{\alpha}_{1} + \dot{\alpha}_{2}\right) \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right)\\\ell_{1} \operatorname{cos}\left(\alpha_{1}\right) \dot{\alpha}_{1} + \ell_{2} \left(\dot{\alpha}_{1} + \dot{\alpha}_{2}\right) \operatorname{cos}\left(\alpha_{1} + \alpha_{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 example, $\ell_2cos(\alpha_1+\alpha_2)\dot{\alpha}_1$, plus the velocity of joint 2 in relation to joint 1, for example, $\ell_1\dot{\alpha}_1 cos(\alpha_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)
Math(mlatex(r'\mathbf{a}=') + mlatex(acc2))
```

$$\mathbf{a}=\left[\begin{matrix}- \ell_{1} \operatorname{sin}\left(\alpha_{1}\right) \ddot{\alpha}_{1} + \ell_{1} \operatorname{cos}\left(\alpha_{1}\right) \left(\dot{\alpha}_{1}\right)^{2} + \ell_{2} \left(\dot{\alpha}_{1} + \dot{\alpha}_{2}\right)^{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right) + \ell_{2} \left(\ddot{\alpha}_{1} + \ddot{\alpha}_{2}\right) \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right)\\- \ell_{1} \operatorname{sin}\left(\alpha_{1}\right) \left(\dot{\alpha}_{1}\right)^{2} + \ell_{1} \operatorname{cos}\left(\alpha_{1}\right) \ddot{\alpha}_{1} - \ell_{2} \left(\dot{\alpha}_{1} + \dot{\alpha}_{2}\right)^{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right) + \ell_{2} \left(\ddot{\alpha}_{1} + \ddot{\alpha}_{2}\right) \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)\end{matrix}\right]$$

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

```python
acc2 = acc2.expand()
A = a1.diff(t, 2)
B = a2.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 = a1.diff(t)**2
B = a2.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 = a1.diff(t)*a2.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(\alpha_{1} + \alpha_{2}\right) \ddot{\alpha}_{2} + \left(- \ell_{1} \operatorname{sin}\left(\alpha_{1}\right) - \ell_{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right)\right) \ddot{\alpha}_{1}\\\ell_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right) \ddot{\alpha}_{2} + \left(\ell_{1} \operatorname{cos}\left(\alpha_{1}\right) + \ell_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)\right) \ddot{\alpha}_{1}\end{matrix}\right]$$

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

$$Coriolis:\;\;\;\;\:\left[\begin{matrix}- 2 \ell_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right) \dot{\alpha}_{1} \dot{\alpha}_{2}\\- 2 \ell_{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right) \dot{\alpha}_{1} \dot{\alpha}_{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}=
\begin{bmatrix}
\frac{\partial r_x}{\partial \alpha_{1}} & \frac{\partial r_x}{\partial \alpha_{2}} \\
\frac{\partial r_y}{\partial \alpha_{1}} & \frac{\partial r_y}{\partial \alpha_{2}} \\
\end{bmatrix} $$

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

```python
J2 = r2.jacobian([a1, a2])
Math(mlatex(r'\mathbf{J}=') + mlatex(J2))
```

$$\mathbf{J}=\left[\begin{matrix}- \ell_{1} \operatorname{sin}\left(\alpha_{1}\right) - \ell_{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right) & - \ell_{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right)\\\ell_{1} \operatorname{cos}\left(\alpha_{1}\right) + \ell_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right) & \ell_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{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((a1, a2)).diff(t)
Math(mlatex(r'\mathbf{\omega}=') + mlatex(w2))
```

$$\mathbf{\omega}=\left[\begin{matrix}\dot{\alpha}_{1}\\\dot{\alpha}_{2}\end{matrix}\right]$$

Then:

```python
vel2J = J2*w2
Math(mlatex(r'\mathbf{v_J}=') + mlatex(vel2J))
```

$$\mathbf{v_J}=\left[\begin{matrix}- \ell_{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right) \dot{\alpha}_{2} + \left(- \ell_{1} \operatorname{sin}\left(\alpha_{1}\right) - \ell_{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right)\right) \dot{\alpha}_{1}\\\ell_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right) \dot{\alpha}_{2} + \left(\ell_{1} \operatorname{cos}\left(\alpha_{1}\right) + \ell_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)\right) \dot{\alpha}_{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)
Math(mlatex(r'\mathbf{a_J}=') + mlatex(acc2J))
```

$$\mathbf{a_J}=\left[\begin{matrix}- \ell_{2} \left(\dot{\alpha}_{1} + \dot{\alpha}_{2}\right) \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right) \dot{\alpha}_{2} - \ell_{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right) \ddot{\alpha}_{2} + \left(- \ell_{1} \operatorname{sin}\left(\alpha_{1}\right) - \ell_{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right)\right) \ddot{\alpha}_{1} + \left(- \ell_{1} \operatorname{cos}\left(\alpha_{1}\right) \dot{\alpha}_{1} - \ell_{2} \left(\dot{\alpha}_{1} + \dot{\alpha}_{2}\right) \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)\right) \dot{\alpha}_{1}\\- \ell_{2} \left(\dot{\alpha}_{1} + \dot{\alpha}_{2}\right) \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right) \dot{\alpha}_{2} + \ell_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right) \ddot{\alpha}_{2} + \left(\ell_{1} \operatorname{cos}\left(\alpha_{1}\right) + \ell_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)\right) \ddot{\alpha}_{1} + \left(- \ell_{1} \operatorname{sin}\left(\alpha_{1}\right) \dot{\alpha}_{1} - \ell_{2} \left(\dot{\alpha}_{1} + \dot{\alpha}_{2}\right) \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right)\right) \dot{\alpha}_{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, \alpha_1(0)=\alpha_2(0)=0$, 1 s of movement duration, $\alpha_1(1)=\alpha_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, 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=(12, 6))
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 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$.

2. 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\alpha_1+\alpha_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.

## 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.
- 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.


# Numerical Solutions of 1st-order ODEs

For numerically solving definite integrals ($\int_a^b f(x) dx$) we have methods like the trapezoidal rule and Simpson's rule. When we need to solve 1st-order ODEs of the form
\begin{equation}
y^{\prime} = \frac{dy}{dx} = f(x, y)
\end{equation}
for $y(x)$, we need other methods. All of them will work by starting at the initial conditions, and then using information provided by the ODE to march forward in the solution, based on an increment (i.e., step size) $\Delta x$.

For example, let's say we want to solve
\begin{equation}
\frac{dy}{dx} = 4 x - \frac{2 y}{x} \;, \quad y(1) = 1
\end{equation}
This problem is fairly simple, and we can find the general and particular solutions to compare our numerical results against:
\begin{align}
\text{general: } y(x) &= x^2 + \frac{x}{x^2} \\
\text{particular: } y(x) &= x^2
\end{align}

## Forward Euler method

Recall that the derivative, $y^{\prime}$, is the same as the slope. At the starting point, $(x,y) = (1,1)$, where $y^{\prime} = 2$, this looks like:

```matlab
format compact
%plot inline

x = linspace(1, 3);
y = x.^2;
plot(x, y); hold on
plot([1, 2], [1, 3], '--')
legend(['Solution'], ['Slope at start'])
hold off
```

Remember that the slope, or derivative, is
\begin{equation}
\text{slope} = \frac{\text{rise}}{\text{run}} = \frac{\Delta y}{\Delta x}
\end{equation}

Let's consider the initial condition—the starting point—as $(x_i, y_i)$, and the next point in our numerical solution is $(x_{i+1}, y_{i+1})$, where $i$ represents an index starting at 1 and ending at the number of steps $N$. Our step size is then $\Delta x = x_{i+1} - x_i$.

Based on our (very simple) approximation to the first derivative based on slope, we can relate the derivative to our two points:
\begin{equation}
\left(\frac{dy}{dx}\right)_{i} = \frac{y_{i+1} - y_i}{x_{i+1} - x_i} = \frac{y_{i+1} - y_i}{\Delta x}
\end{equation}
Then, solve this for our unknown:
\begin{equation}
y_{i+1} = y_i + \left(\frac{dy}{dx}\right)_i \Delta x
\end{equation}
This is the **Forward Euler method**.

Based on a given step size $\Delta x$, we'll use this formula (called a *recursion* formula) to march forward and obtain the full solution over given $x$ domain. That will look something like this:

```matlab
clear x y
x_exact = linspace(1, 3);
y_exact = x_exact.^2;
plot(x_exact, y_exact); hold on

% our derivative function, dy/dx
f = @(x,y) 4*x - (2*y)/x;

dx = 0.5;
x = 1 : dx : 3;
y(1) = 1;
for i = 1 : length(x)-1
    y(i+1) = y(i) + f(x(i), y(i))*dx;
end
plot(x, y, 'o--', 'MarkerFaceColor', 'r')

legend(['Exact solution'], ['Numerical solution'], 'Location','northwest')
hold off
```

Another way to obtain the recursion formula for the Forward Euler method is to use a Taylor series expansion.
Recall that for well-behaved functions, the Taylor series expansion says
\begin{equation}
y(x + \Delta x) = y(x) + \Delta x y^{\prime}(x) + \frac{1}{2} \Delta x^2 y^{\prime\prime}(x) + \frac{1}{3!} \Delta x^3 y^{\prime\prime\prime}(x) \dots \;.
\end{equation}
This is exact for an infinite series. We can apply this formula to our (unknown) solution $y_i$ and cut off the terms of order $\Delta x^2$ and higher; the derivative $y^{\prime}$ is given by our original ODE.
This gives us the same recursion formula as above:
\begin{equation}
\therefore y_{i+1} \approx y_i + \left( \frac{dy}{dx}\right)_i \Delta x
\end{equation}
where we can now see that we are introducing some error on the order of $\Delta x^2$ at each step. This is the *local truncation error*. The *global error* is the accumulation of error over all the steps, and is on the order of $\Delta x$. Thus, the Forward Euler method is a **first-order** method, because its global error is on the order of the step size to the first power: error $\sim \mathcal{O}(\Delta x)$.

Forward Euler is also an **explicit** method, because its recursion formula is explicity defined for $y_{i+1}$. (You'll see when that may not be the case soon.)

In general, for an $n$th-order method:
\begin{align}
\text{local error } &\sim \mathcal{O}(\Delta x^{n+1}) \\
\text{global error } &\sim \mathcal{O}(\Delta x^{n})
\end{align}
(This only applies for $\Delta x < 1$; in cases where you have a $\Delta x > 1$, you should nondimensionalize the problem based on the domain size such that $0 \leq x \leq 1$.)

Applying the Forward Euler method then requires:

1. Have a given first-order ODE: $\frac{dy}{dx} = y^{\prime} = f(x,y)$. Complex and/or nonlinear problems are fine!
2. Specify the step size $\Delta x$ (or $\Delta t$).
3. Specify the domain over which to integrate: $x_1 \leq x \leq x_n$
4. Specify the initial condition: $y(x=x_1) = y_1$

Let's do another example:
\begin{equation}
y^{\prime} = 8 e^{-x}(1+x) - 2y
\end{equation}
with the initial condition $y(0) = 1$, and the domain $0 \leq x \leq 7$. This is a linear 1st-order ODE that we can find the analytical solution for comparison:
\begin{equation}
y(x) = e^{-2x} (8 x e^x + 1)
\end{equation}

To solve, we'll create an anonymous function for the derivative and then incorporate that into our Forward Euler code. We'll start with $\Delta x = 0.2$.

```matlab
clear

f = @(x,y) 8*exp(-x)*(1 + x) - 2*y;

dx = 0.2;
x = 0 : dx : 7;
n = length(x);
y(1) = 1;

% Forward Euler loop
for i = 1 : n - 1
    y(i+1) = y(i) + dx*f(x(i), y(i));
end

x_exact = linspace(0, 7);
y_exact = exp(-2.*x_exact).*(8*x_exact.*exp(x_exact) + 1);
plot(x_exact, y_exact); hold on
plot(x, y, 'o--')
legend('Exact solution', 'Forward Euler solution')
```

Notice the visible error in that plot, which is between 0.2–0.25, or in other words $\mathcal{O}(\Delta x)$.

How can we reduce the error? Just like with the trapezoidal rule, we have two main options:

 - Reduce the step size $\Delta x$
 - Choose a higher-order (i.e., more accurate) method

The downside to reducing $\Delta x$ is the increased number of steps we then have to take, which may make the solution too computationally expensive. A more-accurate method would have less error per step, which might allow us to use the same $\Delta x$ but get a better solution. Let's next consider some better methods.

## Heun's method

Heun's method is a **predictor-corrector** method; these work by *predicting* a solution at some intermediate location and then using that information to get a better overall answer at the next location (*correcting*). Heun's uses the Forward Euler method to predict the solution at $x_{i+1}$, then uses the average of the slopes at $y_i$ and the predicted $y_{i+1}$ to get a better overall answer for $y_{i+1}$.

\begin{align}
\text{predictor: } y_{i+1}^p &= y_i + \Delta x f(x_i, y_i) \\
\text{corrector: } y_{i+1} &= y_i + \frac{\Delta x}{2} \left( f(x_i, y_i) + f(x_{i+1}, y_{i+1}^p) \right)
\end{align}

Heun's method is second-order accurate, meaning the global error is $\mathcal{O}(\Delta x^2)$ and explicit.

Let's see this method in action:

```matlab
clear

f = @(x,y) 8*exp(-x)*(1 + x) - 2*y;

dx = 0.2;
x = 0 : dx : 7;
n = length(x);
y(1) = 1;

% Heun's method loop
for i = 1 : n - 1
    y_p = y(i) + dx*f(x(i), y(i));
    y(i+1) = y(i) + (dx/2)*(f(x(i), y(i)) + f(x(i+1), y_p));
end

x_exact = linspace(0, 7);
y_exact = exp(-2.*x_exact).*(8*x_exact.*exp(x_exact) + 1);
plot(x_exact, y_exact); hold on
plot(x, y, 'o--')
legend('Exact solution', "Heun's method solution")
fprintf('Maximum error: %5.3f', abs(max(y_exact) - max(y)))
```

Notice how the error is visibly smaller than for the Forward Euler method–the maximum error is around 0.05, which is very close to $\Delta x^2 = 0.04$.

## Midpoint method

The midpoint method, also known as the modified Euler method, is another predictor-corrector method, that instead predicts the solution at the midpoint ($x + \Delta x/2$):
\begin{align}
y_{i + \frac{1}{2}} &= y_i + \frac{\Delta x}{2} f(x_i, y_i) \\
y_{i+1} &= y_i + \Delta x f \left( x_{i+\frac{1}{2}} , y_{i + \frac{1}{2}} \right)
\end{align}

Like Heun's method, the midpoint method is explicit and second-order accurate:

```matlab
clear

f = @(x,y) 8*exp(-x)*(1 + x) - 2*y;

dx = 0.2;
x = 0 : dx : 7;
n = length(x);
y(1) = 1;

% midpoint method loop
for i = 1 : n - 1
    y_half = y(i) + (dx/2)*f(x(i), y(i));
    y(i+1) = y(i) + dx * f(x(i) + dx/2, y_half);
end

x_exact = linspace(0, 7);
y_exact = exp(-2.*x_exact).*(8*x_exact.*exp(x_exact) + 1);
plot(x_exact, y_exact); hold on
plot(x, y, 'o--')
legend('Exact solution', "Midpoint method solution")

fprintf('Maximum error: %5.3f', abs(max(y_exact) - max(y)))
```

## Fourth-order Runge–Kutta method

Runge–Kutta methods are a family of methods that use one or more stages; the methods we have discussed so far (Forward Euler, Heun's, and midpoint) actually all fall in this family. There is also a popular fourth-order method: the **fourth-order Runge–Kutta method** (RK4). This uses four stages to get a more accurate solution:
\begin{align}
y_{i+1} &= y_i + \frac{\Delta x}{6} (k_1 + 2 k_2 + 2 k_3 + k_4) \\
k_1 &= f(x_i, y_i) \\
k_2 &= f \left( x_i + \frac{\Delta x}{2}, y_i + \frac{\Delta x}{2} k_1 \right) \\
k_3 &= f \left( x_i + \frac{\Delta x}{2}, y_i + \frac{\Delta x}{2} k_2 \right) \\
k_4 &= f \left( x_i + \Delta x, y_i + \Delta x \, k_3 \right)
\end{align}

This method is explicit and fourth-order accurate: error $\sim \mathcal{O}(\Delta x^4)$:

```matlab
clear

f = @(x,y) 8*exp(-x)*(1 + x) - 2*y;

dx = 0.2;
x = 0 : dx : 7;
n = length(x);
y(1) = 1;

% 4th-order Runge-Kutta method loop
for i = 1 : n - 1
    k1 = f(x(i), y(i));
    k2 = f(x(i) + dx/2, y(i) + dx*k1/2);
    k3 = f(x(i) + dx/2, y(i) + dx*k2/2);
    k4 = f(x(i) + dx, y(i) + dx*k3);
    y(i+1) = y(i) + (dx/6) * (k1 + 2*k2 + 2*k3 + k4);
end

x_exact = linspace(0, 7);
y_exact = @(x) exp(-2.*x).*(8*x.*exp(x) + 1);
plot(x_exact, y_exact(x_exact)); hold on
plot(x, y, 'o--')
legend('Exact solution', "RK4 solution")

fprintf('Maximum error: %6.4f', max(abs(y_exact(x) - y)))
```

The maximum error (0.0004) is actually a bit smaller than $\Delta x^4 = 0.0016$, but approximately the same order of magnitude.

Matlab also offers a built-in RK4 integrator: `ode45`. (It is actually slightly more complicated than the equations shown just now, because it automatically adjusts the step size $\Delta x$ to control error.) You can call this function with the syntax:

    [X, Y] = ode45(function_name, [x_start x_end], [IC]);

where `function_name` is the name of a function that provides the derivative (this can be a regular function given in a file, or an anonymous function); `[x_start x_end]` provides the domain of integration ($x_{\text{start}} \leq x \leq x_{\text{end}}$), and `[IC]` provides the initial condition $y(x=x_{\text{start}})$.

For example, let's use this and compare with our exact solution:

```matlab
clear

f = @(x,y) 8*exp(-x)*(1 + x) - 2*y;

[X, Y] = ode45(f, [0 7], [1]);

x_exact = linspace(0, 7);
y_exact = @(x) exp(-2.*x).*(8*x.*exp(x) + 1);
plot(x_exact, y_exact(x_exact)); hold on
plot(X, Y, 'o--')
legend('Exact solution', "ode45 solution")

fprintf('Maximum error: %6.4f', max(abs(y_exact(X) - Y)))
```

```matlab

```


# Linear Program (LP) Tutorial
For instructions on how to run these tutorial notebooks, please see the [README](https://github.com/RobotLocomotion/drake/blob/master/tutorials/README.md).

## Important Note
Please refer to [mathematical program tutorial](./mathematical_program.ipynb) for constructing and solving a general optimization program in Drake.

## Linear Program
A linear program (LP) is a special type of optimization problem. The cost and constraints in an LP is a linear (affine) function of decision variables. The mathematical formulation of a general LP is
\begin{align}
\min_x \;c^Tx + d\\
\text{subject to } Ax\leq b
\end{align}

A linear program can be solved by many open source or commercial solvers. Drake supports some solvers including SCS, Gurobi, Mosek, etc. Please see our [Doxygen page]( https://drake.mit.edu/doxygen_cxx/group__solvers.html) for a complete list of supported solvers. Note that some commercial solvers (such as Gurobi and Mosek) are not included in the pre-compiled Drake binaries, and therefore not on Binder/Colab.

Drake's API supports multiple functions to add linear cost and constraints. We briefly go through some of the functions in this tutorial. For a complete list of functions, please check our [Doxygen](https://drake.mit.edu/doxygen_cxx/classdrake_1_1solvers_1_1_mathematical_program.html).

### Add linear cost
The easiest way to add linear cost is to call `AddLinearCost` function. We first demonstrate how to construct an optimization program with 2 decision variables, then we will call `AddLinearCost` to add the cost.

```python
from pydrake.solvers.mathematicalprogram import MathematicalProgram, Solve
import numpy as np

# Create an empty MathematicalProgram named prog (with no decision variables,
# constraints or costs)
prog = MathematicalProgram()
# Add two decision variables x[0], x[1].
x = prog.NewContinuousVariables(2, "x")
```

We can call `AddLinearCost(expression)` to add a new linear cost. `expression` is a symbolic linear expression of the decision variables.

```python
# Add a symbolic linear expression as the cost.
cost1 = prog.AddLinearCost(x[0] + 3 * x[1] + 2)
# Print the newly added cost
print(cost1)
# The newly added cost is stored in prog.linear_costs().
print(prog.linear_costs()[0])
```

If we call `AddLinearCost` again, the total cost stored in `prog` is the summation of all the costs. You can see that `prog.linear_costs()` will have two entries.

```python
cost2 = prog.AddLinearCost(2 * x[1] + 3)
print(f"number of linear cost objects: {len(prog.linear_costs())}")
```

If you know the coefficient of the linear cost as a vector, you could also add the cost by calling `AddLinearCost(e, f, x)` which will add a linear cost $e^Tx + f$ to the optimization program

```python
# We add a linear cost 3 * x[0] + 4 * x[1] + 5 to prog by specifying the coefficients
# [3., 4] and the constant 5 in AddLinearCost
cost3 = prog.AddLinearCost([3., 4.], 5., x)
print(cost3)
```

Lastly, the user can call `AddCost` to add a linear expression to the linear cost. Drake will analyze the structure of the expression, if Drake determines the expression is linear, then the added cost is linear.

```python
print(f"number of linear cost objects before calling AddCost: {len(prog.linear_costs())}")
# Call AddCost to add a linear expression as linear cost. After calling this function,
# len(prog.linear_costs()) will increase by 1.
cost4 = prog.AddCost(x[0] + 3 * x[1] + 5)
print(f"number of linear cost objects after calling AddCost: {len(prog.linear_costs())}")
```

### Add linear constraints
We have three types of linear constraints
  * Bounding box constraint. A lower/upper bound on the decision variable: $ lower \le x \le upper $.
  * Linear equality constraint: $Ax = b$.
  * Linear inequality constraint: $lower <= Ax <= upper$.

#### AddLinearConstraint and AddConstraint function
The easiest way to add linear constraints is to call `AddConstraint` or `AddLinearConstraint` function, which can handle all three types of linear constraint. Compared to the generic `AddConstraint` function, `AddLinearConstraint` does more sanity will refuse to add the constraint if it is not linear.

```python
prog = MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
y = prog.NewContinuousVariables(3, "y")

# Call AddConstraint to add a bounding box constraint x[0] >= 1
bounding_box1 = prog.AddConstraint(x[0] >= 1)
print(f"number of bounding box constraint objects: {len(prog.bounding_box_constraints())}")

# Call AddLinearConstraint to add a bounding box constraint x[1] <= 2
bounding_box2 = prog.AddLinearConstraint(x[1] <= 2)
print(f"number of bounding box constraint objects: {len(prog.bounding_box_constraints())}")

# Call AddConstraint to add a linear equality constraint x[0] + y[1] == 3
linear_eq1 = prog.AddConstraint(x[0] + y[1] == 3.)
print(f"number of linear equality constraint objects: {len(prog.linear_equality_constraints())}")

# Call AddLinearConstraint to add a linear equality constraint x[1] + 2 * y[2] == 1
linear_eq2 = prog.AddLinearConstraint(x[1] + 2 * y[2] == 1)
print(f"number of linear equality constraint objects: {len(prog.linear_equality_constraints())}")

# Call AddConstraint to add a linear inequality constraint x[0] + 3*x[1] + 2*y[2] <= 4
linear_ineq1 = prog.AddConstraint(x[0] + 3*x[1] + 2*y[2] <= 4)
print(f"number of linear inequality constraint objects: {len(prog.linear_constraints())}")

# Call AddLinearConstraint to add a linear inequality constraint x[1] + 4 * y[1] >= 2
linear_ineq2 = prog.AddLinearConstraint(x[1] + 4 * y[1] >= 2)
print(f"number of linear inequality constraint objects: {len(prog.linear_constraints())}")
```

`AddLinearConstraint` will check if the constraint is actually linear, and throw an exception if the constraint is not linear.

```python
# Add a nonlinear constraint square(x[0]) == 2 by calling AddLinearConstraint. This should
# throw an exception
try:
    prog.AddLinearConstraint(x[0] ** 2 == 2)
except RuntimeError as err:
    print(err.args)
```

If the users know the coefficients of the constraint as a matrix, they could also call `AddLinearConstraint(A, lower, upper, x)` to add a constraint $lower \le Ax \le upper$. This version of the method does not construct any symbolic representations, and will be more efficient especially when `A` is very large.

```python
# Add a linear constraint 2x[0] + 3x[1] <= 2, 1 <= 4x[1] + 5y[2] <= 3.
# This is equivalent to lower <= A * [x;y[2]] <= upper with
# lower = [-inf, 1], upper = [2, 3], A = [[2, 3, 0], [0, 4, 5]].
linear_constraint = prog.AddLinearConstraint(
    A=[[2., 3., 0], [0., 4., 5.]],
    lb=[-np.inf, 1],
    ub=[2., 3.],
    vars=np.hstack((x, y[2])))
print(linear_constraint)
```

#### AddBoundingBoxConstraint
If your constraint is a bounding box constraint (i.e. $lower \le x \le upper$), apart from calling `AddConstraint` or `AddLinearConstraint`, you could also call `AddBoundingBoxConstraint(lower, upper, x)`, which  will be slightly faster than `AddConstraint` and `AddLinearConstraint`.

```python
# Add a bounding box constraint -1 <= x[0] <= 2, 3 <= x[1] <= 5
bounding_box3 = prog.AddBoundingBoxConstraint([-1, 3], [2, 5], x)
print(bounding_box3)
```

If the variables share the same lower or upper bound, you could use a scalar `lower` or `upper` value in `AddBoundingBoxConstraint`. For example

```python
# Add a bounding box constraint 3 <= y[i] <= 5 for all i.
bounding_box4 = prog.AddBoundingBoxConstraint(3, 5, y)
print(bounding_box4)
```

#### AddLinearEqualityConstraint
If your constraint is a linear equality constraint (i.e. $ Ax = b$), apart from calling `AddConstraint` or `AddLinearConstraint`, you could also call `AddLinearEqualityConstraint` to be more specific (and slightly faster than `AddConstraint` and `AddLinearConstraint`).

```python
# Add a linear equality constraint 4 * x[0] + 5 * x[1] == 1
linear_eq3 = prog.AddLinearEqualityConstraint(np.array([[4, 5]]), np.array([1]), x)
print(linear_eq3)
```

### Solving Linear Program.

Once all the constraints and costs are added to the program, we can call `Solve` function to solve the program and call `GetSolution` to obtain the results.

```python
# Solve an optimization program
# min -3x[0] - x[1] - 5x[2] -x[3] + 2
# s.t 3x[0] + x[1] + 2x[2] = 30
#     2x[0] + x[1] + 3x[2] + x[3] >= 15
#     2x[1] + 3x[3] <= 25
#     -100 <= x[0] + 2x[2] <= 40
#   x[0], x[1], x[2], x[3] >= 0, x[1] <= 10
prog = MathematicalProgram()
# Declare x as decision variables.
x = prog.NewContinuousVariables(4)
# Add linear costs. To show that calling AddLinearCosts results in the sum of each individual
# cost, we add two costs -3x[0] - x[1] and -5x[2]-x[3]+2
prog.AddLinearCost(-3*x[0] -x[1])
prog.AddLinearCost(-5*x[2] - x[3] + 2)
# Add linear equality constraint 3x[0] + x[1] + 2x[2] == 30
prog.AddLinearConstraint(3*x[0] + x[1] + 2*x[2] == 30)
# Add Linear inequality constraints
prog.AddLinearConstraint(2*x[0] + x[1] + 3*x[2] + x[3] >= 15)
prog.AddLinearConstraint(2*x[1] + 3*x[3] <= 25)
# Add linear inequality constraint -100 <= x[0] + 2x[2] <= 40
prog.AddLinearConstraint(A=[[1., 2.]], lb=[-100], ub=[40], vars=[x[0], x[2]])
prog.AddBoundingBoxConstraint(0, np.inf, x)
prog.AddLinearConstraint(x[1] <= 10)

# Now solve the program.
result = Solve(prog)
print(f"Is solved successfully: {result.is_success()}")
print(f"x optimal value: {result.GetSolution(x)}")
print(f"optimal cost: {result.get_optimal_cost()}")
```