# VAE training tutorial

This is a tutorial training a [VAE](https://arxiv.org/abs/1312.6114) on MNIST and latent analysis

Structure:

  * create encoder and decoder, choose latent and model distributions.
  * VAE training
  * check likelihood for overfitting.
  * latent analysis

  Your tasks:

   * define the decoder distribution
   * define the encoder distribution
   * define the terms of the loss
   * define the samples and reconstruction tensors
   * run the KL analysis
   * run the latent traversal task
   * run the colab with a different number of latent dimensions (and see how that affects the kl analysis)

```
!pip install -q dm-sonnet
!pip install -q seaborn -U
```

## Imports

We will use Sonnet to define the encoder and decoder networks.

```
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function

import math

import tensorflow as tf
import sonnet as snt

import numpy as np

# Plotting library.
from matplotlib import pyplot as plt
import seaborn as sns
```

```
tf.logging.set_verbosity(tf.logging.ERROR)
```

```
sns.set(rc={"lines.linewidth": 2.8}, font_scale=2)
sns.set_style("whitegrid")
```

```
# Don't forget to select GPU runtime environment in Runtime -> Change runtime type
device_name = tf.test.gpu_device_name()
if device_name != '/device:GPU:0':
  raise SystemError('GPU device not found')
print('Found GPU at: {}'.format(device_name))
```

## Hyperparameters

```
BATCH_SIZE = 64  # @param
NUM_LATENTS = 20  # @param
TRAINING_STEPS = 10000 # @param
```

## Get the data

We will use the MNIST dataset. Luckly, TensorFlow comes with a simple way to load it.

```
mnist = tf.contrib.learn.datasets.load_dataset("mnist")
```

    Extracting MNIST-data/train-images-idx3-ubyte.gz
    Extracting MNIST-data/train-labels-idx1-ubyte.gz
    Extracting MNIST-data/t10k-images-idx3-ubyte.gz
    Extracting MNIST-data/t10k-labels-idx1-ubyte.gz

```
print(mnist.train.images.shape)
print(type(mnist.train.images))
```

    (55000, 784)
    <type 'numpy.ndarray'>

### Transform the data from numpy arrays to graph tensors.

This allows us to use TensorFlow datasets, which ensure that a new batch from the data is being fed at each session.run. This means that we do not need to use feed_dicts to feed data to each session.

```
def make_tf_data_batch(np_data, shuffle=True):
  # Reshape the data to image size.
  images = np_data.reshape((-1, 28, 28, 1))

  # Create the TF dataset.
  dataset = tf.data.Dataset.from_tensor_slices(images)

  # Shuffle and repeat the dataset for training.
  # This is required because we want to do multiple passes through the entire
  # dataset when training.
  if shuffle:
    dataset = dataset.shuffle(100000).repeat()

  # Batch the data and return the data batch.
  one_shot_iterator = dataset.batch(BATCH_SIZE).make_one_shot_iterator()
  data_batch = one_shot_iterator.get_next()
  return data_batch
```

```
real_data = make_tf_data_batch(mnist.train.images)
print(real_data.shape)
```

    (?, 28, 28, 1)

## Define the encoder and decoder (generator) networks

### Encoder network

We use a conv net encoder, and add a linear layer at the end - the latents will be 1D.

## Exercise: Fill in the distribution for the encoder

Remember: the encoder represents q(z|x)

```
class MnistEncoder(snt.AbstractModule):
  """The learned encoder distribution q(z|x).

    The output distribution will be a Normal distribution for each latent,
    with the mean and variance of the distribution being learned by gradient
    descent.

    The variables of this module get a loss from both the likelihood and
    the KL term. We do not have to worry about the reparamtrization trick, since
    TensorFlow implements that for us, for the Normal distribution.
  """

  def __init__(self, name='MnistEncoder'):
    super(MnistEncoder, self).__init__(name=name)

  def _build(self, input_image):
    """Constructs the encoder graph.

    Args:
      input_image: `tf.Tensor` input to be used by the encoder.

    Returns:
      `tf.Tensor` output with the decision of the encoder.
    """

    conv2d = snt.nets.ConvNet2D(
        output_channels=[8, 16, 32, 64, 128],
        kernel_shapes=[[5, 5]],
        strides=[2, 1, 2, 1, 2],
        paddings=[snt.SAME],
        activate_final=True,
        activation=tf.nn.relu,
        use_batch_norm=False)

    convolved = conv2d(input_image)

    # Flatten the data to 2D for getting the mean and variance from the encoder.
    flat_data = snt.BatchFlatten()(convolved)

    # TODO: build the encoder distribution q(z|x) for the given input.

```

## Exercise: Fill in the distribution for the decoder

Remember: the decoder represents p(x|z)

### Decoder network

We use a simple Transpose convolutional network. Because the input to the model is 2 dimensional, we first use a linear layer to transform the input latents to a size suitable for the conv network.

```
class MnistDecoder(snt.AbstractModule):
  """The learned decoder distribution p(x|z).

    The variables of this module get a loss from the likelihood term and are
    trained using gradient descent.
  """

  def __init__(self, name='MnistDecoder'):
    super(MnistDecoder, self).__init__(name=name)

  def _build(self, inputs):
    """Constructs the generator graph.

    Args:
      inputs: `tf.Tensor` with the input of the generator.

    Returns:
      `tf.Tensor`, the generated samples.
    """
    linear = snt.Linear(7 * 7 * 64)
    inputs = linear(inputs)

    # Reshape the data to have rank 4.
    inputs = snt.BatchReshape((7, 7, 64))(inputs)
    inputs = tf.nn.relu(inputs)

    net = snt.nets.ConvNet2DTranspose(
        output_channels=[32, 1],
        output_shapes=[[14, 14], [28, 28]],
        strides=[2],
        paddings=[snt.SAME],
        kernel_shapes=[[5, 5]],
        use_batch_norm=False)

   # TODO: build the decoder distribution p(x|z) for the given input latents.

```

## VAE logic

Variational bound:
\begin{equation}
 \mathbb{E}_{p^*(x)} \mathbb{E}_{q(z|x)} \log p_\theta(x|z)  - \mathbb{E}_{p^*(x)} KL(q(z|x)||p(z))
\end{equation}

The aim of the model is to maximize the variational lower bound, so the negative of the bound is minimized.

```
encoder = MnistEncoder()
decoder = MnistDecoder()

# Zero mean, unit variance prior.
prior_mean = tf.zeros(shape=(BATCH_SIZE, NUM_LATENTS), dtype=tf.float32)
prior_scale = tf.ones(shape=(BATCH_SIZE, NUM_LATENTS), dtype=tf.float32)
prior = tf.distributions.Normal(loc=prior_mean, scale=prior_scale)

print('Latent samples {}'.format(prior.sample().shape))
```

    Latent samples (?, 20)

## Define the graph for the loss

Since we want to maximize the bound on the log probability of the data, we minimize the negative of the bound.

The terms of the bound are: the log prob term, and the KL term.

### Log prob term

Reminder: this term ensures reconstructions will look good.

\begin{equation}
 \mathbb{E}_{p^*(x)} \mathbb{E}_{q^(z|x)} \log p_\theta(x|z)
\end{equation}

### KL term

Reminder: this term ensures model samples (obtained by sampling from the prior and doing a forward pass through the decoder) are close to data reconstructions (which are obtained by doing a pass through the encoder, getting posterior samples, and then using them to generate data reconstructions).

\begin{equation}
 - \mathbb{E}_{p^*(x)} KL(q(z|x)||p(z))
\end{equation}

## Exercise: Fill in the individual losses

The KL and the reconstruction loss.

```
def bound_terms(data_batch):

  # Define the posterior distribution
  posterior_distribution = ...

  # Samples from q(z |x )
  latent_sample = ...

  # Log prob of the original data under p_theta(  |z)
  all_log_probs = ...

  # Since we used broadcasting, log_prob of a Bernoulli will return a log_prob for
  # each dimension.
  all_log_probs.shape.assert_is_compatible_with([BATCH_SIZE, 28, 28, 1])

  # Reduce sum over the data dimensions
  likelihood_term = tf.reduce_sum(all_log_probs, axis=[1, 2, 3])

  # Reduce mean over the batch dimensions
  likelihood_term = tf.reduce_mean(likelihood_term)

  # Define the KL divergence
  all_kls = ...
  all_kls.shape.assert_is_compatible_with([BATCH_SIZE, NUM_LATENTS])

  # Reduce sum over the latent dimensions.
  kl_term = tf.reduce_sum(all_kls, axis=[1])

  # Reduce over the batch dimension.
  kl_term = tf.reduce_mean(kl_term)

  return likelihood_term, kl_term
```

## Exercise: Total Loss

Combine the likelihood term and the KL term to define the likelihood bound and the loss.

Remember: we will minimize the loss, but we want to maximize the bound.

```
# Maximize the data likelihodd and minimize the KL divergence between the prior and posterior
likelihood_term, kl_term = bound_terms(real_data)
train_bound = ...
loss =  ...
```

## Exercise: Define the sample and reconstruction tensors

Note: VAEs have no direct cost on samples. The quality of the samples improves, because the likelihood term ensures that the model will reconstruct well, while the KL term brings samples close to reconstructions.

```
samples = ...
```

```
reconstructions = ...
```

## Define the optimizer

We will train the encoder and decoder jointly, to minimize the same loss.

```
optimizer = tf.train.AdamOptimizer(0.001, beta1=0.9, beta2=0.9)
update_op = optimizer.minimize(loss)
```

## Create the tensorflow session

```
sess = tf.Session()

# Initialize all variables
sess.run(tf.global_variables_initializer())
```

## Training

```
losses = []

for i in xrange(TRAINING_STEPS):
  sess.run(update_op)

  if i % 100 == 0:
    iteration_loss = sess.run(loss)
    print('Iteration {}. Loss {}'.format(i, iteration_loss))
    losses.append(iteration_loss)
```

    Iteration 0. Loss 608.814880371
    Iteration 100. Loss 213.377548218
    Iteration 200. Loss 194.081817627
    Iteration 300. Loss 164.966598511
    Iteration 400. Loss 144.333953857
    Iteration 500. Loss 137.072845459
    Iteration 600. Loss 132.694793701
    Iteration 700. Loss 122.336486816
    Iteration 800. Loss 116.622253418
    Iteration 900. Loss 122.743522644

## Visualize the loss in time

```
plt.figure(figsize=(10,5))
plt.plot(losses, label='Negative log likelihood Bound')
plt.xlabel('Time')
plt.legend()
```

```

```

## Visualize samples, reconstructions

```
def gallery(array, ncols=10, rescale=True):
    if rescale:
      array = (array + 1.) / 2
    nindex, height, width, intensity = array.shape
    nrows = nindex//ncols
    assert nindex == nrows*ncols
    # want result.shape = (height*nrows, width*ncols, intensity)
    result = (array.reshape(nrows, ncols, height, width, intensity)
              .swapaxes(1,2)
              .reshape(height*nrows, width*ncols, intensity))
    return result

```

```
real_data_examples = sess.run(real_data)
final_samples = sess.run(samples)
data_reconstructions = sess.run(reconstructions)
print(data_reconstructions.shape)
```

    (64, 28, 28, 1)

### Real data

```
plt.gray()
plt.axis('off')
plt.imshow(gallery(real_data_examples, ncols=8).squeeze(axis=2))
```

### Samples

```
plt.gray()
plt.axis('off')
plt.imshow(gallery(final_samples, ncols=8).squeeze(axis=2))
```

### Reconstructions

```
plt.gray()
plt.axis('off')
plt.imshow(gallery(data_reconstructions, ncols=8).squeeze(axis=2))
```

## Final training log prob aver multiple batches

```
NUM_TEST_BATCHES = 10 # @param
```

```
train_log_prob = []

for _ in xrange(NUM_TEST_BATCHES):
  train_log_prob.append(sess.run(train_bound))

train_avg_log_prob = np.mean(train_log_prob)

print('Averaged training log prob bound {}'.format(train_avg_log_prob))
```

    Averaged training log prob -116.380203247

## Exercise: Checking for overfitting - test likelihoods

We want to ensure that the model has not overfitted - to do that, we see if it gives a higher likelihood to training data compared to test data. If this is the case, we can conclude the model is overfitting.

### Get test data

```
test_data_batch = make_tf_data_batch(mnist.test.images)
```

```
test_bound = ...
```

```
test_log_prob = []

for _ in xrange(NUM_TEST_BATCHES):
  test_log_prob.append(sess.run(test_bound))

test_avg_log_prob = np.mean(test_log_prob)

print('Averaged test log prob bound {}'.format(test_avg_log_prob))

```

    Averaged test log prob -115.400169373

## Analyse the structure of the latent variables

VAEs are generative and inference models. So far we have looked at samples, and reconstructions, but we have not looked at the learned representations.

We will now analyze the learned VAE representations by looking at the KLs per latent, but also by doing latent space traversals.

Remember that the KL term is important, since the KL term ensures that the quality of the samples is going to be close to that of the reconstrunctions.

```
num_batches = int(mnist.train.images.shape[0] / BATCH_SIZE)
data_batch = make_tf_data_batch(mnist.train.images, shuffle=False)
```

```
posterior_distribution = encoder(data_batch)

all_kls = tf.distributions.kl_divergence(posterior_distribution, prior)
all_kls.shape.assert_is_compatible_with([BATCH_SIZE, NUM_LATENTS])

# Reduce over the batch dimension.
kls = tf.reduce_mean(all_kls, axis=[0])
kls.shape.assert_is_compatible_with([NUM_LATENTS])
```

```
aggregate_kl = np.zeros([NUM_LATENTS])
for _ in xrange(num_batches):
  aggregate_kl += sess.run(kls)

aggregate_kl /= num_batches
```

### Plot the kl divergences per latent

Because we used a factorizable prior (a Gaussian with unit covariance), the KL divergence
also factorizes, and we can see a KL divergence per unit). Here we analyze the KL divergence of each unit, to see how the model uses each latent unit.

We will see that if we increase the number of latent variables, the VAE "turns off" units, by setting their distribution to that of the prior.

```
def get_grid_size(num_latents):
  num_latents_sqrt = np.sqrt(num_latents)
  width = int(num_latents_sqrt)
  while num_latents % width !=0:
    width -= 1
  return width, int(num_latents / width)

aggregate_kl = np.reshape(aggregate_kl, get_grid_size(NUM_LATENTS))
cmap = sns.cubehelix_palette(4, start=2, rot=0, dark=0, light=.96)
np.random.shuffle(aggregate_kl)
sns.heatmap(aggregate_kl, vmin=0, vmax=2, linewidths=.05, cmap=cmap)
plt.axis('off')

```

## Latent space interpolation

By doing an interpolation in latent space, we can analyze the behaviour of the model.

```
a = np.linspace(0.0, 1.0, 100)
a = np.expand_dims(a, axis=1)

first_latents = tf.random_normal((1, NUM_LATENTS))
second_latents = tf.random_normal((1, NUM_LATENTS))

interpolations =  np.sqrt(a) * first_latents + np.sqrt(1 - a) * second_latents
# interpolations =  a * first_latents + (1 - a) * second_latents

samples_from_interpolations = decoder(interpolations).mean()

```

```
samples_from_interpolations_np = sess.run(samples_from_interpolations)
```

```
print(samples_from_interpolations_np.shape)
```

    (100, 28, 28, 1)

```
plt.gray()
plt.axis('off')
plt.imshow(gallery(samples_from_interpolations_np).squeeze(axis=2))
plt.show()
```

### Exercise: change the number of latents (to 100) and see what happens with the KL plot.

## Close session

```
# sess.close()
```

## Reflections

* What can VAEs be used for?
* How do VAEs learn to sample?

### VAE samples on other datasets

Note: these are samples with the vanilla algorithm, but they can be improved using better complex distributions.

#### CIFAR-10

#### CelebA
#


# Catching Heuristics Are Robust to Systematic Disturbances  and Can Be Found by Reinforcement Learning

## Supplementary Material: Proofs for Chapman's Strategy (Section 4)

This material accompanies my doctoral thesis *On Decomposability in Robot Reinforcement Learning* and the paper *Catching Heuristics Are Robust to Systematic Disturbances  and Can Be Found by Reinforcement Learning*.

© 2017 Sebastian Höfer

In this notebook, we verify the proofs regarding Chapman's strategy in Section 4 of the paper (Thesis Chapter 4).

```python
from IPython.display import Image, display

import sympy as sp
import numpy as np

sp.init_printing()

import os
import sys
# load scripts
#sys.path.append(os.path.join(os.getcwd(), "../../src"))
#import multi_experiment_plot
#from utils import pdfcrop

def concrete_case(formula, V_, theta_, yb0_=0):
	global GRAVITY
	return formula.subs(g,GRAVITY).subs(V,V_).subs(theta,theta_).subs(yb0,yb0_)

def concrete_trajectory(formula, V_, theta_, T_, yb0_=0, N=100):
	time = np.linspace(0, float(T_), N)
	return np.array(map(lambda t_: concrete_case(formula, V_, theta_, yb0_).subs(t, t_), time)), time, time[1]-time[0]
```

```python
%pylab inline

import matplotlib
#matplotlib.rcParams['ps.useafm'] = True
#matplotlib.rcParams['pdf.use14corefonts'] = True
matplotlib.rcParams['text.usetex'] = True
matplotlib.rcParams['text.latex.preamble'] = r"\usepackage{charter}, \usepackage{tgheros}, \renewcommand*\familydefault{\sfdefault}"
matplotlib.rcParams['figure.autolayout'] = True
matplotlib.rcParams['font.size'] = 24

#from matplotlib import rc
#rc('font',**{'family':'serif','serif':['Computer Modern Roman'], 'size': 30})

matplotlib.use('pdf')
```

    Populating the interactive namespace from numpy and matplotlib

    /Users/Hoefer/anaconda/lib/python2.7/site-packages/matplotlib/__init__.py:1350: UserWarning:  This call to matplotlib.use() has no effect
    because the backend has already been chosen;
    matplotlib.use() must be called *before* pylab, matplotlib.pyplot,
    or matplotlib.backends is imported for the first time.

      warnings.warn(_use_error_msg)

```python
def plot_surface(fig, ax, title, X, Y, Z, labels, linewidth=0.2, vlim=None, zlim=[None,None], contour=False, elev=20, azi=45):
  if vlim is None:
    vlim = zlim
  ax.set_title(title)
  #ax = fig.gca(projection='3d')

  xlabelpad, ylabelpad, zlabelpad = [35,35,10]

  if zlim[1] is not None:
    Z[np.where(Z > zlim[1])] = zlim[1]

  surf = ax.plot_surface(X, Y, Z, rstride=1, cstride=1, cmap=cm.coolwarm, vmin=vlim[0], vmax=vlim[1],
        linewidth=linewidth, antialiased=False, )
  ax.zaxis.set_major_locator(LinearLocator(10))
  ax.zaxis.set_major_formatter(FormatStrFormatter('%.01f'))
  ax.set_xlabel(labels[0], labelpad=xlabelpad)
  ax.set_ylabel(labels[1], labelpad=ylabelpad)
  ax.set_zlabel(labels[2], labelpad=zlabelpad)

  if zlim[0] == 0 and zlim[1] == 5:
    # hacky: for agent_ball_distance
    ax.set_zticks([0,1,2,3,4,5])
    ax.set_zticklabels(map (lambda x: "$%d$" % x, [0,1,2,3,4,5]))

  # reformat ticklabels
  for ticks_and_labels, setter, in zip([ zip(ax.get_xticks(), ax.get_xticklabels()),
                                        zip(ax.get_yticks(), ax.get_yticklabels()),
                                        zip(ax.get_zticks(), ax.get_zticklabels())],
                                        (ax.set_xticklabels, ax.set_yticklabels, ax.set_zticklabels)):
    ticklbls = []
    fnt_size = "huge"

    tkz = zip(*ticks_and_labels)[0]
    tick_range =  np.max(tkz) - np.min(tkz)

    for tick, lbl in ticks_and_labels:
      #txt = lbl.get_text()
      #if txt == "":
      if tick_range <= 1.5:
        tl = "%.1f" % tick
      else:
        tl = "%d" % tick
      txt = r"\%s{$%s$}" % (fnt_size, tl)
      #else:
      #  txt = r"\%s{%s}" % (fnt_size, tick)
      lbl.set_text(txt)
      ticklbls.append(lbl)
    setter(ticklbls)

  # move y ticks a bit to the left, so the -15 does not collide
  [t.set_ha('right') for t in ax.get_yticklabels()]

  vmin = 0.
  vmax = 1.

  ax.view_init(elev, azi)

  if contour:
    if np.min(Z) != np.max(Z):
      cset = ax.contour(X, Y, Z, zdir='z', offset=np.min(Z), cmap=cm.coolwarm)
    if np.min(X) != np.max(X):
      #cset = ax.contour(X, Y, Z, zdir='x', offset=np.min(X), cmap=cm.coolwarm)
      cset = ax.contour(X, Y, Z, zdir='x', offset=np.max(X), cmap=cm.coolwarm) # project to opposite side
    if np.min(Y) != np.max(Y):
      #cset = ax.contour(X, Y, Z, zdir='y', offset=np.min(Y), cmap=cm.coolwarm)
      cset = ax.contour(X, Y, Z, zdir='y', offset=np.max(Y), cmap=cm.coolwarm) # project to opposite side

  if zlim != [None,None]:
    ax.set_zlim3d(zlim[0], zlim[1])
  fig.colorbar(surf, shrink=0.75, aspect=20)

  return ax

import subprocess

def pdfcrop(path):
  #call = ["perl", "/usr/bin/pdfcrop", path, path  ]
  call = ["pdfcrop", path, path  ]
  print " ".join(call)
  pipe = subprocess.Popen(call)
  return pipe

```

```python
# HElPER FUNCTIONS
# sympy has problems of simplifying (V^2 sin(theta)^2)^{1/2} to V sin(theta)
def sympy_sqrt_fix(formula):
#  return sp.simplify(formula.subs((V**2*sp.sin(theta)**2)**(0.5), V*sp.sin(theta)))
  return sp.simplify(formula.subs(sp.sqrt( (V**2) * (sp.sin(theta))**2), V*sp.sin(theta)))
def zero_yb0(formula, val=0):
  return sympy_sqrt_fix(formula.subs(yb0,val))

GRAVITY=9.81

# Variable
t, yb0, theta, psi, V, g, rv = sp.symbols("t, b_{y0}, \\varphi, \\psi, \\nu, g, \\vvaref")
#yb0 = 0
yb = V*sp.sin(theta)*t - 0.5*g*t*t + yb0
xb = V*sp.cos(theta)*t

T = sp.solve(yb, t)[1]
R = xb.subs(t, T)
```

$\newcommand{\vva}{\theta}
\newcommand{\dvva}{\dot{\theta}}
\newcommand{\ddvva}{\ddot{\theta}}
\newcommand{\dddvva}{\dddot{\theta}}
\newcommand{\vvaref}{\dot{\theta}_\mathrm{ref}}
\newcommand{\vvarefzero}{\dot{\theta}_{\mathrm{ref},0}}
\newcommand{\vvarefcon}{{\vvaref^*}}
\newcommand{\vvarefincon}{{\vvaref^{\sim}}}
\newcommand{\xyzb}{\mathbf{b}}
\newcommand{\dxyzb}{\dot{\mathbf{b}}}
\newcommand{\ddxyzb}{\ddot{\mathbf{b}}}
\newcommand{\xb}{{b_x}}
\newcommand{\dxb}{{\dot{b}_x}}
\newcommand{\ddxb}{{\dot{b}_x}}
\newcommand{\yb}{{b_y}}
\newcommand{\dyb}{{\dot{b}_y}}
\newcommand{\ddyb}{{\ddot{b}_y}}
\newcommand{\zb}{{b_z}}
\newcommand{\dzb}{{\dot{b}_z}}
\newcommand{\ddzb}{{\ddot{b}_z}}
\newcommand{\xbzero}{{b_{x,0}}}
\newcommand{\ybzero}{{b_{y,0}}}
\newcommand{\dxbzero}{{\dot{b}_{x,0}}}
\newcommand{\dybzero}{{\dot{b}_{y,0}}}
\newcommand{\xyza}{\mathbf{a}}
\newcommand{\dxyza}{\dot{\mathbf{a}}}
\newcommand{\ddxyza}{\ddot{\mathbf{a}}}
\newcommand{\xa}{{a_x}}
\newcommand{\dxa}{{\ddot{a}_x}}
\newcommand{\ddxa}{{\ddot{a}_x}}
\newcommand{\za}{{a_z}}
\newcommand{\dza}{{\ddot{a}_z}}
\newcommand{\ddza}{{\ddot{a}_z}}
\newcommand{\xazero}{{a_{x,0}}}
\newcommand{\aref}{a_\mathrm{ref}}
\newcommand{\daref}{\dot{a}_\mathrm{ref}}
\newcommand{\arefcon}{{\aref^*}}
\newcommand{\darefcon}{{\daref^*}}
\newcommand{\azero}{{a_0}}
\newcommand{\azeroincon}{{a_0^{\sim}}}
\newcommand{\damax}{\dot{a}_\mathrm{max}}
\newcommand{\ddamax}{\ddot{a}_\mathrm{max}}
\newcommand{\vvareftext}{tangent reference velocity\xspace}
\newcommand{\areftext}{agent reference\xspace}
\newcommand{\refcontext}{consistent\xspace}
\newcommand{\refincontext}{inconsistent\xspace}
\newcommand{\refcontextup}{Consistent\xspace}$

## Theorem 4.1.1 (Chapman's Proof)

We begin by formalizing the ball's trajectory. We assume that it is a parabola, parametrized by initial ball throwing velocity $V$, throwing angle $\theta$ and initial height $y_{b,0}$:
\begin{align}
  b_x(t) &= \nu \cos(\varphi) t + b_{x,0},\\
  b_y(t) &= \nu \sin(\varphi) t - \frac{1}{2} g t^2 + b_{y,0}.
\end{align}
W.l.o.g. we assume $b_x(0)=b_{x,0}=:=0$ and $b_y(0)=b_{y,0} \geq 0$.

We compute the impact time $T$ and the impact position (or ball range) $R$:

```python
T
```

```python
R
```

In the following we will inspect the case $b_{y,0}=0$

Then the previous expressions simplify to:

$T=$

```python
T = zero_yb0(T)
T
```

$R=$

```python
R = zero_yb0(R)
R
```

We now relate the tangent reference velocity $\vvaref$ to the motion of agent and ball, i.e. $\dvva=0$.

Ansatz: Assume $\tan \alpha$ as seen by the agent is a linear function with constant slope $\vvaref$.
\begin{align}
\frac{\yb(t)}{\aref(t)-\xb(t)} &= \vvaref t + \vva_{0} \\
&= \vvaref\ t + \frac{\ybzero}{\azero}  \qquad (1).
\end{align}
with $\aref(0)=\azero \neq 0$, $\xb(0)=0$.

```python
# Set-up Eq.(1)

a0 = sp.symbols("a_0")
a = sp.Function(r"\aref")(t)

# we know rv0 = yb(0)/(a(0)-xb) = yb(0)/a(0) = yb0/a0   (since xb=0)
rv0 = yb0/a0

tan_alpha_eq =  yb/(a-xb)
linear_rv_eq = rv*t + rv0
eq = tan_alpha_eq - linear_rv_eq
```

We now solve this Eq. (1) for $\aref(t)$ and verify that $\aref(T) = R$.

$\aref(t)=$

```python
#a_expr = sp.solve(eq, a)[0]
a_expr = sp.solve(zero_yb0(eq), a)[0]
#print sp.latex(a_expr)
a_expr
```

    /Users/Hoefer/anaconda/lib/python2.7/site-packages/matplotlib/font_manager.py:1288: UserWarning: findfont: Font family [u'sans-serif'] not found. Falling back to Bitstream Vera Sans
      (prop.get_family(), self.defaultFamily[fontext]))

$$\frac{1}{\vvaref} \left(\nu \vvaref t \cos{\left (\varphi \right )} + \nu \sin{\left (\varphi \right )} - 0.5 g t\right)$$

```python
# agent at terminal time
#sp.simplify(a_expr.subs(t, zero_yb0(T)))
```

$\aref(T) - R=$

```python
sp.simplify(a_expr.subs(t, T) - R)
```

$a(0)=$

```python
sp.simplify(a_expr.subs(t, 0))
#a_expr.subs(t, 0)
```

$$\frac{\nu}{\vvaref} \sin{\left (\varphi \right )}$$

That shows that $\aref(T) = R$. We can verify that this holds in the limit, too.

Assume $\ybzero=0$ and $t=T-\delta$, i.e. shortly before impact. How far is the agent away from impact point $R$?

```python
delta = sp.symbols("delta")
ateps_m_R = sp.simplify(zero_yb0(a_expr.subs(t, T-delta) - R))
#ateps_m_R = sp.simplify(a_expr.subs(t, T-delta) - R)
##print sp.latex(ateps_m_R)
ateps_m_R
#sp.limit(ateps_m_R, delta, 0)

```

$$- 1.0 \nu \delta \cos{\left (\varphi \right )} + \frac{0.5 \delta}{\vvaref} g$$

```python
# re-arrange
ateps_m_R = sp.simplify(zero_yb0(a_expr.subs(t, T-delta) - R))
ateps_m_R2 = delta * (-V*sp.cos(theta) + 0.5*g/rv)
assert(sp.simplify(ateps_m_R2-ateps_m_R) == 0.)
##print sp.latex(ateps_m_R2)
ateps_m_R2
```

$$\delta \left(- \nu \cos{\left (\varphi \right )} + \frac{0.5 g}{\vvaref}\right)$$

$= \aref(T-\delta) - R$. We see that $\delta$ is multiplied with a constant expression which depends on $\vvaref$. Hence, for $\delta \rightarrow 0: |\aref(T-\delta)-R| \rightarrow 0$.

However, we still need to make sure to choose $\vvaref$ s.t. $\aref(0)=a_0$.

## Lemma 4.1.2

If we use Chapman's result, the *consistent* tangent velocity reference computes as $\vvarefcon=$

```python
rv_opt = sp.solve(zero_yb0(a_expr-a0).subs(t,0), rv)[0]
rv_opt
```

and plug this into $\aref(\delta)$, i.e. shortly after the ball is thrown, we obtain:

```python
a_delta_raw = zero_yb0(a_expr.subs(t, delta).subs(rv, rv_opt))
# re-arrange
a_delta = a0 + delta * (V*sp.cos(theta) - a0*g/(2*V*sp.sin(theta)))
assert (sp.simplify(a_delta - a_delta_raw) == 0.)
a_delta
```

As $\delta \rightarrow 0$ only the term $a_0$ remains.

## Lemma 4.1.3

So there exists a unique $\vvaref$ which fulfills the constraints $\aref(0)=a_0, \aref(T)=R$.

What does the *consistent* agent reference trajectory $\arefcon(t)$ look like?

```python
#at_rvstar_yb0_zero = a_delta.subs(delta, t)
#print sp.latex(at_rvstar_yb0_zero)
#at_rvstar_yb0_zero

at_rvstar_yb0_zero = a_expr.subs(rv, rv_opt)
##print sp.latex(at_rvstar_yb0_zero)
at_rvstar_yb0_zero
```

$=\arefcon(t)$. We see that its derivative is constant:

```python
#adot0 = zero_yb0( (R - a0 ) / T)
at_rvstar_yb0_zero_diff = sp.diff(at_rvstar_yb0_zero, t)
at_rvstar_yb0_zero_diff = sp.simplify(at_rvstar_yb0_zero_diff)
##print sp.latex(at_rvstar_yb0_zero_diff)
at_rvstar_yb0_zero_diff
#sp.simplify(at_rvstar_yb0_zero_diff - adot0)
```

$= \darefcon$.

It corresponds to the velocity the agent needs to arrive at $R$ at time $T$ starting at $a_0$ at time $t=0$, which is $\frac{R-a_0}{T}$:

```python
adot0 = zero_yb0( (R - a0 ) / T)
sp.simplify(at_rvstar_yb0_zero_diff - adot0)
```

This shows that the consistent agent reference trajectory is constant.

Note that this result is similar to (Chapman, 1968), Eq. (9).

### Figure 4.1:  Plotting $\vvaref$

Given this expression we can now plot what the consistent $\vvarefcon$ for reaching the target looks like, with varying throwing angle $\nu$ and distance from impact point $d = -R+a_0$. We keep the throwing angle fixed at $\varphi=\frac{\pi}{4}$ since it is only a scaling factor ($\sin \frac{\pi}{4} \approx 0.7$):

```python
#from matplotlib import rcParams
#rcParams.update({'figure.subplot.bottom': 0.1})

import matplotlib.pyplot as plt

from mpl_toolkits.mplot3d import Axes3D

#%pylab tk

X = np.linspace(-50, 50, 15)  # distance d
Y = np.linspace(5, 50, 25)    # ball velocity V

expr = rv_opt

t_ = 0
#theta_ = np.pi/4
yb0_ = 0.0

for theta_ in [np.pi/4]: #[np.pi/4, np.pi/8, 3*np.pi/8]:

    #fig = plt.figure(figsize=(15,10))
    fig = plt.figure(figsize=(10,7))
    ax = fig.add_subplot(111, projection='3d')

    DIFF = np.zeros( (len(X), len(Y)) )
    for i,d_ in enumerate(X):
        for j,V_ in enumerate(Y):

            R_ = concrete_case(R, V_, theta_, yb0_)
            T_ = concrete_case(T, V_, theta_, yb0_)
            cc = concrete_case(expr,  V_, theta_, yb0_).subs(t, t_)

            val = cc.subs(a0,R_+d_).evalf()
            DIFF[i,j] = np.abs(float(val)) if val != sp.zoo else 0.
            # clamp diff
            DIFF[i,j] = DIFF[i,j] if DIFF[i,j] <= 1 else 1
    DIFF = DIFF.T

    X,Y = np.meshgrid(X,Y)

    vvarefopt = r"\dot{\theta}_\mathrm{ref}^*"

    # clamp diff
    labels = [r"\begin{center}initial distance to \\impact point $D_0$ \end{center}",
              r"\begin{center}initial ball \\ velocity $\nu$ \end{center}",
              r"\begin{center}$%s$\end{center}" % vvarefopt ,]

    if theta_ == np.pi/8:
        pi_str = r"\frac{\pi}{8}"
        pi_slug = "pi8"
    elif theta_ == np.pi/4:
        pi_str = r"\frac{\pi}{4}"
        pi_slug = "pi4"
    elif theta_ == 3*np.pi/8:
        pi_str = r"\frac{3\pi}{8}"
        pi_slug = "3pi8"

    title = r"Consistent $%s$ ($\nu = %s$)" % (vvarefopt, pi_str)
    ax = plot_surface(fig, ax, title, X, Y, DIFF, labels, zlim=(0,1), contour=False,
                                            elev=20., azi=-40.)
                                            #elev=15, azi=-135)
    ax.zaxis.set_rotate_label(False)
    #plt.tight_layout()

    plt.subplots_adjust(top=9.5, bottom=8.5)
    plt.tight_layout()

    fn = "v_r_optimal_%s.pdf" % pi_slug
    plt.savefig(fn,bbox_inches='tight', pad_inches=1.)
    pdfcrop(fn)

#plt.show()
```

## Lemma 4.1.5: Disturbed Trajectories

Although the previous findings give support for Chapman's strategy, they only hold for perfectly parabolic ball trajectories. Realistic ball trajectories are not perfectly parabolic due to random disturbances such as spin and wind or systematic disturbances such as air resistance.
We therefore extend our previous results by showing that all VVA implementations are robust to any additive or multiplicative perturbation of the ball's horizontal component. If we assume that any random or systematic disturbance can be approximated by such perturbations this insight provides evidence that Chapman's strategy successfully copes with realistic ball trajectories.

#### Proof

We consider arbitrary additive perturbations to the ball trajectory:

$\xb'(t) = \xb(t) + \varepsilon(t)$

And then we solve for $\aref(t)$.

```python
eps_x = sp.Function("varepsilon")(t)
xb_e = xb+eps_x

tan_alpha_eq_e =  yb/(a-xb_e)
eq_e = tan_alpha_eq_e - linear_rv_eq
agent_t_e = sp.solve(zero_yb0(eq_e), a)[0]
agent_t_e = sp.simplify(agent_t_e)
##print sp.latex(agent_t_e)
agent_t_e
```

$$\frac{1}{\vvaref} \left(\nu \sin{\left (\varphi \right )} + \vvaref \left(\nu t \cos{\left (\varphi \right )} + \varepsilon{\left (t \right )}\right) - 0.5 g t\right)$$

$= \aref'(t)$

How do the agent trajectory in case of undisturbed and disturbed ball flight relate?

```python
# undisturbed
agent_t = sp.solve(zero_yb0(eq), a)[0]

# difference between undisturbed and disturbed trajectory
sp.simplify(agent_t_e - agent_t)
```

We see that the disturbed trajectory only differs by $\varepsilon$. Hence, if the agent compensates the disturbance $\varepsilon$ in $x$ direction the same guarantees as previously hold: the agent will intercept the ball and $rv$ will remain constant.

### Example: Approximating drag (Figure 4.4)

As an example, consider a noise term which varies quadratically w.r.t. time. The characteristics of such a type of disturbance are very similar to air resistance trajectories.

(We only show this by example; there is no closed-form equation for drag trajectories, therefore we cannot show this analytically.)

```python
eps_x = sp.Symbol("varepsilon")
xb_e = xb+eps_x*t**2
```

```python
# HELPER FUNCTIONS
def ball_trajectory_with_drag(v0, theta, dt, t1):
    # drag parameters
    mass = 0.15
    r = 0.0366
    c = 0.5
    A = np.pi * r * r
    rho = 1.293
    global GRAVITY

    x = 0
    y = 0
    vx = v0 * np.cos(theta)
    vy = v0 * np.sin(theta)
    ax = 0
    ay = -GRAVITY

    T = np.arange(dt,t1,dt)
    X = np.zeros( (T.shape[0]+1, 5) )
    X[0,:] = (0, x, y, vx, vy)

    print X[0,:]

    for i,t in enumerate(T):
        x += vx * dt + 0.5*ax*dt*dt
        y += vy * dt + 0.5*ay*dt*dt
        vx += ax*dt
        vy += ay*dt

        v = np.array( [vx,vy] )
        ax, ay = [0, -GRAVITY] - v*v * 0.5 * rho * c * A/mass

        y = 0.0 if y <= 0 else y

        X[i+1,:] = (t, x, y, vx, vy)

    return X[:i+1,:]

# %pylab inline

V_ = 30
theta_ = np.pi/4
T_ = concrete_case(T, V_, theta_)

tau_x, t_range, dt= concrete_trajectory(xb, V_, theta_, T_, 0)
tau_y, t_range, dt= concrete_trajectory(yb, V_, theta_, T_, 0)
tau_x_eps, t_range, dt= concrete_trajectory(xb_e.subs(eps_x,-1.11), V_, theta_, T_, 0)

tau_drag = ball_trajectory_with_drag(V_*1.11, theta_, dt=dt, t1=T_)

# import matplotlib
# matplotlib.rcParams['ps.useafm'] = True
# matplotlib.rcParams['pdf.use14corefonts'] = True
# matplotlib.rcParams['text.usetex'] = True
# #matplotlib.rcParams['text.latex.preamble'] = [\
#     #r"\usepackage{accents}",]
# matplotlib.rcParams['figure.autolayout'] = True

# from matplotlib import rc
# rc('font',**{'family':'serif','serif':['Computer Modern Roman'], 'size': 30})

matplotlib.rcParams['font.size'] = 24

plt.figure(figsize=(8,4))
plt.plot(tau_x, tau_y, lw=5.0, c="k", ls='-', label="Ideal")
plt.plot(tau_drag[:,1], tau_drag[:,2], lw=5.0, c="b", label="Drag")
plt.plot(tau_x_eps, tau_y, lw=7.0, c="r", ls='--', label="Pseudo-drag")
plt.legend(fontsize=16)#, loc='upper right', bbox_to_anchor=(0.5, 0.5))
plt.xlabel("x")
plt.ylabel("y")
plt.xlim(-5, 130)
plt.ylim(0, 25) #np.max(tau_y))

plt.savefig("pseudo_drag_trajectory.pdf")
```

We see that the pseudo-drag trajectory (red) which is computed by adding $\varepsilon(t)=-1.11 t^2$ to $x_b$ is highly similar to a ball trajectory with drag (blue).

#### Multiplicative Noise

It is similarly easy to show, that multiplicative noise can be compensated by the agent, too.

```python
eps_x = sp.Function("varepsilon")(t)
xb_e = xb*eps_x

tan_alpha_eq_e =  yb/(a-xb_e)
eq_e = tan_alpha_eq_e - linear_rv_eq
agent_t_e = sp.solve(zero_yb0(eq_e), a)[0]
agent_t_e

sp.simplify(agent_t_e - agent_t)
```

Note that these results only hold if the agent does not have any velocity or acceleration constraints which prevent it from executing the action required to compensate the disturbance.

## Lemma 4.1.4 (Deviating from Consistent $\vvarefcon$)

### Example

We have previously shown that for given $a_0$ there exists an $\vvaref = \frac{\nu}{a_0} \sin \varphi$ which will result in the agent running towards the impact point with constant speed. But what if given an initial position $a_0$ we use a different $\vvarefincon \neq \vvaref$ for control?

```python
# CONCRETE EXAMPLE
N = 100

V_ = 30.
theta_ = np.pi/4
yb0_ = 1e-15 # 0 is undefined!
D_ = 10.

eq_yb0 = sympy_sqrt_fix(eq.subs(yb0, yb0_))

agent_t = sp.solve(eq, a)[0]

R_ = concrete_case(sympy_sqrt_fix(R.subs(yb0, yb0_)), V_, theta_, yb0_)
T_ = concrete_case(sympy_sqrt_fix(T.subs(yb0, yb0_)), V_, theta_, yb0_)
a0_ = R_+D_ # 5m right of landing point

adot0_ = sp.simplify( (R_ - a0_ ) / T_)

rv_const_speed_eq = eq_yb0.subs(a, adot0_*t + a0)
rv_const_speed_eq = sp.simplify(sp.solve(rv_const_speed_eq, rv)[0])

rv_const_speed_ = concrete_case(rv_const_speed_eq, V_, theta_, yb0_).subs(a0, a0_).subs(t,0)
agent_tau_const_speed, t_range, dt = concrete_trajectory(agent_t.subs(rv, rv_const_speed_).subs(a0, a0_),  V_, theta_, T_, yb0_)
```

We calculated the consistent reference $\vvarefcon$ as follows:

```python
rv_const_speed_
```

```python
print "R=%f" % R_
```

    R=91.743119

We check the first and last steps of the agent's trajectory and see that the agent arrives at the impact point:

```python
agent_tau_const_speed[:5], agent_tau_const_speed[-5:,]
```

    (array([101.743119266055, 101.642109165045, 101.541099064035,
            101.440088963025, 101.339078862015], dtype=object),
     array([92.1471596700954, 92.0461495690853, 91.9451394680753,
            91.8441293670651, 91.7431192660550], dtype=object))

Now we investigate what happens if we alter $\vvarefcon$ and observe the agent's trajectory using $\vvarefincon = \vvarefcon + \delta$.

```python
N = 1000
delta_ = -0.02

rv_tilde_ = rv_const_speed_+delta_
agent_tau_const_speed_tilde, t_range, dt = concrete_trajectory(agent_t.subs(rv, rv_tilde_).subs(a0, a0_),  V_, theta_, T_, yb0_, N)
```

Again we check the trajectory:

```python
agent_tau_const_speed_tilde[:5], agent_tau_const_speed_tilde[-5:],
```

    (array([101.743119266055, 112.517462570583, 112.496646595328,
            112.475830620073, 112.455014644818], dtype=object),
     array([91.8263831670752, 91.8055671918202, 91.7847512165651,
            91.7639352413101, 91.7431192660550], dtype=object))

Again, the agent reaches the ball - but it moves with very large acceleration in the opposite direction, i.e. away from the ball impact point -  to position $\azeroincon \approx 125$. We now show that for starting posiition  $\azeroincon$, $\vvarefincon$ would be the optimal reference velocity.

```python
# CONCRETE EXAMPLE
N = 1000

R_tilde = concrete_case(sympy_sqrt_fix(R.subs(yb0, yb0_)), V_, theta_, yb0_)
T_tilde = concrete_case(sympy_sqrt_fix(T.subs(yb0, yb0_)), V_, theta_, yb0_)
a0_tilde = agent_tau_const_speed_tilde[1]

adot0_tilde = sp.simplify( (R_tilde - a0_tilde ) / T_tilde)

rv_const_speed_tilde = concrete_case(rv_const_speed_eq, V_, theta_, yb0_).subs(a0, a0_tilde).subs(t,0)
rv_const_speed_tilde
```

```python
rv_tilde_
```

In conclusion, we can predict the behavior of an agent that compensates an inconsistent $\vvaref$. It will run to the corresponding starting position $\azeroincon$ as fast as possible. It also shows that setting $\vvaref$ to an arbitrary value such as suggested by (Marken, 2001) can lead to undesired behavior.

Our simulation experiments show that this behavior also occurs when imposing agent velocity and acceleration constraints (see below).

### Proof of Lemma 4.1.4 (Using Observed Value of $\dvva$)

Instead of setting $\vvaref$ to an arbitrary value, how much is the agent off the consistent $\vvarefcon$ if it observes $\dvva$ before starting to run and then uses this quantity as an estimate of $\vvarefcon$?

Let us assume the agent can perfectly observe $\vvarefincon = \dvva$ after 1% of the trajectory. This would compute as follows:

```python
D = sp.symbols("D_0")
pc = sp.symbols("\delta")
#pc = 0.01
#pc = 0.025
T_pc = sympy_sqrt_fix(pc*T.subs(yb0,0)) # t=1% of trajectory

# simplify: assume yb0=0
eq_yb0 = sympy_sqrt_fix(eq.subs(yb0,0))

# real rv at impact point
#rv_expr = sp.solve(eq_yb0.subs(a, sympy_sqrt_fix(R.subs(yb0,0))), rv)[0]
# rv observed at a0 after 1% of trajectory

#rv_expr_pc = sp.simplify(sp.solve(eq_yb0.subs(a, a0).subs(t, T_pc), rv)[0])
rv_expr_pc = sp.simplify(sp.solve(eq_yb0.subs(a, zero_yb0(R)+D).subs(t, T_pc), rv)[0])

##print (sp.latex(rv_expr_pc))
rv_expr_pc
```

$ = \vvarefincon$

($D$ denotes the agent's distance to the impact point)

Let's compute $\tilde{a}_0$, i.e. the position the agent implicitly assumes to be at when using this rv:

```python
# old a0
a0_expr= sp.solve(rv_opt - rv, a0)[0]

# a0_tilde: a0 when using rv_tilde (=rv_expr_pc) instead of rv
a0_tilde_expr = sp.simplify(a0_expr.subs(rv, rv_expr_pc))
##print (sp.latex(a0_tilde_expr))
a0_tilde_expr
```

$= \azeroincon$.

Now let's look at the difference $\azeroincon - \azero$, i.e. how far the position that the agent implicitly assumes to be at is from the real $\azero$:

```python
a0_diff =  sp.simplify(a0_tilde_expr - a0_expr)
a0_diff =  sp.simplify(a0_diff.subs(rv, rv_opt))

a0_diff
```

$= \azeroincon - \azero$.

Let's substitute $a_0$ by the known position of the agent, namely $R+D$:

```python
a0_diff_D = sp.simplify(a0_diff.subs(a0, zero_yb0(R) + D))
a0_diff_D

```

We can further simplify that:

```python
a0_diff_D = a0_diff_D.subs(sp.sin(2.0*theta), sp.sin(2*theta)) # some sympy bug?
a0_diff_D

a0_diff_D2 = a0_diff_D
a0_diff_D = pc*D / (1.0-pc)

assert (sp.simplify(a0_diff_D - a0_diff_D2) == 0)

a0_diff_D
```

$= \azeroincon - \azero$.

This means that the offset that the agent will try to compensate by running does not depend on the initial angle or velocity of the ball. It only depends on how far the agent is from the impact point and how long it waits.

#### Figure 4.3

```python
#pc_ = np.linspace(0,1, 50)
pc_ = 0.01
D_ = np.linspace(-50, 50, 5)

matplotlib.rcParams['font.size'] = 24
plt.figure(figsize=(10,6))
plt.plot( D_, [ a0_diff_D.subs(pc, 0.01).subs(D, D__).evalf() for D__ in D_], lw=3.0, label="$\delta=1\%$" )
plt.plot( D_, [ a0_diff_D.subs(pc, 0.05).subs(D, D__).evalf() for D__ in D_], lw=3.0, label="$\delta=5\%$"  )
plt.plot( D_, [ a0_diff_D.subs(pc, 0.1).subs(D, D__).evalf() for D__ in D_], lw=3.0, label="$\delta=10\%$"  )
plt.xlabel("$D_0$")
plt.ylabel(r"${a_0^{\sim}} - a_0$")
plt.xlim([-50,50])
plt.title("Consistent vs. inconsistent initial agent position")
plt.legend(loc=2, fontsize=24)

plt.tight_layout()
plt.savefig("tilde_a0_error.pdf")
```

Let us look at how this value scales with $\nu$ and $d = R-a_0$ with fixed $\varphi=\frac{\pi}{4}$:

```python
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

#%pylab tk
#%pylab inline

fig = plt.figure(figsize=(15,10))
ax = fig.add_subplot(111, projection='3d')

X = np.linspace(-50, 50, 20)  # distance d
Y = np.linspace(5, 50, 20)    # ball velocity V

t_ = pc
#theta_ = np.pi/4
theta_ = 3*np.pi/8
yb0_ = 0.0
pc_ = 0.01

DIFF = np.zeros( (len(X), len(Y)) )
ZERO = np.zeros( (len(X), len(Y)) )

for i,d_ in enumerate(X):
    for j,V_ in enumerate(Y):

        R_ = concrete_case(R, V_, theta_, yb0_)
        T_ = concrete_case(T, V_, theta_, yb0_)
#        cc = concrete_case(rv_diff,  V_, theta_, yb0_).subs(t, t_)
#        cc = concrete_case(a0_diff,  V_, theta_, yb0_).subs(t, t_)
        cc = concrete_case(a0_diff_D,  V_, theta_, yb0_).subs(t, t_)
        cc = cc.subs(pc, pc_)

        #DIFF[i,j] = float(cc.subs(dist0,d_).subs(t,t_))
#         if np.abs(d_) >= R_:
#             DIFF[i,j] = 1. #np.nan
        if True:
            #val = cc.subs(a0,R_+d_).evalf()
            val = cc.subs(D,d_).evalf()
            #DIFF[i,j] = np.abs(float(val)) if val != sp.zoo else 0.
            DIFF[i,j] = float(val) if val != sp.zoo else 0.
            # clamp diff
            #DIFF[i,j] = DIFF[i,j] if DIFF[i,j] <= 1 else 1
DIFF = DIFF.T

X,Y = np.meshgrid(X,Y)

# clamp diff

p = ax.plot_surface(X,Y, DIFF, rstride=1, cstride=1, cmap=plt.cm.coolwarm, linewidth=0.3, antialiased=False, alpha=0.9)
#cb = fig.colorbar(p, shrink=0.5)
ax.set_xlabel("$D_0$")
ax.set_ylabel(r"$\nu$")
ax.set_zlabel(r"$\tilde{a}_0 - a_0$")
#ax.set_zlim3d(0, 1)
ax.set_zlim3d(-0.5, 0.5)

p = ax.plot_surface(X,Y, ZERO, rstride=1, cstride=1, cmap=plt.cm.coolwarm, antialiased=False,
                    alpha=0.25, linewidth=0.0)

Z = DIFF
#if np.min(Z) != np.max(Z):
#  cset = ax.contour(X, Y, Z, zdir='z', offset=np.min(Z), cmap=cm.coolwarm)
#if np.min(X) != np.max(X):
if True:
  #cset = ax.contour(X, Y, Z, zdir='x', offset=np.min(X), cmap=cm.coolwarm)
  #cset = ax.contour(X, Y, Z, zdir='x', offset=np.max(X), cmap=cm.coolwarm) # project to opposite side
#if np.min(Y) != np.max(Y):
  #cset = ax.contour(X, Y, Z, zdir='y', offset=np.min(Y), cmap=cm.coolwarm)
  cset = ax.contour(X, Y, Z, zdir='y', offset=np.max(Y), cmap=cm.coolwarm) # project to opposite side

cb = fig.colorbar(p, shrink=0.75, aspect=20)

#ax.view_init(elev=15., azim=45.)
ax.view_init(elev=10., azim=-45.)
#ax.view_init(elev=1., azim=1.)

ax.xaxis._axinfo['label']['space_factor'] = 2.5
ax.yaxis._axinfo['label']['space_factor'] = 2.5
ax.zaxis._axinfo['label']['space_factor'] = 2.0

plt.tight_layout()

#plt.savefig("a0_tilde_diff.pdf" % pi_slug)
#plt.show()

```

## Understanding the McBeath's Illustration of Chapman's Strategy

This is a brief explanation how to explain the "elevator" picture from (McBeath et al. 1995)

It is easy to see that the length of the segment $s_t$ computes as

$$s_t = (R+D_0) \theta(t), $$

assuming that the agent moves towards the impact point with constant velocity $a_x(t) = -\frac{a_0 - R}{T}$

```python
# Optimal agent motion (constant towards goal)
a_opt = sp.simplify(R + t/T * D)
a_opt
```

```python
ybt = zero_yb0(yb)

theta_aopt = tan_alpha_eq.subs(a, a_opt).simplify()
theta_aopt
#sp.sqrt((D**2)/4 + (R * theta))
```

```python
dist = a_opt - xb
dist
```

```python
dist2 = ((R+D*t/T) - (R*t/T)).simplify()
# sanity check
dist - dist2 == 0
```

    True

```python
s = ((R+D)*theta_aopt).simplify()
s
```

```python
s.subs(t, T/2).simplify()
```

```python

```


```python
from utils.eom import *
from sympy import symbols, factor
from sympy import simplify
from sympy.physics.mechanics import *
from sympy import sin, cos, symbols, Matrix, solve
from sympy.physics.vector import init_vprinting
import pylab as pl
import control
import numpy as np
from scipy.integrate import odeint

from matplotlib import pyplot as plt
import matplotlib.patches as patches
from matplotlib.animation import FuncAnimation
from IPython.display import HTML

import rospy
from std_msgs.msg import Float64
from gazebo_msgs.srv import *
from geometry_msgs.msg import *
import sys, select, os
import roslib

roslib.load_manifest('dual_gazebo')
```

```python
import pickle
with open("save_parameter/sys0.txt",'rb') as inf:
    sys0 = pickle.load(inf)

# Define Symbolic Variables
x,x_l,theta = dynamicsymbols('x,x_l,theta')
phi = dynamicsymbols('phi')
F,F_l = dynamicsymbols('F,F_l')
r,h_c,h_t = symbols('r,h_c,h_t')
I_w,I_c,I_t = symbols('I_w,I_c,I_t')
m_w, m_c, m_t, g, t = symbols('m_w, m_c, m_t, g, t')

# Newtonian Reference Frames
N = ReferenceFrame('N')
No = Point('No')
No.set_vel(N, 0)

# Wheel Center Point
Wo = No.locatenew('Wo', x*N.x + r*N.z)
Wo.set_vel(N, Wo.pos_from(No).diff(t, N))

# Pendulum
P = N.orientnew('P', 'Axis', [theta, N.y])
Po = Wo.locatenew('Po', h_c*P.z)
Po.set_vel(P, 0)
J_pend = inertia(P, 0, I_c, 0)
Pend = RigidBody('Pend', Po, P, m_c, (J_pend, Po))

# Torso
T = P.orientnew('T', 'Axis', [0, P.y])
To = Wo.locatenew('To', x_l*P.x + h_t*P.z)
To.set_vel(T, 0)
J_torso = inertia(T, 0, I_t, 0)
Torso = RigidBody('Torso', To, T, m_t, (J_torso, To))

# Wheel
W = P.orientnew('W', 'Axis', [phi, P.y])
Wo.set_vel(W, 0)
J_wheel = inertia(W, 0, I_w, 0)
Wheel = RigidBody('Wheel', Wo, W, m_w, (J_wheel, Wo))

Wn = Wo.locatenew('Wn', -r*N.z)
Wn.v2pt_theory(Wo, N, W)

constraints = Wn.vel(N).express(N).args[0][0]

con = solve(constraints, [phi.diff()])
con_rhs = Matrix(list(con.values()))
con_lhs = Matrix(list(con.keys()))

# Generalized coordinates
q = Matrix([[x], [x_l], [theta]])
qd = q.diff()
qdd = qd.diff()

flist = [(Wo, -m_w*g*N.z),
         (Po, -m_c*g*N.z),
         (To, -m_t*g*N.z),
         (Wo, F*N.x),
         (To, F_l*T.x),
         (P, -F_l*h_t*P.y)]

Lag = Lagrangian(N, Pend, Torso, Wheel)
nonslip_condition = {con_lhs[0]:con_rhs[0]}
Lag_constrainted = msubs(Lag, nonslip_condition)
Le = LagrangesMethod(Lag_constrainted, q, forcelist=flist, frame=N)
eoms = Le.form_lagranges_equations()

eoms_simple = simplify(eoms) ## save point

inv_dyn = get_Simplified_EoM(eoms_simple, q)

linearlize_eq = {sin(theta):theta, cos(theta):1, theta.diff():0, x_l:0}
inv_dyn_linear = msubs(inv_dyn, linearlize_eq)

# Control Input Variable
u = Matrix([[F], [F_l]])

# Inverse Dynamics Equation
# M(q)*qdd + C(q,qd) + G(q) = W*u
M, C, G, W = get_EoM_from_T(inv_dyn,qdd,g,u)

Ml, Cl, Gl, Wl = get_EoM_from_T(inv_dyn_linear,qdd,g,u)  ## save point

# Physical Parameters
#param = {r:0.25, h_c:0.25, h_t:0.25, m_w:30, m_c:370, m_t:300, g:9.8}
param = {r:0.25, h_c:0.25, h_t:0.5, m_w:60, m_c:340, m_t:300, g:9.8}

param['c_width'] = 0.7 #0.5
param['c_height'] = 0.2 #0.25

# torso size
param['t_width'] = 2.5
param['t_height'] = 0.5

# Moment of Inertia
param[I_w] = 1/2*param[m_w]*param[r]**2
param[I_c] = 1/12*param[m_c]*(param['c_width']**2 + param['c_height']**2)
param[I_t] = 1/12*param[m_t]*(param['t_width']**2 + param['t_height']**2)

Mlp = msubs(Ml, param)
Clp = msubs(Cl, param)
Glp = msubs(Gl, param)
Wlp = msubs(Wl, param)

Mlp_inv = simplify(Mlp.inv())
qdd_rhs_A = simplify(Mlp_inv*(-Clp -Glp))
qdd_rhs_B = simplify(Mlp_inv*Wlp*u)

Mp = msubs(M, param)
Cp = msubs(C, param)
Gp = msubs(G, param)
Wp = msubs(W, param)

Mp_inv = (Mp.inv())
qdd_rhs_A_nonL = (Mp_inv*(-Cp -Gp))
qdd_rhs_B_nonL = (Mp_inv*Wp*u)

sys0_output = sys0[3,0]
tf_20 = tf_clean(control.minreal(control.ss2tf(sys0_output)))

```

```python
#Q = pl.eye(sys0.A.shape[0])
#R = pl.eye(sys0.B.shape[1])*0.00001

# state : [x, x_l, theta, xdot, x_ldot, thetadot]
Q = Matrix([ [1,0,0,0,0,0],
             [0,5,0,0,0,0],
             [0,0,1,0,0,0],
             [0,0,0,1,0,0],
             [0,0,0,0,1,0],
             [0,0,0,0,0,1] ])
R = Matrix([ [0.0000001,0],
             [0,0.00001] ])

K, S, E = control.lqr(sys0.A, sys0.B, Q, R)
sysc = sys0.feedback(K)
```

```python
x0 = [0, 0, 0.1, 0, 0, 0]
u = 0
dt = 0.01
tf = 10

t, y = control.forced_response(sysc, X0=x0, T=pl.linspace(0,tf), transpose=True)

vmax_ = 22/3.6
t_ = 20# sec

target_pos = vmax_*t_
v = vmax_/target_pos
a = v/4

t_s, traj_s = Trapezoidal_Traj_Gen_Given_Amax_and_T(a,t_,0.01)

x_des = traj_s[:,0]*target_pos
xdot_des = traj_s[:,1]*target_pos
xl_des = traj_s[:,2]*target_pos/4 # using acceleration as xl_des
zeros = np.zeros(len(traj_s))
Xdes = x_des
Xdes = np.vstack((Xdes, xl_des))
#Xdes = np.vstack((Xdes, zeros))
Xdes = np.vstack((Xdes, zeros))
Xdes = np.vstack((Xdes, xdot_des))
Xdes = np.vstack((Xdes, zeros))
Xdes = np.vstack((Xdes, zeros))

ss = sys0

rad2deg = 180/np.pi

def simulate_model_closed(X0, Xdes, K_gain, time_array, dt):
    Aop = ss.A
    Bop = ss.B

    t = 0
    j = 0
    X = Xref = Xd_prev = Xd = X0

    t_save = [0]
    x_save = xref_save = np.array([0,0,0,0,0,0])
    u_save = np.array([0,0])

    for i in range(len(time_array)):
        t = time_array[i]

        if t<2:
            Xref = X0
        elif t>=2 and j<(Xdes.shape[1]):
            Xref = Xdes[:,j]
            j+=1
        else:
            Xdes_final = Xdes[:,Xdes.shape[1]-1]
            Xdes_final[1] = 0 # force to set xl_des as 0
            Xref = Xdes_final

        # full-state feedback
        #u = K@(Xgoal - X)

        # partial feedback
        u1 = K_gain[0][1:]@(Xref[1:] - X[1:])
        u2 = K_gain[1][1:]@(Xref[1:] - X[1:])

        # Forward Dynamics
        Xd_prev = Xd
        # Linear Model
        #u = [u1, u2]
        #Xd = Aop@X + Bop@u # Xd = [xd, x_ld, thetad, xdd, x_ldd, thetadd]
        # NonLinear Model
        q_qd = {x:X[0], x_l:X[1], theta:X[2], x.diff():X[3], x_l.diff():X[4], theta.diff():X[5]}
        q_qd[F] = u1
        q_qd[F_l] = u2
        qdd = msubs(qdd_rhs_A_nonL,q_qd) + msubs(qdd_rhs_B_nonL,q_qd)
        Xd = np.array([X[3], X[4], X[5], float(qdd[0]), float(qdd[1]), float(qdd[2])])

        t_save = np.vstack((t_save, t))
        x_save = np.vstack((x_save, X))
        xref_save = np.vstack((xref_save, Xref))
        u_save = np.vstack((u_save, np.array([u1,u2])))

        X = X + Xd*dt
        t = t + dt
        i+=1

        #limit setting
        xl_limit = 0.5
        if X[1] >= xl_limit:
            X[1] = xl_limit
        elif X[1] <= -xl_limit:
            X[1] = -xl_limit

    return t_save, x_save, xref_save, u_save

# initial condition
# [x, x_l, theta, x_dot,x_l_dot, theta_dot]
X0 = np.array([0,0,0,0,0,0])

tf = 20 + 7
dt = 0.01
N = int(tf/dt)

# time points
t = np.linspace(0,tf,N)

# simulation
t_sim, x_sim, xref_sim, u_sim = simulate_model_closed(X0, Xdes, K, t, dt)

```

```python

```

```python

```

```python

```

```python

```


# 1일차 Quiz

 류회성(Hoesung Ryu)

 (skainof23@gmail.com)

> 간단하게 Python, NumPy 그리고 Pandas에 대한 간단한 퀴즈를 보겠습니다.

Table of Contents

1   Python

2   NumPy

3   Pandas

4  Reference

```python
import os
import sys
import warnings
warnings.filterwarnings(action='ignore')
```

---

 Python

**Q1.** You are given a list $x=[1,2,3,4,5,6]$.

Print the third items in list x

```python
x = [1,2,3,4,5,6]
```

```python
x[2]
```

    3

**Q2.** Print the reversed order of list $x=[1,2,3,4,5,6]$

```python
x[::-1]
```

    [6, 5, 4, 3, 2, 1]

**Q3.** Given a list iterate it and display numbers which are divisible by $5$

- List = [12, 15, 32, 42, 55, 75, 122, 132]

```python
list_ = [12, 15, 32, 42, 55, 75, 122, 132, 150, 180, 200]
for item in list_:
    if(item % 5 == 0):
        print(item)
```

    15
    55
    75
    150
    180
    200

**Q4.** Print the following pattern:
```
    1
    1 2
    1 2 3
    1 2 3 4
    1 2 3 4 5
```

```python
lastNumber = 6
for row in range(1, lastNumber):
    for column in range(1, row + 1):
        print(column, end=' ')
    print("")
```

    1
    1 2
    1 2 3
    1 2 3 4
    1 2 3 4 5

**Q5.** Print the following pattern:

```
    5 4 3 2 1
    4 3 2 1
    3 2 1
    2 1
    1

```

```python
n = 5
k = 5
for i in range(0,n+1):
    for j in range(k-i,0,-1):
        print(j,end=' ')
    print()
```

    5 4 3 2 1
    4 3 2 1
    3 2 1
    2 1
    1

**Q6.** The Fibonacci sequence is defined as below:
$$F_n = F_{n-1} + F_{n-2}$$

where $n$ denotes the $n^\text{th}$ item of the Fibonacci sequence. You are given the first three numbers of the Fibonacci sequence as F = [0, 1, 1]. Create a for loop to determine the next 20 numbers of the Fibonacci sequence. Print F with the final 23 numbers.

_Hint: use F.append() to add a new Fibonacci value to the end of the list F._

```python
F = [0, 1, 1]
for n in range(3, 23):
    f_n = F[n - 1] + F[n - 2]
    F.append(f_n)
print(F)
```

    [0, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 233, 377, 610, 987, 1597, 2584, 4181, 6765, 10946, 17711]

**Q7.** Given the list $x = [2.0,3.0,5.0,7.0,9.0]$, create a list $Y(x)$ for each float in x. Print the list $Y$.
$$Y(x) = \frac{(3.0x)^2}{(99x - x^3)} - \frac{1}{x}$$

```python
# Q7
x = [2.0, 3.0, 5.0, 7.0, 9.0]
Y = []
for v in x:
    new_val = (3.0 * v) ** 2 / (99 * v - v ** 3) - 1 / v
    Y.append(new_val)

# one-liner with list-comprehension:
Y = [(3.0 * v) ** 2 / (99 * v - v ** 3) - 1 / v
     for v in x]

print(Y)
```

    [-0.31052631578947365, -0.033333333333333326, 0.4081081081081081, 1.1171428571428572, 4.388888888888889]

---

 NumPy

```python
import numpy as np
```

**Q1.** Create a null vector of size 10 (★☆☆)

```python
Z = np.zeros(10)
print(Z)
```

    [0. 0. 0. 0. 0. 0. 0. 0. 0. 0.]

**Q2.** Reverse the below list(first element becomes last) (★☆☆):

```python
x = [1,2,3,4,5]
```

```python
X = [1,2,3,4,5]
```

```python
X[::-1]
```

    [5, 4, 3, 2, 1]

**Q3.** Create a vector of size 10 with values ranging from 0 to 1, both excluded (★★☆)
(hint: np.linspace)

```python
Z = np.linspace(0,1,11,endpoint=False)[1:]
print(Z)
```

    [0.09090909 0.18181818 0.27272727 0.36363636 0.45454545 0.54545455
     0.63636364 0.72727273 0.81818182 0.90909091]

**Q4.** Create a random vector of size 10 and sort it (★★☆)

```python
Z = np.random.random(10)
Z.sort()
print(Z)
```

    [0.02889321 0.39602243 0.40008064 0.48197894 0.52795913 0.55275259
     0.75936614 0.82002682 0.91805959 0.93233571]

**Q5.** Create a 10x10 array with random values and find the minimum and maximum values (★☆☆)

```python
Z = np.random.random((10,10))
Zmin, Zmax = Z.min(), Z.max()
print(Zmin, Zmax)
```

    0.006461933927761954 0.9744015893640688

**Q6.** Create a 3x3x3 array with random values (★☆☆)

```python
Z = np.random.random((3,3,3))
print(Z)
```

    [[[0.6896078  0.59597608 0.43508025]
      [0.83440615 0.03484723 0.15020937]
      [0.60759487 0.83454451 0.72601657]]

     [[0.64333068 0.62188361 0.5527411 ]
      [0.37278046 0.30690588 0.80008019]
      [0.51429488 0.71915612 0.99470295]]

     [[0.20987646 0.12009793 0.39928661]
      [0.46249193 0.27473153 0.7594266 ]
      [0.16051055 0.95553955 0.72917551]]]

**Q7.** solve the equation $F=M \times a $  for a (hint: np.linalg.solve)

```python
M = np.array([[2,3],[-2,9]])
F = np.array([12.9,12.3])
```

```python
M = np.array([[2,3],[-2,9]])
F = np.array([12.9,12.3])
```

```python
a = np.linalg.solve(M,F)
print(a)
```

    [3.3 2.1]

**Q8.** dot product(matrix multiplication)

prodcut those two values t1, t2
```python
t1 = np.array([[1,2],[2,3]])
t2 = np.array([[3,5],[4,6]])
```

```python
t1 = np.array([[1,2],[2,3]])
t2 = np.array([[3,5],[4,6]])
```

```python
t1 @ t2
```

    array([[11, 17],
           [18, 28]])

**Q9.** Consider the following four $x,y$ data points.

|$x$ |$y$|
|--|--|
|0.0 | 1.1 |
|0.7 | 2.99|
|1.7 | 5.69|
|2.1 |6.77 |

where an equation of a line is defined as

$$y = mx + b$$

The matrix equation for fitting a line is defined as
\begin{equation}
\begin{bmatrix}
x_1 & 1.0 \\
x_2 & 1.0 \\
\vdots & \vdots \\
x_n & 1.0 \\
\end{bmatrix}\begin{bmatrix}
m  \\
b
\end{bmatrix}
= \begin{bmatrix}
y_1 \\
y_2 \\
\vdots \\
y_n \\
\end{bmatrix}
\end{equation}

where the first data points are $x_1$ and $y_1$. The second data points are $x_2$ and $y_2$, and so forth.

```python
x = np.array([0.0, 0.7, 1.7, 2.1])
y = np.array([1.1, 2.99, 5.69, 6.77])
A = np.vstack([x, np.ones(len(x))]).T
# or
A = np.array([x, np.ones(len(x))]).T
c, residuals, rank, sing = np.linalg.lstsq(A, y)
print(c)
print(residuals)
print(rank)
```

    [2.7 1.1]
    [1.42981039e-30]
    2

---

 Pandas

**Q1.** Import pandas under the alias `pd`.

```python
import pandas as pd
```

**Q2.** Print the version of pandas that has been imported.

```python
pd.__version__
```

    '1.0.3'

**Q3.** Print out all the version information of the libraries that are required by the pandas library.

```python
pd.show_versions()
```

    INSTALLED VERSIONS
    ------------------
    commit           : None
    python           : 3.6.10.final.0
    python-bits      : 64
    OS               : Windows
    OS-release       : 10
    machine          : AMD64
    processor        : AMD64 Family 23 Model 1 Stepping 1, AuthenticAMD
    byteorder        : little
    LC_ALL           : None
    LANG             : None
    LOCALE           : None.None

    pandas           : 1.0.3
    numpy            : 1.16.6
    pytz             : 2020.1
    dateutil         : 2.8.1
    pip              : 20.0.2
    setuptools       : 47.1.1.post20200604
    Cython           : 0.29.21
    pytest           : None
    hypothesis       : None
    sphinx           : None
    blosc            : None
    feather          : None
    xlsxwriter       : None
    lxml.etree       : 4.5.1
    html5lib         : None
    pymysql          : None
    psycopg2         : None
    jinja2           : 2.11.2
    IPython          : 7.13.0
    pandas_datareader: None
    bs4              : 4.6.0
    bottleneck       : None
    fastparquet      : None
    gcsfs            : None
    lxml.etree       : 4.5.1
    matplotlib       : 3.1.3
    numexpr          : 2.7.1
    odfpy            : None
    openpyxl         : None
    pandas_gbq       : None
    pyarrow          : None
    pytables         : None
    pytest           : None
    pyxlsb           : None
    s3fs             : None
    scipy            : 1.4.1
    sqlalchemy       : None
    tables           : None
    tabulate         : None
    xarray           : None
    xlrd             : 1.2.0
    xlwt             : None
    xlsxwriter       : None
    numba            : None

Note: remember to import numpy using:
```python
import numpy as np
```

Consider the following Python dictionary `data` and Python list `labels`:

``` python
data = {'animal': ['cat', 'cat', 'snake', 'dog', 'dog', 'cat', 'snake', 'cat', 'dog', 'dog'],
        'age': [2.5, 3, 0.5, np.nan, 5, 2, 4.5, np.nan, 7, 3],
        'visits': [1, 3, 2, 3, 2, 3, 1, 1, 2, 1],
        'priority': ['yes', 'yes', 'no', 'yes', 'no', 'no', 'no', 'yes', 'no', 'no']}

labels = ['a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j']
```
(This is just some meaningless data I made up with the theme of animals and trips to a vet.)

**Q4.** Create a DataFrame `df` from this dictionary `data` which has the index `labels`.

```python
import numpy as np

data = {'animal': ['cat', 'cat', 'snake', 'dog', 'dog', 'cat', 'snake', 'cat', 'dog', 'dog'],
        'age': [2.5, 3, 0.5, np.nan, 5, 2, 4.5, np.nan, 7, 3],
        'visits': [1, 3, 2, 3, 2, 3, 1, 1, 2, 1],
        'priority': ['yes', 'yes', 'no', 'yes', 'no', 'no', 'no', 'yes', 'no', 'no']}

labels = ['a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j']

df = pd.DataFrame(data, index=labels)
```

**Q5.** Display a summary of the basic information about this DataFrame and its data (*hint: there is a single method that can be called on the DataFrame*).

```python
df.info()

# ...or...

df.describe()
```

    <class 'pandas.core.frame.DataFrame'>
    Index: 10 entries, a to j
    Data columns (total 4 columns):
     #   Column    Non-Null Count  Dtype
    ---  ------    --------------  -----
     0   animal    10 non-null     object
     1   age       8 non-null      float64
     2   visits    10 non-null     int64
     3   priority  10 non-null     object
    dtypes: float64(1), int64(1), object(2)
    memory usage: 400.0+ bytes

      age
      visits

      count
      8.000000
      10.000000

      mean
      3.437500
      1.900000

      std
      2.007797
      0.875595

      min
      0.500000
      1.000000

      25%
      2.375000
      1.000000

      50%
      3.000000
      2.000000

      75%
      4.625000
      2.750000

      max
      7.000000
      3.000000

**Q6.** Return the first 3 rows of the DataFrame `df`.

```python
df.iloc[:3]

# or equivalently

df.head(3)
```

      animal
      age
      visits
      priority

      a
      cat
      2.5
      1
      yes

      b
      cat
      3.0
      3
      yes

      c
      snake
      0.5
      2
      no

**Q7.** Select just the 'animal' and 'age' columns from the DataFrame `df`.

```python
df.loc[:, ['animal', 'age']]

# or

df[['animal', 'age']]
```

      animal
      age

      a
      cat
      2.5

      b
      cat
      3.0

      c
      snake
      0.5

      d
      dog
      NaN

      e
      dog
      5.0

      f
      cat
      2.0

      g
      snake
      4.5

      h
      cat
      NaN

      i
      dog
      7.0

      j
      dog
      3.0

**Q8.** Select the data in rows `[3, 4, 8]` *and* in columns `['animal', 'age']`.

```python
df.loc[df.index[[3, 4, 8]], ['animal', 'age']]
```

      animal
      age

      d
      dog
      NaN

      e
      dog
      5.0

      i
      dog
      7.0

**Q9.** Select only the rows where the number of visits is greater than 3.

```python
df[df['visits'] > 3]
```

      animal
      age
      visits
      priority

**Q10.** Select the rows where the age is missing, i.e. it is `NaN`.

```python
df[df['age'].isnull()]
```

      animal
      age
      visits
      priority

      d
      dog
      NaN
      3
      yes

      h
      cat
      NaN
      1
      yes

**Q11.** Select the rows where the animal is a cat *and* the age is less than 3.

```python
df[(df['animal'] == 'cat') & (df['age'] < 3)]
```

      animal
      age
      visits
      priority

      a
      cat
      2.5
      1
      yes

      f
      cat
      2.0
      3
      no

**Q12.** Select the rows the age is between 2 and 4 (inclusive).

```python
df[df['age'].between(2, 4)]
```

      animal
      age
      visits
      priority

      a
      cat
      2.5
      1
      yes

      b
      cat
      3.0
      3
      yes

      f
      cat
      2.0
      3
      no

      j
      dog
      3.0
      1
      no

**Q13.** Change the age in row 'f' to 1.5.

```python
df.loc['f', 'age'] = 1.5
df
```

      animal
      age
      visits
      priority

      a
      cat
      2.5
      1
      yes

      b
      cat
      3.0
      3
      yes

      c
      snake
      0.5
      2
      no

      d
      dog
      NaN
      3
      yes

      e
      dog
      5.0
      2
      no

      f
      cat
      1.5
      3
      no

      g
      snake
      4.5
      1
      no

      h
      cat
      NaN
      1
      yes

      i
      dog
      7.0
      2
      no

      j
      dog
      3.0
      1
      no

**Q14.** Calculate the sum of all visits in `df` (i.e. the total number of visits).

```python
df['visits'].sum()
```

    19

**Q15.** Calculate the mean age for each different animal in `df`.

```python
df.groupby('animal')['age'].mean()
```

    animal
    cat      2.333333
    dog      5.000000
    snake    2.500000
    Name: age, dtype: float64

**Q16.** Append a new row 'k' to `df` with your choice of values for each column. Then delete that row to return the original DataFrame.

```python
df.loc['k'] = [5.5, 'dog', 'no', 2]

# and then deleting the new row...

df = df.drop('k')
df
```

      animal
      age
      visits
      priority

      a
      cat
      2.5
      1
      yes

      b
      cat
      3
      3
      yes

      c
      snake
      0.5
      2
      no

      d
      dog
      NaN
      3
      yes

      e
      dog
      5
      2
      no

      f
      cat
      1.5
      3
      no

      g
      snake
      4.5
      1
      no

      h
      cat
      NaN
      1
      yes

      i
      dog
      7
      2
      no

      j
      dog
      3
      1
      no

**Q17.** Count the number of each type of animal in `df`.

```python
df['animal'].value_counts()
```

    dog      4
    cat      4
    snake    2
    Name: animal, dtype: int64

**Q18.** Sort `df` first by the values in the 'age' in *decending* order, then by the value in the 'visit' column in *ascending* order (so row `i` should be first, and row `d` should be last).

```python
df.sort_values(by=['age', 'visits'], ascending=[False, True])
```

      animal
      age
      visits
      priority

      i
      dog
      7
      2
      no

      e
      dog
      5
      2
      no

      g
      snake
      4.5
      1
      no

      j
      dog
      3
      1
      no

      b
      cat
      3
      3
      yes

      a
      cat
      2.5
      1
      yes

      f
      cat
      1.5
      3
      no

      c
      snake
      0.5
      2
      no

      h
      cat
      NaN
      1
      yes

      d
      dog
      NaN
      3
      yes

**Q19.** The 'priority' column contains the values 'yes' and 'no'. Replace this column with a column of boolean values: 'yes' should be `True` and 'no' should be `False`.

```python
df['priority'] = df['priority'].map({'yes': True, 'no': False})
df
```

      animal
      age
      visits
      priority

      a
      cat
      2.5
      1
      True

      b
      cat
      3
      3
      True

      c
      snake
      0.5
      2
      False

      d
      dog
      NaN
      3
      True

      e
      dog
      5
      2
      False

      f
      cat
      1.5
      3
      False

      g
      snake
      4.5
      1
      False

      h
      cat
      NaN
      1
      True

      i
      dog
      7
      2
      False

      j
      dog
      3
      1
      False

**Q20.** In the 'animal' column, change the 'snake' entries to 'python'.

```python
df['animal'] = df['animal'].replace('snake', 'python')
```

```python
df
```

      animal
      age
      visits
      priority

      a
      cat
      2.5
      1
      True

      b
      cat
      3
      3
      True

      c
      python
      0.5
      2
      False

      d
      dog
      NaN
      3
      True

      e
      dog
      5
      2
      False

      f
      cat
      1.5
      3
      False

      g
      python
      4.5
      1
      False

      h
      cat
      NaN
      1
      True

      i
      dog
      7
      2
      False

      j
      dog
      3
      1
      False

## Reference

- https://rfriend.tistory.com/346


```
%load_ext autoreload
%autoreload 2

from __future__ import division

import numpy as np
import matplotlib.pyplot as plt

import pydy
import sympy as sym
import sympy.physics.mechanics as me

from scipy.linalg import solve, lstsq
from scipy.integrate import odeint

import time
import functools

me.Vector.simp = False  # to increase computation speed

from sympy.physics.vector import init_vprinting
init_vprinting()

from matplotlib import animation
from matplotlib.patches import Ellipse, Rectangle

from IPython.html import widgets
from IPython.html.widgets import interact, interactive

# my own imports
import nchain
```

    The autoreload extension is already loaded. To reload it, use:
      %reload_ext autoreload

```
n = 3

dynamic, symbol, setup, fbd = nchain.formulate_nchain_parameters(n)
eom = nchain.make_kane_eom(dynamic, setup, fbd)
```

```
kz1 = sym.trigsimp(eom['kanezero'][1])
```

```
kz11 = sym.signsimp(kz1)
```

```
kz11
```

```
nchain.simp(eom['mass'])
```

```
nchain.simp(eom['forcing'])
```

```
eom['mass'][[0, 2, 3, 4, 6, 7], :]
```

```
eom['forcing'][[0, 2, 3, 4, 6, 7], :]
```

```
eoml = nchain.make_lagrange_eom(dynamic, setup, fbd)
```

```
# get values for the snake
snake = nchain.equal_snake(n)
```

```
# symbols used in the simulation
constants = [symbol['g']]
for i in range(n):
    constants = constants + [symbol['m'][i], symbol['l'][i], symbol['Izz'][i]]

# acutal values for the simulation
constant_vals = [9.81]
for i in range(n):
    constant_vals = constant_vals + [snake['m'][i], snake['l'][i], snake['I'][i]]

consts = dict(names=constants, vals=np.array(constant_vals))
```

```
F_func, M_func, ffunc, mfunc = nchain.callable_matrices(dynamic, eom, consts)
```

```
from scipy.linalg import lstsq

def rhs(x, t, args):

    dx = np.array(np.linalg.solve(mfunc(*x), ffunc(*x))).flatten()
    return dx

def rhs3(x, t, args):

    x[1] = np.interp(t, ts, q1)
    x[2] = np.interp(t, ts, q2)
    x[6] = np.interp(t, ts, u1)
    x[7] = np.interp(t, ts, u2)

    dx = np.array(np.linalg.solve(mfunc(*x), ffunc(*x))).flatten()

    dx[1] = np.interp(t, ts, u1)
    dx[2] = np.interp(t, ts, u2)
    dx[6] = np.interp(t, ts, s1)
    dx[7] = np.interp(t, ts, s2)

    return dx

def rhs2(x, t, args):
    """RHS for the 2 link snake.
    """

    x[1] = np.interp(t, ts, q1)
    x[5] = np.interp(t, ts, u1)

    mm = mfunc(*x)[[0, 2, 3, 4, 6, 7], :]
    ff = ffunc(*x)[[0, 2, 3, 4, 6, 7], :]

    soln = lstsq(mm, ff)
    soln = np.array(soln[0]).flatten()

    y1 = np.interp(t, ts, u1)
    y5 = np.interp(t, ts, s1)

    dx = np.zeros(x.shape)
    dx[1] = y1
    dx[5] = y5
    dx[[0, 2, 3, 4, 6, 7]] = soln

    return dx

def rhs3(x, t, args):
    """RHS for the 3 link snake.
    """

    x[1] = np.interp(t, ts, q1)
    x[2] = np.interp(t, ts, q2)
    x[6] = np.interp(t, ts, u1)
    x[7] = np.interp(t, ts, u2)

    idx = [0, 3, 4, 5, 8, 9]
    mm = mfunc(*x)[idx, :]
    ff = ffunc(*x)[idx, :]

    soln = lstsq(mm, ff)
    soln = np.array(soln[0]).flatten()

    y = np.zeros(x.shape)
    y[1] = np.interp(t, ts, u1)
    y[2] = np.interp(t, ts, u2)
    y[6] = np.interp(t, ts, s1)
    y[7] = np.interp(t, ts, s2)
    sel = [1, 2, 6, 7]

    dx = np.zeros(x.shape)
    dx[sel] = y[1], y[2], y[6], y[7]  # y[sel]
    dx[idx] = soln

    return dx

args = ()

npts = 5000
ts, dt = linspace(0, 20, npts, retstep=True)

# initial conditions
qstart = np.zeros(n + 2)
ustart = np.zeros(n + 2)

q0_0 = np.pi / 2
q1_0 = np.pi / 4
q2_0 = np.pi / 2

f = .5

q1 = np.cos(f * ts + q1_0)
q2 = -np.sin(f * ts + q2_0)
u1 = np.gradient(q1, dt)
u2 = np.gradient(q2, dt)
s1 = np.gradient(u1, dt)
s2 = np.gradient(u2, dt)

# for 3-snake
qstart[0:3] = q0_0, q1[0], q2[0]
ustart[1:3] = u1[0], u2[0]

# for 2-snake
# qstart[0:2] = q0_0, q1[0]
# ustart[1] = u1[0]

x0 = np.hstack((qstart, ustart));
```

```
q1.min()
```

```
plt.close('all')
```

```
fig, ax = plt.subplots()

ax.plot(q1, q2)
ax.plot(u1, u2)
```

    [<matplotlib.lines.Line2D at 0x1345e1d0>]

```
fig, (ax1, ax2) = plt.subplots(2, 1, sharex=True)

ax1.plot(ts, q1)
ax1.plot(ts, u1)
ax1.plot(ts, s1)

ax2.plot(ts, q2)
ax2.plot(ts, u2)
ax2.plot(ts, s2)
```

    [<matplotlib.lines.Line2D at 0x15726c90>]

```
plt.close('all')
```

```
now = time.time()
#y = odeint(rhs2, x0, ts, args=(args,))
y = odeint(rhs3, x0, ts, args=(args,))
print('Simulation time: {0:.2f} sec'.format(time.time() - now))
```

    Simulation time: 0.37 sec

```
fig, (ax1, ax2) = plt.subplots(2, 1, sharex=True)
iq = n + 2

lines = ax1.plot(ts, y[:, :iq])
#lab = xlabel('Time [sec]')
leg = ax1.legend(dynamic['q'])

lines = ax2.plot(ts, y[:, iq:])
#lab = xlabel('Time [sec]')
leg = ax2.legend(dynamic['u'])

# fig, ax = plt.subplots()
# ax.plot(y[:, 1], y[:, 0])
```

```
def err(want, got):
    return (want - got) / got * 100
```

```
fig, ax = plt.subplots()
ax.plot(ts, 10 * err(q1, y[:, 1]))
ax.plot(ts, 10 * err(q2, y[:, 2]))
ax.plot(ts, q1, 'b')
ax.plot(ts, q2, 'g')

#ax.plot(y[:, 6], y[:, 7])
```

    [<matplotlib.lines.Line2D at 0x1609fad0>]

```
fig, ax = plt.subplots()
ax.plot(ts, err(u1, y[:, 6]))
ax.plot(ts, err(u2, y[:, 7]))
ax.plot(ts, u1, 'b')
ax.plot(ts, u2, 'g')
```

    [<matplotlib.lines.Line2D at 0x160ce850>]

```
interval = 5
limx = (-1, 1)
limy = (-1, 1)

anim = nchain.n_movie_maker(n, snake, ts, y, limx=limx, limy=limy, hst=100, interval=interval)
```

```
rads, degs, jtx, jty, mcx, mcy, comx, comy = nchain.organize_data(y, n, snake)
```

```
fig, ax = plt.subplots()
ax.plot(comx, comy)
ax.axis('equal')
```

```
fig, ax = plt.subplots()
ax.plot(y[:, 1], y[:, 0])
```

    [<matplotlib.lines.Line2D at 0x1321af10>]

```
fig, (ax1, ax2) = plt.subplots(2, 1, sharex=True, sharey=True)
ax1.plot(y[:, 3], y[:, 4])
ax2.plot(jtx[:, 0], jty[:, 0])
ax.axis('equal')
```

```
fig, ax = plt.subplots()
ax.plot(np.rad2deg(y[:, 1]), np.rad2deg(y[:, 2]))
ax.plot(np.rad2deg(q1), np.rad2deg(q2))
#ax.axis('equal')
```

```
fig, ax = plt.subplots()
ax.plot(y[:, 1], y[:, 2])
#ax.plot(q1, q2)
ax.axis('equal')
```

```
plt.close('all')
```

```
print(nchain.printer(nchain.simp(eom['kanezero'])))
```

    \left[\begin{matrix}\frac{l_{0} l_{1}}{2} m_{1} \left(u_{0} + u_{1}\right)^{2} \operatorname{sin}\left(q_{1}\right) - \frac{l_{0} l_{1}}{2} m_{1} u^{2}_{0} \operatorname{sin}\left(q_{1}\right) + l_{0} l_{1} m_{2} \left(u_{0} + u_{1}\right)^{2} \operatorname{sin}\left(q_{1}\right) - l_{0} l_{1} m_{2} u^{2}_{0} \operatorname{sin}\left(q_{1}\right) + \frac{l_{0} l_{2}}{2} m_{2} \left(u_{0} + u_{1} + u_{2}\right)^{2} \operatorname{sin}\left(q_{1} + q_{2}\right) - \frac{l_{0} l_{2}}{2} m_{2} u^{2}_{0} \operatorname{sin}\left(q_{1} + q_{2}\right) - \frac{l_{1} l_{2}}{2} m_{2} \left(u_{0} + u_{1}\right)^{2} \operatorname{sin}\left(q_{2}\right) + \frac{l_{1} l_{2}}{2} m_{2} \left(u_{0} + u_{1} + u_{2}\right)^{2} \operatorname{sin}\left(q_{2}\right) - \left(I_{zz 2} + \frac{l_{2} m_{2}}{4} \left(2 l_{0} \operatorname{cos}\left(q_{1} + q_{2}\right) + 2 l_{1} \operatorname{cos}\left(q_{2}\right) + l_{2}\right)\right) \dot{u}_{2} - \left(- \frac{l_{0} m_{0}}{2} \operatorname{sin}\left(q_{0}\right) + m_{1} \left(- l_{0} \operatorname{sin}\left(q_{0}\right) - \frac{l_{1}}{2} \operatorname{sin}\left(q_{0} + q_{1}\right)\right) + m_{2} \left(- l_{0} \operatorname{sin}\left(q_{0}\right) - l_{1} \operatorname{sin}\left(q_{0} + q_{1}\right) - \frac{l_{2}}{2} \operatorname{sin}\left(q_{0} + q_{1} + q_{2}\right)\right)\right) \dot{u}_{3} - \left(\frac{l_{0} m_{0}}{2} \operatorname{cos}\left(q_{0}\right) + m_{1} \left(l_{0} \operatorname{cos}\left(q_{0}\right) + \frac{l_{1}}{2} \operatorname{cos}\left(q_{0} + q_{1}\right)\right) + m_{2} \left(l_{0} \operatorname{cos}\left(q_{0}\right) + l_{1} \operatorname{cos}\left(q_{0} + q_{1}\right) + \frac{l_{2}}{2} \operatorname{cos}\left(q_{0} + q_{1} + q_{2}\right)\right)\right) \dot{u}_{4} - \left(I_{zz 1} + I_{zz 2} + \frac{l_{1} m_{1}}{4} \left(2 l_{0} \operatorname{cos}\left(q_{1}\right) + l_{1}\right) + m_{2} \left(l_{0} l_{1} \operatorname{cos}\left(q_{1}\right) + \frac{l_{0} l_{2}}{2} \operatorname{cos}\left(q_{1} + q_{2}\right) + l_{1}^{2} + l_{1} l_{2} \operatorname{cos}\left(q_{2}\right) + \frac{l_{2}^{2}}{4}\right)\right) \dot{u}_{1} - \left(I_{zz 0} + I_{zz 1} + I_{zz 2} + \frac{l_{0}^{2} m_{0}}{4} + m_{1} \left(l_{0}^{2} + l_{0} l_{1} \operatorname{cos}\left(q_{1}\right) + \frac{l_{1}^{2}}{4}\right) + m_{2} \left(l_{0}^{2} + 2 l_{0} l_{1} \operatorname{cos}\left(q_{1}\right) + l_{0} l_{2} \operatorname{cos}\left(q_{1} + q_{2}\right) + l_{1}^{2} + l_{1} l_{2} \operatorname{cos}\left(q_{2}\right) + \frac{l_{2}^{2}}{4}\right)\right) \dot{u}_{0}\\- \frac{l_{0} l_{1}}{2} m_{1} u^{2}_{0} \operatorname{sin}\left(q_{1}\right) - l_{0} l_{1} m_{2} u^{2}_{0} \operatorname{sin}\left(q_{1}\right) - \frac{l_{0} l_{2}}{2} m_{2} u^{2}_{0} \operatorname{sin}\left(q_{1} + q_{2}\right) - \frac{l_{1} l_{2}}{2} m_{2} \left(u_{0} + u_{1}\right)^{2} \operatorname{sin}\left(q_{2}\right) + \frac{l_{1} l_{2}}{2} m_{2} \left(u_{0} + u_{1} + u_{2}\right)^{2} \operatorname{sin}\left(q_{2}\right) - \left(I_{zz 2} + \frac{l_{2} m_{2}}{4} \left(2 l_{1} \operatorname{cos}\left(q_{2}\right) + l_{2}\right)\right) \dot{u}_{2} - \left(- \frac{l_{1} m_{1}}{2} \operatorname{sin}\left(q_{0} + q_{1}\right) + m_{2} \left(- l_{1} \operatorname{sin}\left(q_{0} + q_{1}\right) - \frac{l_{2}}{2} \operatorname{sin}\left(q_{0} + q_{1} + q_{2}\right)\right)\right) \dot{u}_{3} - \left(\frac{l_{1} m_{1}}{2} \operatorname{cos}\left(q_{0} + q_{1}\right) + m_{2} \left(l_{1} \operatorname{cos}\left(q_{0} + q_{1}\right) + \frac{l_{2}}{2} \operatorname{cos}\left(q_{0} + q_{1} + q_{2}\right)\right)\right) \dot{u}_{4} - \left(I_{zz 1} + I_{zz 2} + \frac{l_{1}^{2} m_{1}}{4} + m_{2} \left(l_{1}^{2} + l_{1} l_{2} \operatorname{cos}\left(q_{2}\right) + \frac{l_{2}^{2}}{4}\right)\right) \dot{u}_{1} - \left(I_{zz 1} + I_{zz 2} + \frac{l_{1} m_{1}}{4} \left(2 l_{0} \operatorname{cos}\left(q_{1}\right) + l_{1}\right) + m_{2} \left(l_{0} l_{1} \operatorname{cos}\left(q_{1}\right) + \frac{l_{0} l_{2}}{2} \operatorname{cos}\left(q_{1} + q_{2}\right) + l_{1}^{2} + l_{1} l_{2} \operatorname{cos}\left(q_{2}\right) + \frac{l_{2}^{2}}{4}\right)\right) \dot{u}_{0}\\- \frac{l_{0} l_{2}}{2} m_{2} u^{2}_{0} \operatorname{sin}\left(q_{1} + q_{2}\right) - \frac{l_{1} l_{2}}{2} m_{2} \left(u_{0} + u_{1}\right)^{2} \operatorname{sin}\left(q_{2}\right) + \frac{l_{2} m_{2}}{2} \operatorname{sin}\left(q_{0} + q_{1} + q_{2}\right) \dot{u}_{3} - \frac{l_{2} m_{2}}{2} \operatorname{cos}\left(q_{0} + q_{1} + q_{2}\right) \dot{u}_{4} - \left(I_{zz 2} + \frac{l_{2}^{2} m_{2}}{4}\right) \dot{u}_{2} - \left(I_{zz 2} + \frac{l_{2} m_{2}}{4} \left(2 l_{1} \operatorname{cos}\left(q_{2}\right) + l_{2}\right)\right) \dot{u}_{1} - \left(I_{zz 2} + \frac{l_{2} m_{2}}{4} \left(2 l_{0} \operatorname{cos}\left(q_{1} + q_{2}\right) + 2 l_{1} \operatorname{cos}\left(q_{2}\right) + l_{2}\right)\right) \dot{u}_{0}\\\frac{l_{0} m_{0}}{2} u^{2}_{0} \operatorname{cos}\left(q_{0}\right) + l_{0} m_{1} u^{2}_{0} \operatorname{cos}\left(q_{0}\right) + l_{0} m_{2} u^{2}_{0} \operatorname{cos}\left(q_{0}\right) + \frac{l_{1} m_{1}}{2} \left(u_{0} + u_{1}\right)^{2} \operatorname{cos}\left(q_{0} + q_{1}\right) + l_{1} m_{2} \left(u_{0} + u_{1}\right)^{2} \operatorname{cos}\left(q_{0} + q_{1}\right) + \frac{l_{2} m_{2}}{2} \left(u_{0} + u_{1} + u_{2}\right)^{2} \operatorname{cos}\left(q_{0} + q_{1} + q_{2}\right) + \frac{l_{2} m_{2}}{2} \operatorname{sin}\left(q_{0} + q_{1} + q_{2}\right) \dot{u}_{2} - \left(- \frac{l_{1} m_{1}}{2} \operatorname{sin}\left(q_{0} + q_{1}\right) + m_{2} \left(- l_{1} \operatorname{sin}\left(q_{0} + q_{1}\right) - \frac{l_{2}}{2} \operatorname{sin}\left(q_{0} + q_{1} + q_{2}\right)\right)\right) \dot{u}_{1} - \left(m_{0} + m_{1} + m_{2}\right) \dot{u}_{3} - \left(- \frac{l_{0} m_{0}}{2} \operatorname{sin}\left(q_{0}\right) + m_{1} \left(- l_{0} \operatorname{sin}\left(q_{0}\right) - \frac{l_{1}}{2} \operatorname{sin}\left(q_{0} + q_{1}\right)\right) + m_{2} \left(- l_{0} \operatorname{sin}\left(q_{0}\right) - l_{1} \operatorname{sin}\left(q_{0} + q_{1}\right) - \frac{l_{2}}{2} \operatorname{sin}\left(q_{0} + q_{1} + q_{2}\right)\right)\right) \dot{u}_{0}\\\frac{l_{0} m_{0}}{2} u^{2}_{0} \operatorname{sin}\left(q_{0}\right) + l_{0} m_{1} u^{2}_{0} \operatorname{sin}\left(q_{0}\right) + l_{0} m_{2} u^{2}_{0} \operatorname{sin}\left(q_{0}\right) + \frac{l_{1} m_{1}}{2} \left(u_{0} + u_{1}\right)^{2} \operatorname{sin}\left(q_{0} + q_{1}\right) + l_{1} m_{2} \left(u_{0} + u_{1}\right)^{2} \operatorname{sin}\left(q_{0} + q_{1}\right) + \frac{l_{2} m_{2}}{2} \left(u_{0} + u_{1} + u_{2}\right)^{2} \operatorname{sin}\left(q_{0} + q_{1} + q_{2}\right) - \frac{l_{2} m_{2}}{2} \operatorname{cos}\left(q_{0} + q_{1} + q_{2}\right) \dot{u}_{2} - \left(\frac{l_{1} m_{1}}{2} \operatorname{cos}\left(q_{0} + q_{1}\right) + m_{2} \left(l_{1} \operatorname{cos}\left(q_{0} + q_{1}\right) + \frac{l_{2}}{2} \operatorname{cos}\left(q_{0} + q_{1} + q_{2}\right)\right)\right) \dot{u}_{1} - \left(m_{0} + m_{1} + m_{2}\right) \dot{u}_{4} - \left(\frac{l_{0} m_{0}}{2} \operatorname{cos}\left(q_{0}\right) + m_{1} \left(l_{0} \operatorname{cos}\left(q_{0}\right) + \frac{l_{1}}{2} \operatorname{cos}\left(q_{0} + q_{1}\right)\right) + m_{2} \left(l_{0} \operatorname{cos}\left(q_{0}\right) + l_{1} \operatorname{cos}\left(q_{0} + q_{1}\right) + \frac{l_{2}}{2} \operatorname{cos}\left(q_{0} + q_{1} + q_{2}\right)\right)\right) \dot{u}_{0}\end{matrix}\right]

```
n
```

```

```