# Time Optimal Velocity Profiles

***

When the maze solver commands that the robot go forward, it can say that it must go forward one or more squares depending on what it knows about the maze. When we don't know what is after the square we pass through, we must be going slow enough to handle any scenario. In other words, there is some $V_f$ that we must reach by the end of our motion. We also begin motions at this speed, since between we arrived where we are we required that we reach $V_f$ to get there. Therefore, we start and end at $V_f$, and we want to cover some distance $d$ in the fast possible time. To do so, we accelerate at our fixed $a$ until we reach max speed, or until we need to start slowing down (whichever comes first). This gives us a trapezoid shaped velocity profile.

## Going Straight

```python
%load_ext tikzmagic
```

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

```python
%%tikz -s 400,400
\draw[->] (0,0) -- (10,0);
\draw[->] (0,0) -- (0,5);

\draw[line width=1] (0,0.5) -- (2.5,3);
\draw[line width=1] (2.5,3) -- (5.5,3);
\draw[line width=1] (5.5,3) -- (8,0.5);
\draw[dashed] (0,0.5) -- (10,0.5);
\draw[dashed] (0,3) -- (10,3);
\draw[dashed] (2.5,0) -- (2.5,5);
\draw[dashed] (5.5,0) -- (5.5,5);
\draw[dashed] (8,0) -- (8,5);

\draw (-0.5, 0.5) node {$V_{f}$};
\draw (-0.5, 3) node {$V_{max}$};
\draw (2.5, -0.5) node {$t_b$};
\draw (5.5, -0.5) node {$t_f-t_b$};
\draw (8, -0.5) node {$t_f$};
```

The time to accelerate from $V_f$ to $V_{max}$ is $t_b = \frac{V-V_f}{a}$. We can substitute this into newtons first equation of motion as follows.

\begin{align}
d &= Vt_b - \frac{1}{2}a{t_b}^2 \\
  &= V\Big(\frac{V-V_f}{a}\Big) - \frac{1}{2}a\Big(\frac{V-V_f}{a}\Big)^2 \\
  &= \Big(\frac{V^2-VV_f}{a}\Big) - \Big(\frac{a(V-V_f)^2}{2a^2}\Big) \\
  &= \Big(\frac{2V^2-2VV_f}{2a}\Big) - \Big(\frac{V^2-2VV_f+{V_f}^2}{2a}\Big) \\
  &= \frac{2V^2-2VV_f - V^2 + 2VV_f - {V_f}^2}{2a} \\
d &= \frac{V^2-{V_f}^2}{2a} \\
\end{align}

For example, if you're at starting at $V_f=0.2\frac{m}{s}$, and you're ramping up to $V=0.5\frac{m}{s}$, and you're acceleration is fixed at the $a=2\frac{m}{s^2}$, the distance you'll need to do that is $d = \frac{0.5 - 0.2}{2*2} = 0.075m$

## Code that proves it

```python
# dependencies and global setup
import numpy as np
import matplotlib.pyplot as plt
np.set_printoptions(suppress=True, precision=3, linewidth=100)

LOG_LVL = 2

def debug(*args):
    if LOG_LVL <= 0:
        print(*args)

def info(*args):
    if LOG_LVL <= 1:
        print(*args)

def warning(*args):
    if LOG_LVL <= 2:
        print(*args)

def log(*args):
    if LOG_LVL < 100:
        print(*args)

```

```python
def profile(V0, Vf, Vmax, d, A, buffer=3e-4):
    v = V0
    x = 0
    a = A
    vs = [v]
    xs = [x]
    a_s = [a]

    dt = 0.001
    while x < d:
        x = x + v*dt + a*dt*dt/2.0
        v = v + a*dt
        ramp_d = (v*v+ - Vf*Vf) / (2.0*A)
        if (d-x) < ramp_d + buffer:
            a = -A
        elif v < Vmax:
            a = A
        else:
            a = 0

        if v > Vmax:
            v = Vmax
        elif v < Vf:
            v = Vf

        xs.append(x)
        vs.append(v)
        a_s.append(a)

    return xs, vs, a_s

def graph(title, idx):
    plt.figure()
    plt.title(title)
    # various max speeds
    Vs = [0.35, 0.5, 0.75, 1, 2]
    Vf = 0.02
    V0 = 0.2
    d = 0.35
    a = 2
    for V in Vs:
        results  = profile(V0, Vf, V, d, a)
        vs = results[1]
        if V == 2: # make V=2 dashed so we can see it over V=1
            plt.plot(results[idx], label='V={}'.format(V), linestyle='dashed')
        else:
            plt.plot(results[idx], label='V={}'.format(V))
        plt.legend(bbox_to_anchor=(1, 1), loc=2)

graph("position", 0)
graph("velocity", 1)
graph("acceleration", 2)
plt.show()
```

## General Form Trajectory Planning

Let's start out with a generating trajectories that are not time optimal, but rely on specifying the final time $v_f$. For smartmouse, our state space is $[x, y, \theta]$, and a turn can be defined as starting at a point $[x_0, y_0, \theta_0]$ and going to $[x_f, y_f, \theta_0]$. Of course, we also want to specify the velocities at these point, $[\dot{x}_0, \dot{y}_0,\dot{\theta}_0]$ and $[\dot{x}_f, \dot{y}_f,\dot{\theta}_f]$. We have four constraints, so if we want to fit a smooth polynomial to those points we need a 4th order polynomial.

$$q(t) = a_0 + a_1t + a_2t^2 + a_3t^3$$
$$\dot{q}(t) = a_1 + 2a_2t + 3a_3t^2$$

If we sub in our constraints, we get the following system of equations.

\begin{align}
q(0) &= a_0 \\
\dot{q}(0) &= a_1 \\
q(t_f) &= a_0 + a_1t_f + a_2{t_f}^2 + a_3{t_f}^3\\
\dot{q}(t_f) &= a_1 + 2a_2t_f + 3a_3{t_f}^2\\
\end{align}

In matrix form that looks like:
\begin{equation}
\begin{bmatrix}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
1 & t_f & t_f^2 & t_f^3 \\
0 & 1 & 2t_f & 3t_f^2 \\
\end{bmatrix}
\begin{bmatrix}
a_0 \\
a_1 \\
a_2 \\
a_3 \\
\end{bmatrix} =
\begin{bmatrix}
q(0) \\
\dot{q}(0) \\
q(t_f) \\
\dot{q}(t_f) \\
\end{bmatrix}
\end{equation}

It can be shown that the matrix on the left is invertable, so long as $t_f-t_0 > 0$. So we can invert and solve this equation and get all the $a$ coefficients. We can then use this polynomial to generate the $q(t)$ and $\dot{q}(t)$ -- our trajectory.

```python
def simple_traj_solve(q_0, q_f, q_dot_0, q_dot_t_f, t_f):
    # Example: you are a point in space (one dimension) go from rest at the origin to at rest at (0.18, 0, 0) in 1 second
    q_0 = np.array([0])
    q_dot_0 = np.array([0])
    q_t_f = np.array([0.18])
    q_dot_t_f = np.array([0])

    b = np.array([q_0, q_dot_0, q_t_f, q_dot_t_f])
    a = np.array([[1,0,0,0],[0,1,0,0],[1, t_f, pow(t_f,2),pow(t_f,3)],[0,1,2*t_f,3*pow(t_f,2)]])
    log(a, b)
    coeff = np.linalg.solve(a, b)
    log(coeff)

    return coeff

simple_traj_info = (0, 0, 0.18, 0, 1)
simple_traj_coeff = simple_traj_solve(*simple_traj_info)
```

    [[1 0 0 0]
     [0 1 0 0]
     [1 1 1 1]
     [0 1 2 3]] [[0.  ]
     [0.  ]
     [0.18]
     [0.  ]]
    [[ 0.  ]
     [ 0.  ]
     [ 0.54]
     [-0.36]]

Here you can see that the resulting coeffictions are $a_0=0$, $a_1=0$, $a_2=0.54$, $a_0=-0.36$. Intuitively, this says that we're going to have positive acceleration, but our acceleration is going to slow down over time. Let's graph it!

```python
def simple_traj_plot(coeff, t_f):
    dt = 0.01
    ts = np.array([[1, t, pow(t,2), pow(t,3)] for t in np.arange(0, t_f+dt,  dt)])
    qs = ts@coeff
    plt.plot(ts[:,1], qs, label="x")
    plt.xlabel("time (seconds)")
    plt.xlabel("X (meters)")
    plt.legend(bbox_to_anchor=(1,1), loc=2)
    plt.show()

simple_traj_plot(simple_traj_coeff, simple_traj_info[-1])
```

**ooooooooooh so pretty**

Let's try another example, now with our full state space of $[x, y, \theta]$.

```python
def no_dynamics():
    # In this example, we go from (0.18, 0.09, 0) to (0.27,0.18, -1.5707). Our starting and ending velocities are zero
    q_0 = np.array([0.09,0.09,0])
    q_dot_0 = np.array([0,0,0])
    q_f = np.array([0.27,0.18,-1.5707])
    q_dot_f = np.array([0,0,0])
    t_f = 1

    b = np.array([q_0, q_dot_0, q_f, q_dot_f])
    a = np.array([[1,0,0,0],[0,1,0,0],[1, t_f, pow(t_f,2),pow(t_f,3)],[0,1,2*t_f,3*pow(t_f,2)]])
    coeff = np.linalg.solve(a, b)
    log(coeff)

    dt = 0.1
    ts = np.array([[1, t, pow(t,2), pow(t,3)] for t in np.arange(0, t_f+dt,  dt)])
    qs = ts@coeff

    plt.rc('text', usetex=True)
    plt.rc('font', family='serif')
    plt.gca().set_adjustable("box")
    plt.subplot(221)
    plt.plot(ts[:,1], qs[:,0])
    plt.xlabel("time (seconds)")
    plt.title("x")
    plt.subplot(222)
    plt.plot(ts[:,1], qs[:,1])
    plt.xlabel("time (seconds)")
    plt.title("y")
    plt.subplot(223)
    plt.plot(ts[:,1], qs[:,2])
    plt.xlabel("time (seconds)")
    plt.title(r"$\theta$")
    plt.subplot(224)
    plt.scatter(qs[:,0], qs[:,1])
    plt.axis('equal')
    plt.xlabel("X")
    plt.ylabel("Y")
    plt.tight_layout()
    plt.show()

no_dynamics()
```

Well, they are smooth, but these are not possible to execute! The robot cannot simply translate sideways.

# Trajectory Planning With a Simple Dynamics Model

***

```python
%%tikz -s 100,100

\draw [rotate around={-45:(0,0)}] (-.5,-1) rectangle (0.5,1);
\filldraw (0,0) circle (0.125);

\draw [->] (0,0) -- (0,1.5);
\draw [->] (0,0) -- (1.5,0);
\draw [->] (0,0) -- (1.5,1.5);
\draw (1.2, -0.2) node {$x$};
\draw (-0.2, 1.2) node {$y$};
\draw (1, 1.2) node {$v$};
```

We need to change our constraints to the system of equations. Specifically, we need our dynamics model. For now, let's assume a simplified car model.

$$ \dot{x} = v\cos(\theta) $$
$$ \dot{y} = v\sin(\theta) $$

This basically claims that for any instant in time the robot is moving a constant velocity along $\theta$. This isn't very accurate, but let's just start with that since the real dynamics of our robot are more complex.

First we will bring in the constraints from before. We must satisfy specific initial and final positions in $[x, y, \theta]$. I've used new letters for cofficients to avoid confusion.

\begin{align}
x_0 &= c_0 + c_1(0) + c_2(0)^2 + c_3(0)^3 + c_3(0)^4 + c_3(0)^5 \\
y_0 &= d_0 + d_1(0) + d_2(0)^2 + d_3(0)^3 + d_3(0)^4 + d_3(0)^5 \\
x_{t_f} &= c_0 + c_1(t_f) + c_2(t_f)^2 + c_3(t_f)^3 + c_3(t_f)^4 + c_3(t_f)^5 \\
y_{t_f} &= d_0 + d_1(t_f) + d_2(t_f)^2 + d_3(t_f)^3 + d_3(t_f)^4 + d_3(t_f)^5 \\
\end{align}

Notice here we have 12 unknowns, $c_0 \dots c_5$ and $d_0 \dots d_5$. So we're gonna need more equations for there to be a unique solution. Also notice we haven't defined any constraints related to our dynamics model. That would be a good place to get our other equations!

First, we want to be able to specify initial velocity $v_0$ and final velocity $v_{t_f}$. It is easlier to just constrain $\dot{x}_0$,  $\dot{y}_0$,  $\dot{x}_{t_f}$,  $\dot{y}_{t_f}$. So if we want to specify that we start facing $\tfrac{\pi}{2}$ going 1m/s, we'd just specify $cos(\tfrac{\pi}{2})$ for $\dot{x}_0$ and $sin(\tfrac{\pi}{2})$ for $\dot{y}_0$.

\begin{align}
\dot{x}_0 &= c_1 \\
\dot{y}_0 &= d_1 \\
\dot{x}_{t_f} &= (0)c_0 + (1)c_1 + 2t_fc_2 + 3{t_f}^2c_3 + 4{t_f}^3c_4 + 5{t_f}^4c_5 \\
\dot{y}_{t_f} &= (0)d_0 + (1)d_1 + 2t_fd_2 + 3{t_f}^2d_3 + 4{t_f}^3d_4 + 5{t_f}^4d_5
\end{align}

Let's also make sure x and y components obey trigonometry.

\begin{align}
  v\cos(\theta)\sin(\theta) + v\cos(\theta)\sin(\theta) &= v\sin(2\theta) \\
  \dot{x}\sin(\theta) + \dot{y}\sin(\theta) &= v\sin(2\theta)
\end{align}

We can get two equations out of this by specifying initial and final velocities

\begin{align}
v_0\sin(2\theta_0) &= \dot{x}_0\sin(\theta_0) + \dot{y}_0\cos(\theta_0) \\
v_{t_f}\sin(2\theta_{t_f}) &= \dot{x}_{t_f}\sin(\theta_{t_f}) + \dot{y}_{t_f}\cos(\theta_{t_f})
\end{align}

We should write out the full form though, to make things in terms of our coefficients.

\begin{align}
v(0)\sin(2\theta_0) &= \Big[c_1 + 2(0)c_2 + 3(0)^2c_3 + 4(0)^3c_4 + 5(0)^4c_5\Big]\sin(\theta_0) + \Big[d_1 + 2(0)d_2 + 3(0)^2d_3 + 4(0)^3d_4 + 5(0)^4d_5\Big]\cos(\theta_0) \\
v(0)\sin(2\theta_0) &= \sin(\theta_0)c_1 + \cos(\theta_0)d_1
\end{align}

\begin{align}
v(t_f)\sin(2\theta_{t_f}) &= \Big[c_1 + 2(t_f)c_2 + 3(t_f)^2c_3 + 4(t_f)^3c_4\ + 5(t_f)^4c_5\Big]\sin(\theta_{t_f}) + \Big[d_1 + 2(t_f)d_2 + 3(t_f)^2d_3 + 4(t_f)^3d_4 + 5(t_f)^4d_5\Big]\cos(\theta_{t_f}) \\
v(t_f)\sin(2\theta_{t_f}) &= \sin(\theta_{t_f})c_1 + 2\sin(\theta_{t_f})t_fc_2 + 3\sin(\theta_{t_f}){t_f}^2c_3 + 4\sin(\theta_{t_f}){t_f}^3c_4  + 5\sin(\theta_{t_f}){t_f}^4c_5 + \cos(\theta_{t_f})d_1 + 2\cos(\theta_{t_f})t_fd_2 + 3\cos(\theta_{t_f}){t_f}^2d_3 + 4\cos(\theta_{t_f}){t_f}^3d_4 + 5\cos(\theta_{t_f}){t_f}^4d_5 \\
\end{align}

The last two equations constrains the robot from moving in any direction other than its heading. Of course it must relate $\dot{x}$ to $\dot{y}$. Still not totally sure how we got this equation so I'm just copying it from some slides$\dots$. However you can plug in some example values and check. For instance translating sideways violates this equation: set $\dot{x}=1$, $\dot{y}=0$, $v=1$, $\theta=\tfrac{\pi}{2}$.

\begin{align}
v\cos(\theta)\sin(\theta) - v\cos(\theta)\sin(\theta) &= 0 \\
v\cos(\theta)\sin(\theta) - v\sin(\theta)\cos(\theta) &= 0 \\
\dot{x}\sin(\theta) - \dot{y}\cos(\theta) &= 0
\end{align}

and again written out fully in terms of our coefficients

\begin{align}
\Big[c_1 + 2(0)c_2 + 3(0)^2c_3 + 4(0)^3c_4 + 5(0)^4c_5\Big]\sin(\theta_0) - \Big[d_1 + 2(0)d_2 + 3(0)^2d_3 + 4(0)^3d_4 + 5(0)^4d_5\Big]\cos(\theta_0) &= 0 \\
\sin(\theta_0)c_1 - \cos(\theta_0)d_1 &= 0
\end{align}

\begin{align}
\Big[c_1 + 2(t_f)c_2 + 3(t_f)^2c_3 + 4(t_f)^3c_4 + 5(t_f)^4c_5\Big]\sin(\theta_{t_f}) - \Big[d_1 + 2(t_f)d_2 + 3(t_f)^2d_3 + 4(t_f)^3d_4 + 5(t_f)^4d_5\Big]\cos(\theta_{t_f}) &= 0 \\
\sin(\theta_{t_f})c_1 + 2\sin(\theta_{t_f})t_fc_2 + 3\sin(\theta_{t_f}){t_f}^2c_3 + 4\sin(\theta_{t_f}){t_f}^3c_4  + 5\sin(\theta_{t_f}){t_f}^4c_5 - \cos(\theta_{t_f})d_1 - 2\cos(\theta_{t_f})t_fd_2 - 3\cos(\theta_{t_f}){t_f}^2d_3 - 4\cos(\theta_{t_f}){t_f}^3d_4 - 5\cos(\theta_{t_f}){t_f}^4d_5 &= 0
\end{align}

Ok, that should work. Now let's write it out in matrix form. We use $c$ and $s$ to shorten $\sin$ and $\cos$.

\setcounter{MaxMatrixCols}{20}
\begin{equation}
\begin{bmatrix}
1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\
0 & s(\theta_0) & 0 & 0 & 0 & 0 & 0 & c(\theta_0) & 0 & 0 & 0 & 0\\
0 & s(\theta_0) & 0 & 0 & 0 & 0 & 0 & -c(\theta_0) & 0 & 0 & 0 & 0\\
0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\
0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0\\
1 & t & {t_f}^2 & {t_f}^3 & {t_f}^4 & {t_f}^5 & 0 & 0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 0 & 1 & t_f & {t_f}^2 & {t_f}^3 & {t_f}^4 & {t_f}^5 \\
0 & s(\theta_{t_f}) & 2s(\theta_{t_f})t_f & 3s(\theta_{t_f}){t_f}^2 & 4s(\theta_{t_f}){t_f}^3 & 5s(\theta_{t_f}){t_f}^4 & 0 & c(\theta_{t_f}) & 2c(\theta_{t_f}){t_f} & 3c(\theta_{t_f}){t_f}^2 & 4c(\theta_{t_f}){t_f}^3 & 5c(\theta_{t_f}){t_f}^4 \\
0 & s(\theta_{t_f}) & 2s(\theta_{t_f})t_f & 3s(\theta_{t_f}){t_f}^2 & 4s(\theta_{t_f}){t_f}^3 & 5s(\theta_{t_f}){t_f}^4 & 0 & -c(\theta_{t_f}) & -2c(\theta_{t_f}){t_f} & -3c(\theta_{t_f}){t_f}^2 & -4c(\theta_{t_f}){t_f}^3 & -5c(\theta_{t_f}){t_f}^4 \\
0 & 1 & 2t_f & 3{t_f}^2 & 4{t_f}^3 & 5{t_f}^4 & 0 & 0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 2t_f & 3{t_f}^2 & 4{t_f}^3 & 5{t_f}^4
\end{bmatrix}
\begin{bmatrix}
c_0 \\
c_1 \\
c_2 \\
c_3 \\
c_4 \\
c_5 \\
d_0 \\
d_1 \\
d_2 \\
d_3 \\
d_4 \\
d_5
\end{bmatrix} =
\begin{bmatrix}
x_0 \\
y_0 \\
0 \\
v_0s(2\theta_0) \\
c(\theta_0)v_0 \\
s(\theta_0)v_0 \\
x_{t_f} \\
y_{t_f} \\
0 \\
v_{t_f}s(2\theta_{t_f}) \\
c(\theta_{t_f})v_{t_f} \\
s(\theta_{t_f})v_{t_f} \\
\end{bmatrix}
\end{equation}

```python
# Let's solve this in code like we did before
def plot_vars(traj_plan):
    dt = 0.001
    T = np.arange(0, traj_plan.get_t_f()+dt, dt)
    xts = np.array([[1, t, pow(t,2), pow(t,3), pow(t,4), pow(t,5), 0, 0, 0, 0, 0, 0] for t in T])
    xdts = np.array([[0, 1, 2*t, 3*pow(t,2), 4*pow(t,3), 5*pow(t,4), 0, 0, 0, 0, 0, 0] for t in T])
    yts = np.array([[0, 0, 0, 0, 0, 0, 1, t, pow(t,2), pow(t,3), pow(t,4), pow(t,5)] for t in T])
    ydts = np.array([[0, 0, 0, 0, 0, 0, 0, 1, 2*t, 3*pow(t,2), 4*pow(t,3), 5*pow(t,4)] for t in T])
    xs = xts@traj_plan.get_coeff()
    ys = yts@traj_plan.get_coeff()
    xds = xdts@traj_plan.get_coeff()
    yds = ydts@traj_plan.get_coeff()

    plt.rc('text', usetex=True)
    plt.rc('font', family='serif')
    plt.rc('axes.formatter', useoffset=False)
    plt.figure(figsize=(10, 2.5))

    plt.subplot(141)
    plt.plot(T, xs, linewidth=3)
    plt.xlabel("time (seconds)")
    plt.title("X")

    plt.subplot(142)
    plt.plot(T, ys, linewidth=3, color='r')
    plt.xlabel("time (seconds)")
    plt.title("Y")

    plt.subplot(143)
    plt.plot(T, xds, linewidth=3, color='g')
    plt.xlabel("time (seconds)")
    plt.title("$\dot{x}$")
    plt.tight_layout()

    plt.subplot(144)
    plt.plot(T,yds, linewidth=3, color='y')
    plt.xlabel("time (seconds)")
    plt.title("$\dot{y}$")
    plt.tight_layout()
    plt.show()

def plot_traj(traj_plan):
    dt = 0.03
    T = np.arange(0, traj_plan.get_t_f()+dt, dt)
    xts = np.array([[1, t, pow(t,2), pow(t,3), pow(t,4), pow(t,5), 0, 0, 0, 0, 0, 0] for t in T])
    yts = np.array([[0, 0, 0, 0, 0, 0, 1, t, pow(t,2), pow(t,3), pow(t,4), pow(t,5)] for t in T])
    xs = xts@traj_plan.get_coeff()
    ys = yts@traj_plan.get_coeff()
    plot_traj_pts(xs, ys, T, traj_plan.waypoints)

def plot_traj_pts(xs, ys, T, waypoints):
    plt.figure(figsize=(5, 5))
    plt.title("Trajectory")
    plt.xlabel("X")
    plt.ylabel("Y")
    W = 2
    plt.xlim(0, W * 0.18)
    plt.ylim(0, W * 0.18)
    plt.xticks(np.arange(2*W+1)*0.09)
    plt.yticks(np.arange(2*W+1)*0.09)
    plt.grid(True)
    plt.gca().set_axisbelow(True)

    for t, pt in waypoints:
        arrow_dx = cos(pt.theta) * (pt.v) * 0.1
        arrow_dy = sin(pt.theta) * (pt.v) * 0.1
        plt.arrow(pt.x, pt.y, arrow_dx, arrow_dy, head_width=0.005, head_length=0.005, width=0.001, fc='k', ec='k')

    plt.scatter(xs, ys, marker='.', linewidth=0)

    plt.show()
```

```python
from math import sin, cos, pi
from collections import namedtuple

WayPoint = namedtuple('WayPoint', ['x', 'y', 'theta', 'v'])

class TrajPlan:
    def x_constraint(t):
        return [1, t, pow(t, 2), pow(t, 3), pow(t, 4), pow(t, 5), 0, 0, 0, 0, 0, 0]

    def y_constraint(t):
        return [0, 0, 0, 0, 0, 0, 1, t, pow(t, 2), pow(t, 3), pow(t, 4), pow(t, 5)]

    def non_holonomic_constraint(theta_t, t):
        s_t = sin(theta_t)
        c_t = cos(theta_t)
        t_2 = pow(t, 2)
        t_3 = pow(t, 3)
        t_4 = pow(t, 4)
        return [0, s_t, 2 * s_t * t, 3 * s_t * t_2, 4 * s_t * t_3, 5 * s_t * t_4, 0, c_t, 2 * c_t * t, 3 * c_t * t_2, 4 * c_t * t_3, 5 * c_t * t_4]

    def trig_constraint(theta_t, t):
        s_t = sin(theta_t)
        c_t = cos(theta_t)
        t_2 = pow(t, 2)
        t_3 = pow(t, 3)
        t_4 = pow(t, 4)
        return [0, s_t, 2 * s_t * t, 3 * s_t * t_2, 4 * s_t * t_3, 5 * s_t * t_4, 0, -c_t, -2 * c_t * t, -3 * c_t * t_2, -4 * c_t * t_3, -5 * c_t * t_4]

    def x_dot_constraint(t):
        return [0, 1, 2 * t, 3 * pow(t, 2), 4 * pow(t, 3), 5 * pow(t, 4), 0, 0, 0, 0, 0, 0]

    def y_dot_constraint(t):
        return [0, 0, 0, 0, 0, 0, 0, 1, 2 * t, 3 * pow(t, 2), 4 * pow(t, 3), 5 * pow(t, 4)]

    def solve(self, waypoints):
        # Setup the matrices to match the equation above
        A = []
        b = []

        for t, pt in waypoints:
            A += [TrajPlan.x_constraint(t),
                      TrajPlan.y_constraint(t),
                      TrajPlan.non_holonomic_constraint(pt.theta, t),
                      TrajPlan.trig_constraint(pt.theta, t),
                      TrajPlan.x_dot_constraint(t),
                      TrajPlan.y_dot_constraint(t)]
            b += [pt.x,
                      pt.y,
                      0,
                      pt.v*sin(2*pt.theta),
                      cos(pt.theta)*pt.v,
                      sin(pt.theta)*pt.v]

        A = np.array(A)
        b = np.array(b)
        rank = np.linalg.matrix_rank(A)

        if rank == A.shape[1]:
            if A.shape[0] == A.shape[1]:
                coeff = np.linalg.solve(A, b)
            else:
                warning("not square, using least squares.".format(A.shape))
                coeff, resid, rank, s = np.linalg.lstsq(A, b)
        else:
            warning("Ranks don't match! {} equations {} variables, using least squares".format(rank, A.shape[1]))
            coeff, resid, rank, s = np.linalg.lstsq(A, b)

        debug("rank {}".format(rank))
        debug("A: \n{}".format(A))
        debug("coeff: \n{}".format(coeff))
        error = np.sum(np.power(A@coeff - b, 2))
        if error > 1e-10:
            info("These two vectors should be equal! But there is error.")
            info("b is: \n{}".format(b))
            info("A@coeff is: \n{}".format(A@coeff))
        info("RMS Error of solution to equations")
        info(error)

        self.coeff = coeff
        self.waypoints = waypoints

    def get_coeff(self):
        return self.coeff

    def get_t_f(self):
        return self.waypoints[-1][0]
```

## Example Plots

```python
# forward 1 cell, start from rest, end at 40cm/s, do it in .5 seconds
LOG_LVL = 5
fwd_1 = TrajPlan()
fwd_1.solve([(0, WayPoint(0.09, 0.09, pi/2, 0)), (0.5, WayPoint(0.09, 0.27, pi/2, 0.6))])
plot_vars(fwd_1)
plot_traj(fwd_1)
```

```python
# continue by turning right 90 degrees
LOG_LVL = 1
turn_right = TrajPlan()
turn_right.solve([(0, WayPoint(0.09, 0.18, pi/2, 0.4)), (0.5, WayPoint(0.18, 0.27, 0, 0.4))])
plot_vars(turn_right)
plot_traj(turn_right)
```

```python
# 3 waypoints!
LOG_LVL = 1
turn_right = TrajPlan()
turn_right.solve([(0, WayPoint(0.09, 0.09, pi/2, 0.0)), (0.5, WayPoint(0.18, 0.18, 0, 0.35)), (1, WayPoint(0.27, 0.27, pi/2, 0))])
plot_vars(turn_right)
plot_traj(turn_right)
```

**Note for this system of equations with 3 waypoints, there is no solution. However, the error of the solution found is very small.**

Now let's find one that really sucks!

```python
# 4 waypoints!
LOG_LVL = 1
turn_right = TrajPlan()
turn_right.solve([(0, WayPoint(0.09, 0.0, pi/2, 0.1)),
                  (1, WayPoint(0.09, 0.18, pi/2, 0.1)),
                  (2, WayPoint(0.18, 0.27, 0, 0.1)),
                  (3, WayPoint(0.27, 0.27, 0, 0.1))])
plot_traj(turn_right)
```

# Trajectory Following

***

Now that we have a trajectory, we want to design a controller that will follow it as closely as possible. To do this, I'm just going to do a proportional controller. Later we will design an optimal controller. We want to make sure the robot is on the path, facing along the path, and going the right speed. When all of these are true the change in speed should be zero. Let's come up with an equation to relate current pose and velocity to the desired pose and velocity. Let our outputs be the linear velocity $v$ and the rotational velocity $w$.

$$ w = \bar{w} + d*P_1 + (\bar{\theta} - \theta)P_2$$
$$ v = \bar{v} + l*P_3$$

where $v_d$ is desired velocity, $\theta_d$ is the desired angle, $d$ is signed distance to the planned trajectory (to the right of the plan is positive), $v_d$ and $w_d$ are the desired velocities of the robot, and $P_1$, $P_2$, and $P_3$ are constants. Essentially what we're saying with the first equation is that when you're far off the trajectory you need to turn harder to get back on to it, but you also need to be aligned with it. The second equation says if you're lagging behind your plan speed up, or slow down if you're overshooting.

```python
from math import atan2, sqrt

LOG_LVL = 5
def simulate(q_0, waypoints, P_1, P_2, P_3, A=3):
    traj = TrajPlan()
    traj.solve(waypoints)

    dt = 0.01
    x = q_0[0]
    y = q_0[1]
    theta = q_0[2]
    v = q_0[3]
    w = q_0[4]
    actual_v = q_0[3]
    actual_w = q_0[4]
    v_acc = A * dt
    TRACK_WIDTH_M = 0.0633
    w_acc = v_acc / (TRACK_WIDTH_M/2)
    T = np.arange(0, traj.get_t_f()+dt, dt)
    x_bar_list = []
    y_bar_list = []
    x_list = []
    y_list = []
    for t in T:
        x_bar = [1, t, pow(t,2), pow(t,3), pow(t,4), pow(t,5), 0, 0, 0, 0, 0, 0] @ traj.get_coeff()
        dx_bar = [0, 1, 2*t, 3*pow(t,2), 4*pow(t,3), 5*pow(t,4), 0, 0, 0, 0, 0, 0] @ traj.get_coeff()
        ddx_bar = [0, 0, 0, 0, 0, 0, 0, 0, 2, 6*t, 12*pow(t,2), 20*pow(t,3)] @ traj.get_coeff()
        y_bar = [0, 0, 0, 0, 0, 0, 1, t, pow(t,2), pow(t,3), pow(t,4), pow(t,5)] @ traj.get_coeff()
        dy_bar = [0, 0, 0, 0, 0, 0, 0, 1, 2*t, 3*pow(t,2), 4*pow(t,3), 5*pow(t,4)] @ traj.get_coeff()
        ddy_bar = [0, 0, 0, 0, 0, 0, 0, 0, 2, 6*t, 12*pow(t,2), 20*pow(t,3)] @ traj.get_coeff()
        theta_bar = atan2(dy_bar, dx_bar)
        v_bar = sqrt(dx_bar*dx_bar + dy_bar*dy_bar)
        w_bar = 1/v_bar * (ddy_bar*cos(theta_bar) - ddx_bar*sin(theta_bar));

        # simple Dubin's Car forward kinematics
        x += cos(theta) * actual_v * dt
        y += sin(theta) * actual_v * dt
        theta += actual_w * dt

        # control
        euclidian_error = np.sqrt(pow(x_bar - x, 2) + pow(y_bar - y, 2))
        transformed_x = (x - x_bar) * cos(-theta_bar) + (y - y_bar) * -sin(-theta_bar)
        transformed_y = (x - x_bar) * sin(-theta_bar) + (y - y_bar) * cos(-theta_bar)
        right_of_traj = transformed_y < 0
        signed_euclidian_error = euclidian_error if right_of_traj else -euclidian_error
        lag_error = -transformed_x
        w = w_bar + signed_euclidian_error * P_1 + (theta_bar - theta) * P_2
        v = v_bar + lag_error * P_3

        # simple acceleration model
        if v < actual_v:
            actual_v = max(v, actual_v - v_acc)
        elif v > actual_v:
            actual_v = min(v, actual_v + v_acc)
        if w < actual_w:
            actual_w = max(w, actual_w - w_acc)
        elif w > actual_w:
            actual_w = min(w, actual_w + w_acc)

        x_bar_list.append(x_bar)
        y_bar_list.append(y_bar)
        x_list.append(x)
        y_list.append(y)

    plt.figure(figsize=(5, 5))
    W = 3
    plt.scatter(x_bar_list, y_bar_list, marker='.', linewidth=0, c='black', label='desired traj')
    plt.scatter(x_list, y_list, marker='.', linewidth=0, c=T, label='robot traj')
    plt.xlim(0, W * 0.18)
    plt.ylim(0, W * 0.18)
    plt.xticks(np.arange(2*W+1)*0.09)
    plt.yticks(np.arange(2*W+1)*0.09)
    plt.grid(True)
    plt.gca().set_axisbelow(True)
    plt.xlabel("X")
    plt.ylabel("Y")
    plt.title("Trajectory Tracking")
    plt.legend(bbox_to_anchor=(1,1), loc=2)
```

```python
test_P_1=300
test_P_2=50
test_P_3=10
robot_q_0 = (0.08, 0.18, pi/2, 0.3, 0)
traj = [(0, WayPoint(0.09, 0.18, pi/2, 0.5)), (0.5, WayPoint(0.18, 0.27, 0, 0.35)), (1, WayPoint(0.27, 0.36, pi/2, 0))]
simulate(robot_q_0, traj, test_P_1, test_P_2, test_P_3)
plt.show()
```

```python
robot_q_0 = (0.11, 0.18, pi/2, 0.2, 5)
traj = [(0, WayPoint(0.09, 0.18, pi/2, 0.2)), (1, WayPoint(0.18, 0.27, 0, 0.35))]
simulate(robot_q_0, traj, test_P_1, test_P_2, test_P_3)
plt.show()
```

```python
robot_q_0 = (0.0, 0.25, 0, 0.2, 0)
traj = [(0, WayPoint(0.0, 0.27, 0, 0.2)), (1.25, WayPoint(0.54, 0.27, 0, 0.2))]
simulate(robot_q_0, traj, test_P_1, test_P_2, test_P_3)
plt.show()
```

```python
robot_q_0 = (0.45, 0.05, pi+0.25, 0.3, 0)
traj = [(0, WayPoint(0.45, 0.09, pi, 0.4)), (0.75, WayPoint(0.27, 0.27, pi/2, 0.4))]
simulate(robot_q_0, traj, test_P_1, test_P_2, test_P_3)
plt.show()
```

```python
robot_q_0 = (0.0, 0.25, 0, 0.2, -5)
traj = [(0, WayPoint(0.0, 0.27, 0, 0.2)), (2, WayPoint(0.48, 0.36, pi/2, 0.2))]
simulate(robot_q_0, traj, test_P_1, test_P_2, test_P_3)
plt.show()
```

```python
robot_q_0 = (0.25, 0.28, -pi*4/7, 0.5, 0)
traj = [(0, WayPoint(0.27, 0.27, -pi/2, 0.8)), (0.35, WayPoint(0.45, 0.09, 0, 0.8))]
simulate(robot_q_0, traj, test_P_1, test_P_2, test_P_3, A=6)
plt.show()
```

```python
# no initial error
robot_q_0 = (0.11, 0.18, pi/2, 0.8, 0)
traj = [(0, WayPoint(0.09, 0.18, pi/2, 0.8)), (0.25, WayPoint(0.18, 0.27, 0, 0.6)), (.5, WayPoint(0.27, 0.36, pi/2, 0.4))]
simulate(robot_q_0, traj, 10, 1000, 6, A=5)
plt.show()
```

**Note**: The code above has a bug if I use `-pi` instead of `pi` in `robot_q_0`

# LQR - The Optimal Controller

***

## Overview of the Steps:

### 1. Write out the non-linear dynamics $\dot{\vec{x}} = f(\vec{x}, \vec{u})$

Here we are interested in the full blown system dynamics of the actual smartmouse robot. The forward kinematics, which depend on the current state $x$, $y$, and $\theta$ and the velocity inputs of the wheels $v_l$, and $v_r$ are as follows. In the general case where the two wheels have different velocities, we have this:

\begin{align}
  R &= \frac{W(v_l+v_r)}{2(v_r-v_l)} && \text{radius of turn} \\
  \theta &\leftarrow \theta + \dfrac{v_l}{R-\frac{W}{2}}\Delta t \\
  x &\leftarrow x-R\Bigg(\sin{\Big(\frac{v_r-v_l}{W}\Delta t-\theta\Big)}+\sin{\theta}\Bigg) \\
  y &\leftarrow y-R\Bigg(\cos{\Big(\frac{v_r-v_l}{W}\Delta t-\theta\Big)}-\cos{\theta}\Bigg)
\end{align}

And in the special case where we're going perfectly straight:

\begin{align}
  \theta &\leftarrow \theta \\
  x &\leftarrow x + v\Delta t\cos(\theta) \\
  y &\leftarrow y + v\Delta t\sin(\theta) \\
\end{align}

We can take these equations and write them in the form of $\dot{\vec{x}} = f(\vec{x},\vec{u})$. Confusingly, $\vec{x}$ here is the full state vector $[x, y, \theta]$. Most controls texts simply use $\vec{x}$, so I'm sticking with that. Also, we defined $u = [v_l, v_r]$

\begin{align}
  \dot{x} &= \begin{bmatrix}\dot{x}\\ \dot{y}\\ \dot{\theta}\end{bmatrix} \\
          &= \begin{bmatrix}
                -R\Bigg(\sin{\Big(\frac{v_r-v_l}{W}\Delta t-\theta\Big)}+\sin{\theta}\Bigg) \\
                -R\Bigg(\cos{\Big(\frac{v_r-v_l}{W}\Delta t-\theta\Big)}-\cos{\theta}\Bigg) \\
                \frac{v_l}{R-\frac{W}{2}}\Delta t \\
                \end{bmatrix}
\end{align}

### 2. Identify the points around which we linearize our system, $(\bar{u}, \bar{x})$

Because we are tracking a trajectory, we want to linearize around the trajectory we are trying to track. That means $\bar{u}$ is the control input associated with the trajectory, which means solving for the feed forward inputs. Specifically, that means we need to compute the $v_l$ and $v_r$ that would follow the trajectory at the point $\bar{x}$. To do this we must pick velocities that make instantaneous turning radius $R$ equal the instantaneous radius of the trajcetory at $\bar{x}$, and make the linear velocity at $\bar{x}$ equal the instantaneous linear velocity of the robot center $v$. To do this, we go back to our basic kinematics equations which rely on the fact that all points on the robot (center, left wheel, right wheel) have the same rotational velocity $\omega$ around the ICC.

\begin{align}
  \omega = \frac{v}{R} &= \frac{v_l}{R-\frac{W}{2}} \\
  \frac{v}{R}\bigg(R - \frac{W}{2}\bigg) &= v_l \\
  \omega = \frac{v}{R} &= \frac{v_r}{R+\frac{W}{2}} \\
  \frac{v}{R}\bigg(R + \frac{W}{2}\bigg) &= v_r \\
\end{align}

Using these equations we can solve for the velocities of the wheels, which together make up $\bar{u}$. We just need the $R$ and $v$. These should be derived from the equation of the trajectory we are tracking. These are well studied equations, for which [a proof can be found other places on the internet](http://mathworld.wolfram.com/Curvature.html).

$$ R = \frac{\dot{x}\ddot{y}-\dot{y}\ddot{x}}{{\big({\dot{x}}^2 + {\dot{y}}^2\big)}^{\frac{3}{2}}} = \frac{(c_1+2c_2t+3c_3t^2+4c_4t^3)(2d_2+6d_3t+12d_4t^2) - (d_1+2d_2t+3d_3t^3+4d_4t^3)(2c_2+6c_3t+12c_4t^2)}{{\big({(c_1+2c_2t+3c_3t^2+4c_4t^3)}^2 + {(d_1+2d_2t+3d_3t^3+4d_4t^3)}^2\big)}^{\frac{3}{2}}} $$

$$ v = \sqrt{{(\dot{x})}^2 + {(\dot{y})}^2}  = \sqrt{{(c_1+2c_2t+3c_3t^2+4c_4t^3)}^2 + {(d_1+2d_2t+3d_3t^3+4d_4t^3)}^2} $$

We can plug in the coefficients of our polynomials and get values for $v$ and $R$. Then, we can plus these into the equations just above and get the feed forward wheel velocities.

### 3. Write the linearized dynamics around $\bar{x}$ as $\dot{\vec{x}} \approx A\delta_x + B\delta_u$, where $\delta_x = (\vec{x} - \bar{x})$ and $\delta_u = (\vec{u} - \bar{u})$

To do this, we need the partial derivitive matrixes $A$ and $B$.

$$ A = \begin{bmatrix}
         \frac{\partial f_1}{\partial x}\big|_{\bar{x}\bar{u}} &
         \frac{\partial f_1}{\partial y}\big|_{\bar{x}\bar{u}} &
         \frac{\partial f_1}{\partial \theta}\big|_{\bar{x}\bar{u}} \\
         \frac{\partial f_2}{\partial x}\big|_{\bar{x}\bar{u}} &
         \frac{\partial f_2}{\partial y}\big|_{\bar{x}\bar{u}} &
         \frac{\partial f_2}{\partial \theta}\big|_{\bar{x}\bar{u}} \\
         \frac{\partial f_3}{\partial x}\big|_{\bar{x}\bar{u}} &
         \frac{\partial f_3}{\partial y}\big|_{\bar{x}\bar{u}} &
         \frac{\partial f_3}{\partial \theta}\big|_{\bar{x}\bar{u}} \\
       \end{bmatrix}
     = \begin{bmatrix}
        0 & 0 & R\bigg(\cos\Big(\frac{\bar{v}_r-\bar{v}_l}{W}\Delta t - \bar{\theta}\Big) - \cos(\bar{\theta})\bigg) \\
        0 & 0 & -R\bigg(\sin\Big(\frac{\bar{v}_r-\bar{v}_l}{W}\Delta t - \bar{\theta}\Big) + \sin(\bar{\theta})\bigg) \\
        0 & 0 & 0 \\
       \end{bmatrix}
$$

$$ B = \begin{bmatrix}
         \frac{\partial f_1}{\partial v_l}\big|_{\bar{x}\bar{u}} &
         \frac{\partial f_1}{\partial v_r}\big|_{\bar{x}\bar{u}} \\
         \frac{\partial f_2}{\partial v_l}\big|_{\bar{x}\bar{u}} &
         \frac{\partial f_2}{\partial v_r}\big|_{\bar{x}\bar{u}} \\
         \frac{\partial f_3}{\partial v_l}\big|_{\bar{x}\bar{u}} &
         \frac{\partial f_3}{\partial v_r}\big|_{\bar{x}\bar{u}} \\
       \end{bmatrix}
     = \begin{bmatrix}
          R\cos\Big(\frac{(\bar{v}_r-\bar{v}_l)\Delta t}{W}-\bar{\theta}\Big)\frac{\Delta t}{W} &
          -R\cos\Big(\frac{(\bar{v}_r-\bar{v}_l)\Delta t}{W}-\bar{\theta}\Big)\frac{\Delta t}{W} \\
          -R\sin\Big(\frac{(\bar{v}_r-\bar{v}_l)\Delta t}{W}-\bar{\theta}\Big)\frac{\Delta t}{W} &
          R\sin\Big(\frac{(\bar{v}_r-\bar{v}_l)\Delta t}{W}-\bar{\theta}\Big)\frac{\Delta t}{W} \\
          \frac{\Delta t}{R-\frac{W}{2}} &
          0 \\
       \end{bmatrix}
$$

### 4. Check if our system is controllable by looking at the rank of the controllability matrix $C = [B, AB, A^2B, \dots, A^{n-1}B]$

We have three state variables so $n = 3$, which means $C = [B, AB, A^2B]$.

$$
AB = \begin{bmatrix}
       R\bigg(\cos\Big(\frac{v_r-v_l}{W}\Delta t - \theta\Big) - \cos(\theta)\bigg)\frac{\Delta t}{R-\frac{W}{2}} & 0 \\
       -R\bigg(\sin\Big(\frac{v_r-v_l}{W}\Delta t - \theta\Big) + \sin(\theta)\bigg)\frac{\Delta t}{R-\frac{W}{2}} & 0 \\
       0 & 0 \\
     \end{bmatrix}
$$

$$
A^2B =
     \begin{bmatrix}
       0 & 0 & 0\\
       0 & 0 & 0\\
       0 & 0 & 0\\
     \end{bmatrix}
     * B
     = \begin{bmatrix}
       0 & 0 & 0\\
       0 & 0 & 0\\
       0 & 0 & 0\\
     \end{bmatrix}
$$

$$ C = \begin{bmatrix}
         \begin{bmatrix}
           R\cos\Big(\frac{(\bar{v_r}-\bar{v_l})\Delta t}{W}-\theta\Big)\frac{\Delta t}{W} &
           -R\cos\Big(\frac{(\bar{v_r}-\bar{v_l})\Delta t}{W}-\theta\Big)\frac{\Delta t}{W} \\
           -R\sin\Big(\frac{(\bar{v_r}-\bar{v_l})\Delta t}{W}-\theta\Big)\frac{\Delta t}{W} &
           R\sin\Big(\frac{(\bar{v_r}-\bar{v_l})\Delta t}{W}-\theta\Big)\frac{\Delta t}{W} \\
           \frac{\Delta t}{R-\frac{W}{2}} &
           0 \\
         \end{bmatrix} &
         \begin{bmatrix}
           R\bigg(\cos\Big(\frac{v_r-v_l}{W}\Delta t - \theta\Big) - \cos(\theta)\bigg)\frac{\Delta t}{R-\frac{W}{2}} & 0 \\
           -R\bigg(\sin\Big(\frac{v_r-v_l}{W}\Delta t - \theta\Big) + \sin(\theta)\bigg)\frac{\Delta t}{R-\frac{W}{2}} & 0 \\
           0 & 0 \\
         \end{bmatrix} &
         \begin{bmatrix}
           0 & 0 & 0\\
           0 & 0 & 0\\
           0 & 0 & 0\\
         \end{bmatrix}
       \end{bmatrix}
$$

What is the rank of this matrix? It seems to depend on specific values of $x, y, \theta$.

### 5. Pick cost parameters $Q$ and $R$

These need to be tuned on the simulation or real system, but the identity matrices $I$ are good starting points.

### 6. Solve for $K$ given $LQR(A, B, Q, R)$

We want to minimize the quadratice cost function $J$, which is defined as follows.

$$ J = \sum_0^N(\vec{x}_t - \bar{x}_t)^TQ(\vec{x}_t - \bar{x}_t) + \sum_0^N(\vec{u}_t - \bar{u}_t)^TR(\vec{u}_t - \bar{u}_t) $$

We can do this with least squares, or dynamics programming. DP is more efficient O(Nn^3). N is some finite horizon, and n is the number of state dimensions (3 for us).

maybe we compute K once instead of at every time step. could be consistent within one motion primitive.

### 7. Apply our new controller of the form $\vec{u} = -K(\vec{x} - \bar{x}) + \bar{u}$

```python
from math import atan2
import scipy.linalg

# source: http://www.kostasalexis.com/lqr-control.html
def dlqr(A,B,Q,R):
    """Solve the discrete time lqr controller.

    p[k+1] = A p[k] + B u[k]

    cost = sum p[k].T*Q*p[k] + u[k].T*R*u[k]
    """
    #ref Bertsekas, p.151

    #first, try to solve the ricatti equation
    P = np.matrix(scipy.linalg.solve_discrete_are(A, B, Q, R))

    #compute the LQR gain
    K = np.matrix(scipy.linalg.inv(B.T*P*B+R)*(B.T*P*A))

    eigVals, eigVecs = scipy.linalg.eig(A-B*K)

    return K, P, eigVals

def follow_plan(q_0, waypoints, P_1, P_2, P_3):
    traj = TrajPlan()
    traj.solve(waypoints)

    dt = 0.01
    x = q_0[0]
    y = q_0[1]
    theta = q_0[2]
    vl = q_0[3]
    vr = q_0[3]
    actual_vl = vl
    actual_vr = vr
    v_acc = 2 * dt
    W = 0.0633
    T = np.arange(0, traj.get_t_f()+dt, dt)
    x_bar_list = []
    y_bar_list = []
    x_list = []
    y_list = []
    vl_list = []
    vr_list = []
    actual_vl_list = []
    actual_vr_list = []
    for t in T:
        x_bar = [1, t, pow(t,2), pow(t,3), pow(t,4), pow(t,5), 0, 0, 0, 0, 0, 0] @ traj.get_coeff()
        dx_bar = [0, 1, 2*t, 3*pow(t,2), 4*pow(t,3), 5*pow(t,4), 0, 0, 0, 0, 0, 0] @ traj.get_coeff()
        ddx_bar = [0, 0, 0, 0, 0, 0, 0, 0, 2, 6*t, 12*pow(t,2), 20*pow(t,3)] @ traj.get_coeff()
        y_bar = [0, 0, 0, 0, 0, 0, 1, t, pow(t,2), pow(t,3), pow(t,4), pow(t,5)] @ traj.get_coeff()
        dy_bar = [0, 0, 0, 0, 0, 0, 0, 1, 2*t, 3*pow(t,2), 4*pow(t,3), 5*pow(t,4)] @ traj.get_coeff()
        ddy_bar = [0, 0, 0, 0, 0, 0, 0, 0, 2, 6*t, 12*pow(t,2), 20*pow(t,3)] @ traj.get_coeff()
        theta_bar = atan2(dy_bar, dx_bar)

        # full forward kinematics
        if vr - vl < 1e-5:
            x = x + cos(theta) * vl * dt
            y = y + sin(theta) * vl * dt
        else:
            R = W*(vl + vr)/(2*(vr - vl))
            x = x - R * (sin((vr-vl)*dt/W - theta) + sin(theta))
            y = y - R * (cos((vr-vl)*dt/W - theta) - cos(theta))
            theta = theta + vl / (R - W/2) * dt

        # compute instanteneous Radius
        R_bar = (dx_bar*ddy_bar - dy_bar*ddy_bar)/pow((pow(dx_bar, 2) + pow(dy_bar, 2)), 3/2)

        # feed forward inputs
        v_bar = np.sqrt(dx_bar*dx_bar + dy_bar*dy_bar)
        vl_bar = v_bar/R_bar*(R_bar-W/2)
        vr_bar = v_bar/R_bar*(R_bar+W/2)

        A = np.array([[0, 0, R_bar*(cos((vr_bar - vl_bar)*dt/W - theta_bar) - cos(theta_bar))],
                      [0, 0, -R_bar*(sin((vr_bar - vl_bar)*dt/W - theta_bar) + sin(theta_bar))],
                      [0, 0, 0]])

        B = np.array([[R_bar*cos((vr_bar - vl_bar)*dt/W - theta_bar)*dt/W, -R_bar*cos((vr_bar - vl_bar)*dt/W - theta_bar)*dt/W],
                      [-R_bar*sin((vr_bar - vl_bar)*dt/W - theta_bar)*dt/W, R_bar*sin((vr_bar - vl_bar)*dt/W - theta_bar)*dt/W],
                      [dt/(R_bar-W/2), 0]]);

        Q= np.eye(3);
        R = np.eye(2);

        K, P, eigs = dlqr(A, B, Q, R)
        eigs = np.linalg.eig(A - B*K)
#         info("eigs", eigs[0])
#         debug("K", K)
        x_vec = np.array([[x],[y],[theta]])
        x_bar_vec = np.array([[x_bar],[y_bar],[theta_bar]])
        u = -K * (x_vec - x_bar_vec) + np.array([[vl_bar],[vr_bar]]);
        vl = u[0,0]
        vr = u[1,0]

        # simple acceleration model
        if vl < actual_vl:
            actual_vl = max(vl, actual_vl - v_acc)
        elif vl > actual_vl:
            actual_vl = min(vl, actual_vl + v_acc)
        if vr < actual_vr:
            actual_vr = max(vr, actual_vr - v_acc)
        elif vr > actual_vr:
            actual_vr = min(vr, actual_vr + v_acc)

        x_bar_list.append(x_bar)
        y_bar_list.append(y_bar)
        x_list.append(x)
        y_list.append(y)
        vr_list.append(vr)
        vl_list.append(vl)
        actual_vr_list.append(actual_vr)
        actual_vl_list.append(actual_vl)

    plt.figure(figsize=(5, 5))
    CELL_COUNT = 3
    plt.scatter(x_bar_list, y_bar_list, marker='.', linewidth=0, c='black', label='desired traj')
    plt.scatter(x_list, y_list, marker='.', linewidth=0, label='robot traj')
    plt.xlim(0, CELL_COUNT * 0.18)
    plt.ylim(0, CELL_COUNT * 0.18)
    plt.xticks(np.arange(CELL_COUNT+1)*0.18)
    plt.yticks(np.arange(CELL_COUNT+1)*0.18)
    plt.grid(True)
    plt.gca().set_axisbelow(True)
    plt.xlabel("X")
    plt.ylabel("Y")
    plt.title("LQR Trajectory Tracking")
    plt.legend(bbox_to_anchor=(1,1), loc=2)

    plt.figure()
    plt.plot(vr_list, label="vr")
    plt.plot(vl_list, label="vl")
    plt.plot(actual_vr_list, label="actual vr")
    plt.plot(actual_vl_list, label="actual vl")
    plt.legend(bbox_to_anchor=(1,1), loc=2)
```

```python
LOG_LVL=1
robot_q_0 = (0.08, 0.18, pi/2, 0.3)
traj = [(0, WayPoint(0.09, 0.18, pi/2, 0.5)), (0.5, WayPoint(0.18, 0.27, 0, 0.35))]
follow_plan(robot_q_0, traj, test_P_1, test_P_2, test_P_3)
plt.show()
```

```python
LOG_LVL=1
robot_q_0 = (0.07, 0.18, pi/2, 0.2)
traj = [(0, WayPoint(0.09, 0.18, pi/2, 0.2)), (0.5, WayPoint(0.09, 0.36, pi/2, 0.2))]
follow_plan(robot_q_0, traj, test_P_1, test_P_2, test_P_3)
plt.show()
```

# Tuning new PIDs

```python
import csv
reader = csv.reader(open("./pid_data.csv", 'r'))
setpoints = []
speeds = []
for row in reader:
    setpoints.append(float(row[0]))
    speeds.append(float(row[1]))

t = np.arange(0, len(setpoints)/100, 0.01)
plt.plot(t, setpoints, label="setpoint")
plt.plot(t, speeds, label="actual speed")
plt.xlabel("time (s)")
plt.ylabel("speed (m/s)")
plt.title("PID Performance")
plt.show()
```

```python

```

docs/Time Optimal Smartmouse Controls.ipynb


## Exercise 1 (4 points): Basic TensorFlow Computations

TensorFlow is a software library for numerical operations which in its core uses the concept of data flow graphs. In such a graph, we can define mathematical operations and let tensor objects flow through the graph to compute an output. The graph representation is especially useful to determine which parts of the code can be executed in parallel (including devices like a GPU). In TensorFlow, this is done automatically without active intervention by the user.

In this first exercise, we want to get a basic idea of how such a data flow graph works, which objects we can use and how we define the operations. For this, we use the simple example function
\begin{equation} f(x) = a(x=1)^2 + bx \end{equation}

with the constants 𝑎 = 2 and 𝑏 = 4. For the test script, please pay also attention to Table 1.

1. [Python] Our first goal is to minimize Equation 1 numerically. For this, we need to define a graph which computes 𝑓 (𝑥) and treats 𝑥 as a variable so that we can apply gradient descent to optimize it.

    a) Let’s begin with the construction phase.

        i. Define a graph for 𝑓 (𝑥). 𝑎 and 𝑏 are constants (tf.constant) and 𝑥 should be treated as a variable (tf.Variable) with the initial value 𝑥0 = 0. The latter is important for our goals since variables maintain their state (their current value) and can be changed by other operations, i.e. 𝑥0 is the starting point for gradient descent.

        ii. An optimizer is defined by an operation which changes the variables according to a target function by computing the gradients. Define this operation which applies one step of gradient descent (tf.train.GradientDescentOptimi zer) to our target function 𝑓 (𝑥). Use a learning rate of 𝜂 = 0.05.

    b) It is now time to enter the execution phase. We need an active session to perform any operations in our graph. It is common to create a session enclosed in a with block so that the session is closed automatically when leaving the block. The session object also determines the lifetime of the variables as their state is only maintained during the currently active session.

        i. Even though we specified a value for our variable 𝑥, we still need to explicitly initialize it. Since it would be a bit laborious to initialize all variables manually, there is the global initializer operation tf.global_variables_initializer() which automatically initializes all variables in the graph. Execute this operation (tf.Operation objects have a run() method which you can use).

        ii. Now, apply 30 steps of gradient descent by first executing the training operation and then retrieving the current values for 𝑓 (𝑥∗) and 𝑥∗. Show these values in each epoch. To stay efficient, you should calculate both values in the same graph run, i.e. do not use the eval() method of your tensors. If implemented correctly, the values in the last iteration should be close to the analytic solution 𝑓 (−2) = −6.

2. [Python] Let’s plot our function 𝑓 (𝑥) together with the minimum found. For this, we need
to evaluate 𝑓 (𝑥) over a predefined range. 𝑥 has now a different purpose than before: we
want to plug in arbitrary values instead of using it as a variable subject to optimization.
a) Implement Equation 1 again in a new graph (reset the default one via tf.reset_d
efault_graph()) but this time defining 𝑥 as a placeholder instead of a variable
(tf.placeholder). A placeholder is a node representing an arbitrary tensor which
is defined later in the execution phase with concrete values. In the construction
phase, they only have a type and a defined shape. If one of the shape components
is not known in advance (like here), it is possible to use None instead (the shape is
then set during the execution phase).
b) In a new session, evaluate 𝑓 (𝑥) for 50 values in the range [−6; 2]. You need the
feed_dict argument to specify the values for the placeholder 𝑥.
c) Plot 𝑓 (𝑥) in the defined range and highlight your minimum 𝑓 (𝑥∗) found previously
in the plot.

3. [Pen and Paper] We found the minimum of our function 𝑓 (𝑥) without ever specifying any gradient. This is possible because TensorFlow implements reverse-mode autodiff and we now want to go through the steps by calculating the derivative 𝑓 ′(2) = 16 via Table 2. The basis is the graph of our function as shown in Figure 1.

    a) Calculate the forward phase by computing the output of each node 𝑛𝑖 for the input 𝑥 = 2.

    b) Calculate the backward phase by computing ∂𝑓/∂𝑛𝑖, i.e. the derivative of each node 𝑛𝑖 with respect to the target function 𝑓 (𝑥). For this, you need the chain rule
\begin{equation} \frac{\partial f}{\partial n_i} = \sum_{n \in parent(n_i)} \frac{\partial f}{\partial n} \frac{\partial n}{\partial n_i}\end{equation}
where the sum iterates over all parent nodes on which the node 𝑛𝑖 depends on (e.g. parent($n_5$) = {$n_6$}). Specify also the parents for the other nodes.

4. [Pen and Paper] Which operations in the graph of Figure 1 can be executed in parallel
(theoretically)?

## Exercise 2 (6 points): Neural Network in TensorFlow [Python]

Even though TensorFlow is a library to built data flow graphs suitable for general purposes, it is mostly used to implement neural networks. In this exercise, we want to see how this works for a simple neural network consisting only of a single hidden layer. There are multiple ways of defining the architecture of a network. Here, we are building it from scratch and in later
exercises we are going to use helper functions which hide some of the logic.

We are using a small classification problem based on the wine dataset1 as an example. It classifies three different wines based on 𝑑 = 13 different (chemical) features for 𝑛 = 178 instances. The dataset has a very good class discrimination so that the neural network should not have any problems with it.

Please use the file TensorFlowNeuralNetwork.ipynb attached to this exercise as the basis for your implementation. For the test script, please pay also attention to Table 3.
1. The dataset is included in sklearn and you can load it via the load_wine function of the sklearn.datasets package.

2. We need to split the dataset so that we can use one part to train and another to evaluate our network. You can use the function sklearn.model_selection.train_test_s
plit for this job (with a test size of 30 %).

3. The feature dimensions have highly different ranges and it is hence a good idea to standardize our train and test sets (sklearn.preprocessing.StandardScaler). Make
sure you scale your test data only with the parameters extracted from the training data.

4. We have now all ingredients together to build our neural network which is also sketched in Figure 2. In the following, use named scopes whenever you think it is suited and add names to your nodes.

    a) Create the input layer by defining placeholders for the input data and the labels.
        • The input tensor should have a dimension of 𝑚 × 𝑑 whereas 𝑚 denotes the batch size and does not have to be specified yet, i.e. it can be set to None.
        • The tensor storing the class labels is a vector with a length equal to the batch size 𝑚.

    b) Define a hidden layer with 60 neurons which essentially compute the output 𝐻 = tanh(𝑋𝑊1 + 𝒃1) based on the input 𝑋, the weight matrix 𝑊1 and the bias vector 𝒃1. We want to learn 𝑊1 and 𝒃1 so they have to be defined as variables and initialized properly. 𝒃1 can be set to zero but 𝑊1 should be initialized according to a truncated normal distribution with an appropriate standard deviation

\begin{equation} \sigma = \sqrt{\frac{2}{n_{in} + n_{out}}} \end{equation}
$n_{in}$ and $n_{out}$ denote the number of input and output connections of each neuron, respectively

    c) Define the output layer in a similar way but this time using linear transfer functions2 \begin{equation}y = HW_2 + b_2\end{equation} Note: the order in which the variables are defined matters for the test script so please define first 𝑊1 and 𝒃1 before you define 𝑊2 and 𝒃2.

    d) We need an error function to train our network. Since we are dealing with a classification problem, the cross-entropy function is a good choice. You can use $tf.nn.sparse\_softmax\_cross\_entropy\_with\_logits$ for this task. The function accepts the labels and the logits directly. Do not apply the softmax function yourself or convert the labels to one-hot vectors.

    e) The cross-entropy calculates an error value for each instance. Calculate the average over the complete batch dimension (tf.reduce_mean) to arrive at a scalar value.

    f) Like in Exercise 1, we need a training operation which adjusts the variables in the graph according to our error function. Define this operation but this time using tf.train.AdamOptimizer (default parameters are fine). This is a more advanced optimization technique than classical gradient descent.

    g) It is helpful to track the error as well as the accuracy on the training set during the learning process. For this, define a tensor which holds the accuracy
\begin{equation}acc = \frac{1}{m} \sum_{i=1}^m correct_i \hspace{0.5cm} with \hspace{0.5cm} correct_i = \left\{
        \begin{array}{ll}
            1, & argmax(y_i) = T_i\\
            0, & \quad else
        \end{array}
    \right. \end{equation}
over all 𝑚 instances in the current batch. 𝒚𝑖 denotes the output vector containing the logit value for each class of the 𝑖-th instance and 𝑇𝑖 ∈ {0, 1, 2} denotes the corresponding label.

    h) TensorFlow ships with a visualization tool called TensorBoard which can be used to explore the network and its result. To use it, we need to write some data about our model to a log folder which can then later be read by TensorBoard.
```python
now = datetime.utcnow().strftime('%Y-%m-%d %H;%M;%S')
logdir = '{}/run-{}/'.format('tf_logs', now)
file_writer = tf.summary.FileWriter(logdir, tf.get_default_graph())
```
which creates a log folder and already includes basic information about the graph. The file_writer object can later be used to add more information to the log. Don’t forget to close the file_writer at the end.

5. We can now enter the execution phase to train and evaluate our network.

    a) Run an operation which initializes all variables (similar to Exercise 1).

    b) Use 50 epochs to train the network. For simplicity, you can use full batches, i.e. supply all training instances at once (𝑛 = 𝑚). After each training step (in a separate graph run), evaluate the model on the training set by calculating the loss and the accuracy.

    c) We can include the evaluation metrics to the log folder so that TensorBoard can plot the result. The error and the loss are both simple scalar values which can be added to the log via
```python
loss_summary = tf.Summary(value=[tf.Summary.Value(tag='loss', simple_value=loss_value)])
accuracy_summary = tf.Summary(value=[tf.Summary.Value(tag='accuracy', simple_value=acc_value)])
file_writer.add_summary(loss_summary, epoch)
file_writer.add_summary(accuracy_summary, epoch)
```
This first creates a binary string (optimized representation used by TensorFlow) and
then adds it to the log folder via the file_writer object.

    d) After finishing with the training, evaluate the network on the test set. For this, compute the accuracy as well as the confusion matrix. For the latter, you need to retrieve the corresponding logits and then create a vector storing the predicted labels which can be compared with the ground truth.

6. Explore your network in TensorBoard. You can open it by executing
```python
tensorboard --logdir tf_logs
```
on the command line in the working directory where the Jupyter notebook is located.


# From RankNet to LambdaRank to LambdaMART: A Revisit

## 1. Sigmoid Function & Logistic Function

A sigmoid function **having a characteristic of S-shaped curve** is defined as follows,

$$ f(x)=\frac{1}{1+\exp(-\sigma x)} $$

A logistic function is defined as,

$$ f(x)=\frac{L}{1+\exp(-k(x-x_0))} $$

Commonly, with parameters ($k=1$, $x_0=0$, $L=1$), the standard logistic function is just a sigmoid function.

```python
import matplotlib.pyplot as plt
import numpy as np

x = np.arange(-10, 10, 0.1)
f = 1. / (1. + np.exp(-x))

plt.plot(x, f)
plt.xlabel('x')
plt.ylabel('f(x)')
```

### 1.1 Gradient

The gradient of $f(x)$ w.r.t. $x$ is

$$ f'(x)=\sigma f(x)(1-f (x)) $$

## 2. RankNet

### 2.1 Essence

Cast the learning of a scoring function as a pairwise classification problem.

### 2.1 Formulation

Given two documents $d_i$ and $d_j$ that are represented as feature vectors $\mathbf{x}_i$ and $\mathbf{x}_j$, let $f$ be the ranking function, the ranking scores will be $s_i=f(\mathbf{x}_i)$ and $s_j=f(\mathbf{x}_j)$, the probability $p_{ij}$ indicating $d_i$ should be ranked higher than $d_j$ is given as

$$ p_{ij}=\frac{1}{1+\exp(-(s_i-s_j))} $$

Let $\bar{p_{ij}}$ be the known probability that $d_i$ should be ranked higher than $d_j$, we then apply the **cross entropy cost function** that penalizes the deviation of the model output probabilities from the desired probabilities,

$$ C=-\bar{p_{ij}} \log(p_{ij}) - (1-\bar{p_{ij}})\log(1-p_{ij}) $$

For a given query, let $S_{ij}\in \{-1, 0, 1\}$ be defined to be 1 if doc-i has been labeled to be more relevant than doc-j, −1 if doc-i has been labeled to be less relevant than doc-j, and 0 if they have the same label. In particular, we assume that the desired ranking is deterministically known, so that $\bar{p_{ij}}=\frac{1}{2}(1+S_{ij})$. Combining the above two equations gives

$$ C=\frac{1}{2}(1-S_{ij})(s_i-s_j) + \log(1+\exp(-(s_i-s_j))) $$

The cost is comfortingly **symmetric** (swapping $i$ and $j$ and changing the sign of $S_{ij}$ should leave the cost invariant): for $S_{ij}=1$,

$$ C=\log(1+\exp(-(s_i-s_j))) $$

while for $S_{ij}=-1$,

$$ C=\log(1+\exp(-(s_j-s_i))) $$

Note that when $s_i = s_j$, the cost is $\log2$, so the model incorporates a margin (that is, documents with different labels, but to which the model assigns the same scores, are still pushed away from each other in the ranking). Also, asymptotically, the cost becomes linear (if the scores give the wrong ranking), or zero (if they give the correct ranking).

For the gradient, essentially we have

$$\frac{\partial C}{\partial s_{i}}=\frac{1}{2}(1-S_{ij})-\frac{1}{1+e^{(s_{i}-s_{j})}}=-\frac{\partial C}{\partial s_{j}}$$

Furthermore,

\begin{equation}
\frac{\partial^{2} C}{\partial s_{i}^{2}}=\sigma (s_{j}-s_{i})(1-\sigma (s_{j}-s_{i}))
=\frac{1}{1+e^{(s_{i}-s_{j})}} (1-\frac{1}{1+e^{(s_{i}-s_{j})}})=-\frac{\partial^{2} C}{\partial s_{j}^{2}}
\end{equation}

### 2.3 Implementation
TBA

## 3. LambdaRank

### 3.1 Essence
Impose a weight on the pairwise loss in RankNet by simply multiplying the absolute change in terms of a specific evaluation metric when we swap the rank positions of the two documents within the currently predicted ranking while leaving the rank positions of other documents unchanged.

### 3.2 Formulation

The key observation of LambdaRank is that: in order to train a model, we don't need the costs themselves, and we only need the gradients of the costs w.r.t. the model scores.

We denote the gradient $\frac{\partial C}{\partial s_{i}}$ as $\lambda_{ij}$, namely

\begin{equation}
\lambda_{ij}=\frac{\partial C(s_{i}-s_{j})}{\partial s_{i}}=\frac{1}{2}(1-S_{ij})-\frac{1}{1+e^{(s_{i}-s_{j})}}
\end{equation}

LambdaRank mainly differs from RankNet in that: **modifying** the above gradient by simply multiplying by the size of change in a particular metric $|\Delta nDCG|$ (say, in term of $nDCG$) given by swapping the rank positions of $d_i$ and $d_j$ while leaving the rank positions of other documents unchanged.

### 3.3 Implementation


# 向量
> 有值和方向的量

假设在二维平面有一个点 x = 2, y = 1

\begin{equation}\vec{v} = \begin{bmatrix}2 \\ 1 \end{bmatrix}\end{equation}

```python
from IPython.core.interactiveshell import InteractiveShell
InteractiveShell.ast_node_interactivity = "last_expr"
# %matplotlib inline
import math
import numpy as np
import matplotlib.pyplot as plt
```

```python
%matplotlib inline
v = np.array([2,1])

origin = [0], [0]
plt.axhline()
plt.axvline()
plt.axis('equal')
plt.grid()
plt.ticklabel_format(style='sci', axis='both', scilimits=(0,0))
plt.quiver(*origin, *v, scale=10, color='r')
plt.show()
```

## 计算向量对应的标量

\begin{equation}\|\vec{v}\| = \sqrt{v_{1}\;^{2} + v_{2}\;^{2}}\end{equation}

\begin{equation}\|\vec{v}\| = \sqrt{2^{2} + 1^{2}}\end{equation}

\begin{equation}\|\vec{v}\| = \sqrt{4 + 1}\end{equation}

\begin{equation}\|\vec{v}\| = \sqrt{5} \approx 2.24\end{equation}

```python
vMag = math.sqrt(v[0]**2 + v[1]**2)
vMag
```

    2.23606797749979

对于n维空间的情况

\begin{equation}\|\vec{v}\| = \sqrt{v_{1}\;^{2} + v_{2}\;^{2} ... + v_{n}\;^{2}}\end{equation}

```python
vMag = np.linalg.norm(v) # 使用 norm 方法
vMag
```

    2.23606797749979

```python
help(np.linalg.norm)
```

    Help on function norm in module numpy.linalg:

    norm(x, ord=None, axis=None, keepdims=False)
        Matrix or vector norm.

        This function is able to return one of eight different matrix norms,
        or one of an infinite number of vector norms (described below), depending
        on the value of the ``ord`` parameter.

        Parameters
        ----------
        x : array_like
            Input array.  If `axis` is None, `x` must be 1-D or 2-D.
        ord : {non-zero int, inf, -inf, 'fro', 'nuc'}, optional
            Order of the norm (see table under ``Notes``). inf means numpy's
            `inf` object.
        axis : {int, 2-tuple of ints, None}, optional
            If `axis` is an integer, it specifies the axis of `x` along which to
            compute the vector norms.  If `axis` is a 2-tuple, it specifies the
            axes that hold 2-D matrices, and the matrix norms of these matrices
            are computed.  If `axis` is None then either a vector norm (when `x`
            is 1-D) or a matrix norm (when `x` is 2-D) is returned.

            .. versionadded:: 1.8.0

        keepdims : bool, optional
            If this is set to True, the axes which are normed over are left in the
            result as dimensions with size one.  With this option the result will
            broadcast correctly against the original `x`.

            .. versionadded:: 1.10.0

        Returns
        -------
        n : float or ndarray
            Norm of the matrix or vector(s).

        Notes
        -----
        For values of ``ord <= 0``, the result is, strictly speaking, not a
        mathematical 'norm', but it may still be useful for various numerical
        purposes.

        The following norms can be calculated:

        =====  ============================  ==========================
        ord    norm for matrices             norm for vectors
        =====  ============================  ==========================
        None   Frobenius norm                2-norm
        'fro'  Frobenius norm                --
        'nuc'  nuclear norm                  --
        inf    max(sum(abs(x), axis=1))      max(abs(x))
        -inf   min(sum(abs(x), axis=1))      min(abs(x))
        0      --                            sum(x != 0)
        1      max(sum(abs(x), axis=0))      as below
        -1     min(sum(abs(x), axis=0))      as below
        2      2-norm (largest sing. value)  as below
        -2     smallest singular value       as below
        other  --                            sum(abs(x)**ord)**(1./ord)
        =====  ============================  ==========================

        The Frobenius norm is given by [1]_:

            :math:`||A||_F = [\sum_{i,j} abs(a_{i,j})^2]^{1/2}`

        The nuclear norm is the sum of the singular values.

        References
        ----------
        .. [1] G. H. Golub and C. F. Van Loan, *Matrix Computations*,
               Baltimore, MD, Johns Hopkins University Press, 1985, pg. 15

        Examples
        --------
        >>> from numpy import linalg as LA
        >>> a = np.arange(9) - 4
        >>> a
        array([-4, -3, -2, ...,  2,  3,  4])
        >>> b = a.reshape((3, 3))
        >>> b
        array([[-4, -3, -2],
               [-1,  0,  1],
               [ 2,  3,  4]])

        >>> LA.norm(a)
        7.745966692414834
        >>> LA.norm(b)
        7.745966692414834
        >>> LA.norm(b, 'fro')
        7.745966692414834
        >>> LA.norm(a, np.inf)
        4.0
        >>> LA.norm(b, np.inf)
        9.0
        >>> LA.norm(a, -np.inf)
        0.0
        >>> LA.norm(b, -np.inf)
        2.0

        >>> LA.norm(a, 1)
        20.0
        >>> LA.norm(b, 1)
        7.0
        >>> LA.norm(a, -1)
        -4.6566128774142013e-010
        >>> LA.norm(b, -1)
        6.0
        >>> LA.norm(a, 2)
        7.745966692414834
        >>> LA.norm(b, 2)
        7.3484692283495345

        >>> LA.norm(a, -2)
        0.0
        >>> LA.norm(b, -2)
        1.8570331885190563e-016 # may vary
        >>> LA.norm(a, 3)
        5.8480354764257312 # may vary
        >>> LA.norm(a, -3)
        0.0

        Using the `axis` argument to compute vector norms:

        >>> c = np.array([[ 1, 2, 3],
        ...               [-1, 1, 4]])
        >>> LA.norm(c, axis=0)
        array([ 1.41421356,  2.23606798,  5.        ])
        >>> LA.norm(c, axis=1)
        array([ 3.74165739,  4.24264069])
        >>> LA.norm(c, ord=1, axis=1)
        array([ 6.,  6.])

        Using the `axis` argument to compute matrix norms:

        >>> m = np.arange(8).reshape(2,2,2)
        >>> LA.norm(m, axis=(1,2))
        array([  3.74165739,  11.22497216])
        >>> LA.norm(m[0, :, :]), LA.norm(m[1, :, :])
        (3.7416573867739413, 11.224972160321824)

### 计算向量的方向（角度）
\begin{equation}tan(\theta) = \frac{1}{2}\end{equation}

\begin{equation}\theta = tan^{-1} (0.5) \approx 26.57^{o}\end{equation}

```python
angle = math.degrees(math.atan(v[1] / v[0])) # ard (arctan) → degree
angle
```

    26.56505117707799

```python
print(math.degrees(math.atan(-2 / 1)))       # 角度应该在第二象限
```

    -63.43494882292201

> [atan2](https://docs.scipy.org/doc/numpy/reference/generated/numpy.arctan2.html?highlight=atan2)文档

```python
print(np.degrees(np.arctan2(v[1], v[0])))
s = np.array([-2,1])
print (np.degrees(np.arctan2(s[1], s[0])))
```

    26.56505117707799
    153.434948822922

## 向量加

\begin{equation}\vec{v} = \begin{bmatrix}2 \\ 1 \end{bmatrix}\end{equation}

\begin{equation}\vec{s} = \begin{bmatrix}-3 \\ 2 \end{bmatrix}\end{equation}

```python
v = np.array([2,1])
s = np.array([-3,2])

vecs = np.array([v,s])
origin = [0], [0]
plt.axis('equal')
plt.grid()
plt.ticklabel_format(style='sci', axis='both', scilimits=(0,0))
plt.quiver(*origin, vecs[:,0], vecs[:,1], color=['r', 'b'], scale=10)
plt.show()
```

\begin{equation}\vec{z} = \vec{v}+\vec{s}\end{equation}

\begin{equation}\vec{z} = \begin{bmatrix}2 \\ 1 \end{bmatrix} + \begin{bmatrix}-3 \\ 2 \end{bmatrix}\end{equation}

\begin{equation}\vec{z} = \begin{bmatrix}2 \\ 1 \end{bmatrix} + \begin{bmatrix}-3 \\ 2 \end{bmatrix} = \begin{bmatrix}-1 \\ 3 \end{bmatrix}\end{equation}

```python
z = v + s
vecs = np.array([v,s,z])
origin = [0], [0]
plt.axis('equal')
plt.grid()
plt.ticklabel_format(style='sci', axis='both', scilimits=(0,0))
plt.quiver(*origin, vecs[:,0], vecs[:,1], color=['r', 'b', 'g'], scale=10)
plt.show()
```

# 向量乘

向量乘有三种

- 标量乘
- 点积
- 叉积

## 标量乘

\begin{equation} \vec{w} = 2\vec{v}\end{equation}

\begin{equation}\vec{v} = \begin{bmatrix}2 \\ 1 \end{bmatrix}\end{equation}

\begin{equation}\vec{w} = \begin{bmatrix}2 \cdot 2 \\  2 \cdot 1 \end{bmatrix}\end{equation}

\begin{equation}\vec{w} = \begin{bmatrix}2 \cdot 2 \\  2 \cdot 1 \end{bmatrix} = \begin{bmatrix}4 \\ 2 \end{bmatrix}\end{equation}

```python
v = np.array([2,1])

w = 2 * v

origin = [0], [0]
plt.grid()
plt.ticklabel_format(style='sci', axis='both', scilimits=(0,0))
plt.quiver(*origin, *w, scale=10)
plt.show()
```

乘以一个分数就是标量除

\begin{equation}\vec{b} = \frac{\vec{v}}{2}\end{equation}

```python
b = v / 2

origin = [0], [0]
plt.axis('equal')
plt.grid()
plt.ticklabel_format(style='sci', axis='both', scilimits=(0,0))
plt.quiver(*origin, *b, scale=10)
plt.show()
```

## 点乘（内积）

- 点乘的结果是一个标量
- 叉乘的结果是一个向量

\begin{equation} \vec{v} \cdot \vec{s} = (v_{1} \cdot s_{1}) + (v_{2} \cdot s_{2}) ... + \; (v_{n} \cdot s_{n})\end{equation}

\begin{equation} \vec{v} \cdot \vec{s} = (2 \cdot -3) + (1 \cdot 2) = -6 + 2 = -4\end{equation}

```python
v = np.array([2,1])
s = np.array([-3,2])
d = np.dot(v,s)             # 方法名是 dot
print(d)
print(v @ s)                # 或者直接用 @ 操作符
```

    -4
    -4

### 用点乘计算向量的余弦

$$ \vec{v} \cdot \vec{s} = \|\vec{v} \|\|\vec{s}\| \cos (\theta) $$

$$ \cos(\theta) = \frac{\vec{v} \cdot \vec{s}}{\|\vec{v} \|\|\vec{s}\|} $$

$$ \cos(\theta) = \frac{(2 \cdot-3) + (-3 \cdot 2)}{\sqrt{2^{2} + 1^{2}} \times \sqrt{-3^{2} + 2^{2}}} $$

$$\cos(\theta) = \frac{-4}{8.0622577483}$$

$$\cos(\theta) = -0.496138938357 $$

$$\theta \approx 119.74 $$

```python
v = np.array([2,1])
s = np.array([-3,2])

vMag = np.linalg.norm(v)               # 计算标量
sMag = np.linalg.norm(s)

cos = (v @ s) / (vMag * sMag)          # 点乘除标量积是余弦

theta = math.degrees(math.acos(cos))

theta
```

    119.74488129694222

## 叉乘（外积）

\begin{equation}\vec{p} = \begin{bmatrix}2 \\ 3 \\ 1 \end{bmatrix}\;\; \vec{q} = \begin{bmatrix}1 \\ 2 \\ -2 \end{bmatrix}\end{equation}

\begin{equation}r_{1} = p_{2}q_{3} - p_{3}q_{2}\end{equation}
\begin{equation}r_{2} = p_{3}q_{1} - p_{1}q_{3}\end{equation}
\begin{equation}r_{3} = p_{1}q_{2} - p_{2}q_{1}\end{equation}

\begin{equation}\vec{r} = \vec{p} \times \vec{q} = \begin{bmatrix}(3 \cdot -2) - (1 \cdot 2) \\ (1 \cdot 1) - (2 \cdot -2) \\ (2 \cdot 2) - (3 \cdot 1) \end{bmatrix} = \begin{bmatrix}-6 - 2 \\ 1 - -4 \\ 4 - 3 \end{bmatrix} = \begin{bmatrix}-8 \\ 5 \\ 1 \end{bmatrix}\end{equation}

```python
p = np.array([2,3,1])
q = np.array([1,2,-2])
r = np.cross(p,q)
r
```

    array([-8,  5,  1])

### 理解向量外积

- 只知道定义和方法cross是不够的
- 应用场景：求共点向量的法向量
- 需要三维可视化

> 编程实现！

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

def vec3d_range(m):
    a = [(min(0,m[:,i].min()), m[:,i].max()) for i in [0, 1, 2]]
    return np.array(a) * 1.2

def vec3d_label(v):
    return '(' + ','.join(str(i) for i in v) + ')'

def vec3d_cord(m):
    return zip(*np.concatenate((np.zeros(m.shape), m), axis=1))

def vec3d_color(m):
    l = np.random.rand(*m.shape)
    return [*l, *[i for s in [*zip(l,l)] for i in s]]
```

```python
from IPython.core.interactiveshell import InteractiveShell
InteractiveShell.ast_node_interactivity = "last_expr"
```

```python
%matplotlib widget

import numpy as np
import matplotlib.pyplot as plt3d

p, q, r = [[2, 3, 1], [1, 2, -1], [-8, 5, 1]]
m = np.array([p,q,r])
ax = plt3d.axes(projection='3d')
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
rx, ry, rz = vec3d_range(m)
ax.set_xlim(rx)
ax.set_ylim(ry)
ax.set_zlim(rz)
ax.quiver(*vec3d_cord(m), arrow_length_ratio=0.1, color=vec3d_color(m))
[ax.text(*v, vec3d_label(v), tuple(v)) for v in [p, q, r]]
plt.show()
```

- 3D绘制的帮助函数

```python
def vec3d_range(m):
    a = [(min(0,m[:,i].min()), m[:,i].max()) for i in [0, 1, 2]]
    return np.array(a) * 1.2
def vec3d_label(v):
    return '(' + ','.join(str(i) for i in v) + ')'
def vec3d_cord(m):
    return zip(*np.concatenate((np.zeros(m.shape), m), axis=1))
def vec3d_color(m):
    l = np.random.rand(*m.shape)
    return [*l, *[i for s in [*zip(l,l)] for i in s]]
```

- 很多时候文档可以帮助了解接口（目的）
- 需要运行代码来深入理解实现（测试即文档）
- 经过训练，人脑也可以运行代码（仔细阅读）

# 矩阵

- 矩阵的记法

\begin{equation}A = \begin{bmatrix}
  1 & 2 & 3 \\
  4 & 5 & 6
 \end{bmatrix}
\end{equation}

- 矩阵的下标

\begin{equation}A = \begin{bmatrix}
  a_{1,1} & a_{1,2} & a_{1,3} \\
  a_{2,1} & a_{2,2} & a_{2,3}
 \end{bmatrix}
\end{equation}

```python
A = np.array([[1,2,3],
              [4,5,6]])
print(A)
M = np.matrix([[1,2,3],        # 也可以用matrix，它是array的子类
               [4,5,6]])
print(M)
```

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

### 向量加

\begin{equation}\begin{bmatrix}1 & 2 & 3 \\4 & 5 & 6\end{bmatrix}+ \begin{bmatrix}6 & 5 & 4 \\3 & 2 & 1\end{bmatrix} = \begin{bmatrix}7 & 7 & 7 \\7 & 7 & 7\end{bmatrix}\end{equation}

```python
A = np.array([[1,2,3],
              [4,5,6]])
B = np.array([[6,5,4],
              [3,2,1]])
print(A + B)                            # + 操作符
print(np.add(A, B))                     # add方法
```

    [[7 7 7]
     [7 7 7]]
    [[7 7 7]
     [7 7 7]]

### 向量减

\begin{equation}\begin{bmatrix}1 & 2 & 3 \\4 & 5 & 6\end{bmatrix}- \begin{bmatrix}6 & 5 & 4 \\3 & 2 & 1\end{bmatrix} = \begin{bmatrix}-5 & -3 & -1 \\1 & 3 & 5\end{bmatrix}\end{equation}

```python
A = np.array([[1,2,3],
              [4,5,6]])
B = np.array([[6,5,4],
              [3,2,1]])
print (A - B)                           # - 操作符
print(np.subtract(A, B))                # subtract方法
```

    [[-5 -3 -1]
     [ 1  3  5]]
    [[-5 -3 -1]
     [ 1  3  5]]

### 求负

\begin{equation}C = \begin{bmatrix}-5 & -3 & -1 \\1 & 3 & 5\end{bmatrix}\end{equation}

\begin{equation}-C = \begin{bmatrix}5 & 3 & 1 \\-1 & -3 & -5\end{bmatrix}\end{equation}

```python
C = np.array([[-5,-3,-1],
              [1,3,5]])
print(-C)                               # - 这里是一元的操作符，和减法（二元的操作符）是不同的
print(np.negative(C))                   # negative方法
```

    [[ 5  3  1]
     [-1 -3 -5]]
    [[ 5  3  1]
     [-1 -3 -5]]

### 矩阵转置
\begin{equation}\begin{bmatrix}1 & 2 & 3 \\4 & 5 & 6\end{bmatrix}^{T} = \begin{bmatrix}1 & 4\\2 & 5\\3 & 6 \end{bmatrix}\end{equation}

```python
A = np.array([[1,2,3],
              [4,5,6]])
print(A.T)                             # T 简写
print(np.transpose(A))                 # transpose方法
```

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

### 矩阵乘

简单情况：矩阵乘以标量

\begin{equation}2 \times \begin{bmatrix}1 & 2 & 3 \\4 & 5 & 6\end{bmatrix} = \begin{bmatrix}2 & 4 & 6 \\8 & 10 & 12\end{bmatrix}\end{equation}

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

    array([[ 2,  4,  6],
           [ 8, 10, 12]])

### 矩阵相乘

\begin{equation}\begin{bmatrix}1 & 2 & 3 \\4 & 5 & 6\end{bmatrix} \cdot \begin{bmatrix}9 & 8 \\ 7 & 6 \\ 5 & 4\end{bmatrix}\end{equation}

\begin{equation}(1,2,3) \cdot (9,7,5) = (1 \times 9) + (2 \times 7) + (3 \times 5) = 38\end{equation}

\begin{equation}\begin{bmatrix}38 & ?\\? & ?\end{bmatrix} \end{equation}

\begin{equation}(1,2,3) \cdot (8,6,4) = (1 \times 8) + (2 \times 6) + (3 \times 4) = 32\end{equation}

\begin{equation}\begin{bmatrix}38 & 32\\? & ?\end{bmatrix} \end{equation}

\begin{equation}(4,5,6) \cdot (9,7,5) = (4 \times 9) + (5 \times 7) + (6 \times 5) = 101\end{equation}

\begin{equation}\begin{bmatrix}38 & 32\\101 & ?\end{bmatrix} \end{equation}

\begin{equation}(4,5,6) \cdot (8,6,4) = (4 \times 8) + (5 \times 6) + (6 \times 4) = 86\end{equation}

\begin{equation}\begin{bmatrix}38 & 32\\101 & 86\end{bmatrix} \end{equation}

```python
A = np.array([[1,2,3],
              [4,5,6]])
B = np.array([[9,8],
              [7,6],
              [5,4]])
print(A.shape, B.shape)
print(np.dot(A,B))
print(A @ B)                    # @ 代表点乘（内积）
```

    (2, 3) (3, 2)
    [[ 38  32]
     [101  86]]
    [[ 38  32]
     [101  86]]

```python
A = np.matrix([[1,2,3]
               ,[4,5,6]])
B = np.matrix([[9,8],
               [7,6],
               [5,4]])
print(A * B)                                    # * 操作符也是可以的，但是注意：不要用在向量上
line = np.array(A)[0]
column = np.array(B)[:,0]
print(line, line.shape, column, column.shape)   # 取出第一行和第一列
print(line * column, line @ column)             # 对于向量，* 和 @ 完全不同
```

    [[ 38  32]
     [101  86]]
    [1 2 3] (3,) [9 7 5] (3,)
    [ 9 14 15] 38

矩阵相乘没有交换律
\begin{equation}2 \times 4 = 4 \times 2\end{equation}

\begin{equation}\begin{bmatrix}2 & 4 \\6 & 8\end{bmatrix} \cdot \begin{bmatrix}1 & 3 \\ 5 & 7\end{bmatrix} \ne \begin{bmatrix}1 & 3 \\ 5 & 7\end{bmatrix} \cdot \begin{bmatrix}2 & 4 \\6 & 8\end{bmatrix}\end{equation}

可以想像一个3x2的矩阵乘以一个2x3的矩阵

```python
A = np.array([[2,4],
              [6,8]])
B = np.array([[1,3],
              [5,7]])
print(A @ B)
print(B @ A)
```

    [[22 34]
     [46 74]]
    [[20 28]
     [52 76]]

## 单位矩阵

\begin{equation}\begin{bmatrix}1 & 0 & 0\\0 & 1 & 0\\0 & 0 & 1\end{bmatrix} \end{equation}

乘以单位矩阵还是矩阵本身

\begin{equation}\begin{bmatrix}1 & 2 & 3 \\4 & 5 & 6\\7 & 8 & 9\end{bmatrix} \cdot \begin{bmatrix}1 & 0 & 0\\0 & 1 & 0\\0 & 0 & 1\end{bmatrix} = \begin{bmatrix}1 & 2 & 3 \\4 & 5 & 6\\7 & 8 & 9\end{bmatrix} \end{equation}

```python
A = np.array([[1,2,3],
              [4,5,6],
              [7,8,9]])
B = np.array([[1,0,0],
              [0,1,0],
              [0,0,1]])
print(A @ B)
```

    [[1 2 3]
     [4 5 6]
     [7 8 9]]

## 逆矩阵

\begin{equation}A \div B = A \cdot B^{-1}\end{equation}

\begin{equation}B \cdot B^{-1} = B^{-1} \cdot B = I\end{equation}

**I** 是单位矩阵

```python
B = np.array([[6,2],
              [1,2]])

print(np.linalg.inv(B))    # 矩阵求逆
print(np.matrix(B).I)      # 简写，参考转置
```

    [[ 0.2 -0.2]
     [-0.1  0.6]]
    [[ 0.2 -0.2]
     [-0.1  0.6]]

## 解方程组

\begin{equation}2x + 4y = 18\end{equation}
\begin{equation}6x + 2y = 34\end{equation}

\begin{equation}\begin{bmatrix}2 & 4\\6 & 2\end{bmatrix} \cdot \begin{bmatrix}x\\y\end{bmatrix}=\begin{bmatrix}18\\34\end{bmatrix}\end{equation}

\begin{equation}A=\begin{bmatrix}2 & 4\\6 & 2\end{bmatrix}\;\;\;\;X=\begin{bmatrix}x\\y\end{bmatrix}\;\;\;\;B=\begin{bmatrix}18\\34\end{bmatrix}\end{equation}

\begin{equation}\begin{bmatrix}2 & 4\\6 & 2\end{bmatrix}^{-1} = \begin{bmatrix}-0.1 & 0.2\\0.3 & -0.1\end{bmatrix}\end{equation}

\begin{equation}X = \begin{bmatrix}-0.1 & 0.2\\0.3 & -0.1\end{bmatrix} \cdot \begin{bmatrix}18\\34\end{bmatrix}\end{equation}

\begin{equation}X = \begin{bmatrix}5\\2\end{bmatrix}\end{equation}

```python
A = np.array([[2,4],
              [6,2]])

B = np.array([[18],
              [34]])

C = np.linalg.inv(A) @ B

print(C)
print(np.linalg.solve(A, B))             # 调用方法solve，在Numpy介绍中提到过
```

    [[5.]
     [2.]]
    [[5.]
     [2.]]

# 变换，特征向量和特征值

## 线性变换

对矩阵 ***A*** 和向量 ***v***:

$$ A = \begin{bmatrix}2 & 3\\5 & 2\end{bmatrix} \;\;\;\; \vec{v} = \begin{bmatrix}1\\2\end{bmatrix}$$

定义 ***T*** 为:

$$ T(\vec{v}) = A\vec{v} $$

向量和矩阵的点乘是和右侧矩阵的每一列做向量点乘，最后得到一个变换后的向量

$$\begin{bmatrix}2 & 3\\5 & 2\end{bmatrix} \cdot  \begin{bmatrix}1\\2\end{bmatrix} = \begin{bmatrix}8\\9\end{bmatrix}$$

```python
v = np.array([1,2])
A = np.array([[2,3],
              [5,2]])

t = A@v
print (t)
```

    [8 9]

可见变换T是一个从二维实数向量向另一个二维实数向量的变换: ${\rm I\!R^{2} \to \rm I\!R^{2} }$

输出向量的维度可以和原向量不同，因此准确地说：${\rm I\!R^{n} \to \rm I\!R^{m}}$

比如

$$ A = \begin{bmatrix}2 & 3\\5 & 2\\1 & 1\end{bmatrix} \;\;\;\; \vec{v} = \begin{bmatrix}1\\2\end{bmatrix}$$

$$ T(\vec{v}) = A\vec{v} $$

$$\begin{bmatrix}2 & 3\\5 & 2\\1 & 1\end{bmatrix} \cdot  \begin{bmatrix}1\\2\end{bmatrix} = \begin{bmatrix}8\\9\\3\end{bmatrix}$$

所以${ T: \rm I\!R^{2} \to \rm I\!R^{3} }$

```python
v = np.array([1,2])
A = np.array([[2,3],
              [5,2],
              [1,1]])

t = A@v                   # (2) → (3)
print (t)
```

    [8 9 3]

```python
v = np.array([1,2])
A = np.array([[1,2],
              [2,1]])

t = A@v                   # (2) → (2)
print (t)
```

    [5 4]

## 向量的缩放和旋转变换

$$ A = \begin{bmatrix}2 & 0\\0 & 2\end{bmatrix} \;\;\;\; \vec{v} = \begin{bmatrix}1\\0\end{bmatrix}$$

缩放变换的例子：

\begin{equation}\begin{bmatrix}2 & 0\\0 & 2\end{bmatrix} \cdot  \begin{bmatrix}1\\0\end{bmatrix} = \begin{bmatrix}2\\0\end{bmatrix}\end{equation}

旋转90°的变换
\begin{equation}\begin{bmatrix}0 & -1\\1 & 0\end{bmatrix} \cdot  \begin{bmatrix}1\\0\end{bmatrix} = \begin{bmatrix}0\\1\end{bmatrix}\end{equation}

```python
import matplotlib.pyplot as plt
%matplotlib inline

v = np.array([1,0])
A = np.array([[0,-1],
              [1,0]])

t = A@v

vecs = np.array([v,t])
origin = [0], [0]
plt.axis('equal')
plt.grid()
plt.ticklabel_format(style='sci', axis='both', scilimits=(0,0))
plt.quiver(*origin, vecs[:,0], vecs[:,1], color=['orange', 'blue'], scale=10)
plt.show()
```

变换
\begin{equation}\begin{bmatrix}2 & 1\\1 & 2\end{bmatrix} \cdot  \begin{bmatrix}1\\0\end{bmatrix} = \begin{bmatrix}2\\1\end{bmatrix}\end{equation}

```python
v = np.array([1,0])
A = np.array([[2,1],                       # 旋转 + 缩放
              [1,2]])

t = A@v

vecs = np.array([v,t])
origin = [0], [0]
plt.axis('equal')
plt.grid()
plt.ticklabel_format(style='sci', axis='both', scilimits=(0,0))
plt.quiver(*origin, vecs[:,0], vecs[:,1], color=['orange', 'blue'], scale=10)
plt.show()
```

### 线性变换的几何解释

- 缩放: ${a \cdot I \cdot \vec{v}}$
- 旋转: ${\vec{v} \cdot T}$
- 平移: ${\vec{v} + \vec{b}}$

在二维平面上可以定义旋转矩阵${T}$为：
$$T = \begin{bmatrix}\cos{\theta} & -\sin{\theta}\\ \sin{\theta} & \cos{\theta} \end{bmatrix}$$

```python
cos, sin, pi = math.cos, math.sin, math.pi

def rotate(a):
    return np.array([[cos(a), -sin(a)],
                   [sin(a),cos(a)]])

v = np.array([1,0])
alpha = pi/5                      # 旋转36°
beta  = pi/4                      # 旋转45°
gamma = pi/3                      # 旋转60°

vecs = np.array([v, *[rotate(a)@v for a in [alpha, beta, gamma]]])
origin = [0], [0]
plt.axis('equal')
plt.grid()
plt.ticklabel_format(style='sci', axis='both', scilimits=(0,0))
plt.quiver(*origin, vecs[:,0], vecs[:,1], color=['r', 'b', 'm', 'g'], scale=10)
plt.show()
```

### 仿射变换

$$T(\vec{v}) = A\vec{v} + \vec{b}$$

例如：

\begin{equation}\begin{bmatrix}5 & 2\\3 & 1\end{bmatrix} \cdot  \begin{bmatrix}1\\1\end{bmatrix} + \begin{bmatrix}-2\\-6\end{bmatrix} = \begin{bmatrix}5\\-2\end{bmatrix}\end{equation}

```python
v = np.array([1,1])
A = np.array([[5,2],
              [3,1]])
b = np.array([-2,-6])

t = A@v + b

vecs = np.array([v,t])
origin = [0], [0]
plt.axis('equal')
plt.grid()
plt.ticklabel_format(style='sci', axis='both', scilimits=(0,0))
plt.quiver(*origin, vecs[:,0], vecs[:,1], color=['orange', 'blue'], scale=15)
plt.show()
```

## 特征向量和特征值

变换 ${\begin{bmatrix}2 & 0\\0 & 2\end{bmatrix} \cdot  \begin{bmatrix}1\\0\end{bmatrix} = \begin{bmatrix}2\\0\end{bmatrix}}$ 等价于 ${2 \times \begin{bmatrix}1\\0\end{bmatrix} = \begin{bmatrix}2\\0\end{bmatrix}}$，也可记作${ T(\vec{v}) = \lambda\vec{v}}$，其中

$$ T(\vec{v}) = A\vec{v} = \lambda\vec{v}$$

则称 ${\vec{v}}$ 和 ${\lambda}$ 是 ${A}$ 的特征向量和特征值。

假设，${A=\begin{bmatrix}2 & 0\\0 & 3\end{bmatrix}}$, 下面求 ${A}$ 的特征向量和特征值：

>[特征值](https://docs.scipy.org/doc/numpy/reference/generated/numpy.linalg.eig.html?highlight=eig#numpy.linalg.eig)文档

```python
A = np.array([[2,0],
              [0,3]])
eVals, eVecs = np.linalg.eig(A)
print(eVals)
print(eVecs)
```

    [2. 3.]
    [[1. 0.]
     [0. 1.]]

特征值和特征向量是成对出现的：
$$ \bigg( \lambda_{1} = 2, \;\; \vec{v_{1}} = \begin{bmatrix}1 \\ 0\end{bmatrix} \bigg) \;\;\; 和\;\;\; \bigg( \lambda_{2} = 3, \;\; \vec{v_{2}} = \begin{bmatrix}0 \\ 1\end{bmatrix} \bigg) $$

可以验证，特征值乘相应的特征向量等于矩阵点乘特征向量

$$ 2 \times \begin{bmatrix}1 \\ 0\end{bmatrix} = \begin{bmatrix}2 \\ 0\end{bmatrix}  \Leftrightarrow \begin{bmatrix}2 & 0\\0 & 3\end{bmatrix} \cdot \begin{bmatrix}1 \\ 0\end{bmatrix} = \begin{bmatrix}2 \\ 0\end{bmatrix} $$

以及

$$ 3 \times \begin{bmatrix}0 \\ 1\end{bmatrix} = \begin{bmatrix}0 \\ 3\end{bmatrix}  \Leftrightarrow \begin{bmatrix}2 & 0\\0 & 3\end{bmatrix} \cdot \begin{bmatrix}0 \\ 1\end{bmatrix} = \begin{bmatrix}0 \\ 3\end{bmatrix} $$

练习：

求矩阵 ${\begin{bmatrix}2 & 1\\1 & 2\end{bmatrix}}$ 的特征值和特征向量

```python
A = np.array([[2,1],
              [1,2]])

eVals, eVecs = np.linalg.eig(A)
print(eVals, eVecs)                                        # 注意取eVecs的列
[A @ eVecs[:,i] == eVals[i] * eVecs[:,i] for i in [0, 1]]  # 向量的 == 操作

```

    [3. 1.] [[ 0.70710678 -0.70710678]
     [ 0.70710678  0.70710678]]

    [array([ True,  True]), array([ True,  True])]

$$ \bigg( \lambda_{1} = 3, \;\; \vec{v_{1}} = \begin{bmatrix}0.70710678 \\ 0.70710678\end{bmatrix} \bigg) \;\;\;和\;\;\; \bigg( \lambda_{2} = 1, \;\; \vec{v_{2}} = \begin{bmatrix}-0.70710678 \\ 0.70710678\end{bmatrix} \bigg) $$

$$ 3 \times \begin{bmatrix}0.70710678 \\ 0.70710678\end{bmatrix} = \begin{bmatrix}2.12132034 \\ 2.12132034\end{bmatrix}  \Leftrightarrow \begin{bmatrix}2 & 1\\1 & 2\end{bmatrix} \cdot \begin{bmatrix}0.70710678 \\ 0.70710678\end{bmatrix} = \begin{bmatrix}2.12132034 \\ 2.12132034\end{bmatrix} $$

和

$$ 1 \times \begin{bmatrix}-0.70710678 \\ 0.70710678\end{bmatrix} = \begin{bmatrix}-0.70710678\\0.70710678\end{bmatrix}  \Leftrightarrow \begin{bmatrix}2 & 1\\1 & 2\end{bmatrix} \cdot \begin{bmatrix}-0.70710678 \\ 0.70710678\end{bmatrix} = \begin{bmatrix}-0.70710678\\0.70710678\end{bmatrix} $$

## 特征分解

从前面的例子可以推导出下面的等式：

$$A = Q \Lambda Q^{-1}$$

（可以理解为变换到特征向量定义的空间上，投影值就是特征值，然后再变换回来）

```python
A = np.array([[3,2],
              [1,0]])

l, Q = np.linalg.eig(A)
L = np.diag(l)
Qinv = np.linalg.inv(Q)
print(Q)
print(L)
print(Q)
```

    [[ 0.96276969 -0.48963374]
     [ 0.27032301  0.87192821]]
    [[ 3.56155281  0.        ]
     [ 0.         -0.56155281]]
    [[ 0.96276969 -0.48963374]
     [ 0.27032301  0.87192821]]

计算${Q}$、${\Lambda}$ 和 ${Q^{-1}}$
$$Q=\begin{bmatrix}0.96276969 & -0.48963374\\0.27032301 & 0.87192821\end{bmatrix}$$

$$\Lambda=\begin{bmatrix}3.56155281 & 0\\0 & -0.56155281\end{bmatrix}$$

$$Q^{-1}=\begin{bmatrix}0.89720673 & 0.50382896\\-0.27816009 & 0.99068183\end{bmatrix}$$

验证

${A = Q \lambda Q^{-1}}$

```python
v = np.array([1.,3.])
np.around(A@v) == np.around(Q@L@Qinv@v)
```

    array([ True,  True])

- 下面可视化变换过程

|${Q}$|${\Lambda}$|${Q^{-1}}$|
|-|-|-|
|${{\color{orange}橙} \to {\color{red}红}}$|${{\color{red}红} \to {\color{magenta}紫}}$|${{\color{magenta}紫} \to {\color{blue}蓝}}$|

```python
import numpy as np
import matplotlib.pyplot as plt
%matplotlib inline

t1 = np.linalg.inv(Q)@v
print(v.shape, t1.shape, L.shape)
t2 = L@t1
t3 = Q@t2

vecs = np.array([v, t1, t2, t3])
origin = [0], [0]
plt.axis('equal')
plt.grid()
plt.ticklabel_format(style='sci', axis='both', scilimits=(0,0))
plt.quiver(*origin, vecs[:,0], vecs[:,1], color=['orange', 'red', 'magenta', 'blue'], scale=20)
plt.show()
```

## 矩阵的秩

对于一个方形矩阵，不为0的特征值的个数。

比如
${A=\begin{bmatrix}1 & 2\\4 & 3\end{bmatrix}}$ 其特征值对角阵是 ${\Lambda=\begin{bmatrix}-1 & 0\\0 & 5\end{bmatrix}}$

所以${A}$的秩是2，和矩阵的维度一样，称为满秩矩阵。

```python
A = np.matrix([[1,2],
              [4,3]])
l, Q = np.linalg.eig(A)
L = np.diag(l)
L
```

    array([[-1.,  0.],
           [ 0.,  5.]])

又例如${B=\begin{bmatrix}3 & -3 & 6\\2 & -2 & 4\\1 & -1 & 2\end{bmatrix}}$ 的特征值对角阵是 ${\Lambda=\begin{bmatrix}3 & 0& 0\\0 & -6\times10^{-17} & 0\\0 & 0 & 3.6\times10^{-16}\end{bmatrix} \approx \begin{bmatrix}3 & 0& 0\\0 & 0 & 0\\0 & 0 & 0\end{bmatrix}}$

所以${B}$的秩是1，比矩阵的维度3小，称为欠定矩阵。

```python
B = np.array([[3,-3,6],
              [2,-2,4],
              [1,-1,2]])
lb, Qb = np.linalg.eig(B)
Lb = np.diag(lb)
Lb
```

    array([[3.00000000e+00, 0.00000000e+00, 0.00000000e+00],
           [0.00000000e+00, 5.23364153e-16, 0.00000000e+00],
           [0.00000000e+00, 0.00000000e+00, 0.00000000e+00]])

## 满秩方阵的可逆性

证明：${A^{-1} = Q \Lambda^{-1} Q^{-1}}$

$$ I = A^{-1}A $$
因为：${A=Q \Lambda Q^{-1}}$
$$ I = A^{-1}Q \Lambda Q^{-1} $$
两边同时乘：${Q\Lambda^{-1}Q^{-1}}$
$$ IQ\Lambda^{-1}Q^{-1} = A^{-1}Q \Lambda Q^{-1}Q\Lambda^{-1}Q^{-1}$$
两边同时消去单位阵
$$ Q\Lambda^{-1}Q^{-1} = A^{-1} $$

- 对角阵求逆只要把对角元素求倒数即可（矩阵除法→算术除法）
- 0没有倒数（满秩要求）
- 对于满秩方阵的求逆
  - 特征值和特征向量
  - 算术除
  - 矩阵乘

练习:

验证 ${A = \begin{bmatrix}1 & 2\\4 & 3\end{bmatrix} \Rightarrow A^{-1}=\begin{bmatrix}-0.6 & 0.4\\0.8 & -0.2\end{bmatrix}}$

```python
A = np.matrix([[1,2],
              [4,3]])
l, Q = np.linalg.eig(A)
L = np.matrix(np.diag(l))
np.around(Q@L.I@Q.I,1) == np.around(A.I,1)
```

    array([[ True,  True],
           [ True,  True]])


(c) Juan Gomez 2019. Thanks to Universidad EAFIT for support. This material is part of the course Introduction to Finite Element Analysis

# Two-dimensional frame elements

The code can now be easily extended to consider also frame elements as the one shown in the figure in its local reference system:

In this case the element has infinite axial stiffness therefore in the local system the only degrees of freedom are the transverse displacement and the rotation. The generalized displacements vector reads:

$$
u^T=\begin{bmatrix}v_1&\theta_1&v_2&\theta_2\end{bmatrix}
$$

while the vector of generalized forces (shears and moments) is:

$$
f^T=\begin{bmatrix}f_1&m_1&f_2&m_2\end{bmatrix}
$$

The force-displacement stiffness matrix in the local reference system is:

$$
$$
\begin{bmatrix}12\frac{EI}{\mathcal l^3}&6\frac{EI}{\mathcal l^3}&-12\frac{EI}{\mathcal l^3}&6\frac{EI}{\mathcal l^2}\\6\frac{EI}{\mathcal l^3}&4\frac{EI}{\mathcal l}&-6\frac{EI}{\mathcal l^2}&2\frac{EI}{\mathcal l}\\12\frac{EI}{\mathcal l^3}&-6\frac{EI}{\mathcal l^2}&12\frac{EI}{\mathcal l^3}&-6\frac{EI}{\mathcal l^2}\\6\frac{EI}{\mathcal l^2}&2\frac{EI}{\mathcal l}&-6\frac{EI}{\mathcal l^2}&4\frac{EI}{\mathcal l}\end{bmatrix}
$$
$$

To obtain the total stiffness from the structure it is now necessary to consider the stiffness contribution from all the elements in a common (**Global**) reference system and for that purpose we proceed like in the two-dimensional truss problem giving for the elemental stiffness matrix in the global reference system:

$$K=\lambda^Tk\lambda$$

where $K$ is the stiffness matrix for the two-dimensional frame element in the global reference system. It must be observed that in the global reference system the element has now three degrees of freedom per node.

In the actual implementation all the information required to compute $K$ is passed as input paramters to the elemental subroutine **uel()** as described below.

### Simple framed strucure

Cosnsider the following three-elements assemblage (The required input files are available in the files folder from the REPO):

find the lateral displacement of the structure when subject to the lateral load $P.$

**Question: Find the rotational transformation matrix $\lambda$ required for the formulation of the stiffness matrix in the global reference system.**

The modifications that must be applied to the spring-elements based code are only related to the fact that now there are 3 degrees of freedom are each nodal point.

```python
%matplotlib inline
import matplotlib.pyplot as plt
import numpy as np
import sympy as sym
```

Read the input files from the **files** folder.

```python
def readin():
    nodes    = np.loadtxt('files/' + 'Fnodes.txt', ndmin=2)
    mats     = np.loadtxt('files/' + 'Fmater.txt', ndmin=2)
    elements = np.loadtxt('files/' + 'Feles.txt' , ndmin=2)
    loads    = np.loadtxt('files/' + 'Floads.txt', ndmin=2)

    return nodes, mats, elements, loads
```

**eqcounter** counts equations and generates the boundary conditions array in its second instance.

```python
def eqcounter(nodes):
    nnodes = nodes.shape[0]
    IBC = np.zeros([nnodes, 3], dtype=np.integer)
    for i in range(nnodes):
        for k in range(3):
            IBC[i, k] = int(nodes[i, k+3])
    neq = 0
    for i in range(nnodes):
        for j in range(3):
            if IBC[i, j] == 0:
                IBC[i, j] = neq
                neq = neq + 1

    return neq, IBC
```

**DME** computes the assembly operator.

```python
def DME(nodes, elements):

    nels = elements.shape[0]
    IELCON = np.zeros([nels, 2], dtype=np.integer)
    DME = np.zeros([nels, 6], dtype=np.integer)
    neq, IBC = eqcounter(nodes)
    ndof = 6
    nnodes = 2
    for i in range(nels):
        for j in range(nnodes):
            IELCON[i, j] = elements[i, j+3]
            kk = IELCON[i, j]
            for l in range(3):
                DME[i, 3*j+l] = IBC[kk, l]
    return DME, IBC, neq
```

**assembly** uses the model and the **DME** operator to compute the global stiffness matrix.

```python
def assembly(elements, mats, nodes, neq, DME, uel=None):

    IELCON = np.zeros([2], dtype=np.integer)
    KG = np.zeros((neq, neq))
    nels = elements.shape[0]
    nnodes = 2
    ndof = 6
    for el in range(nels):
        elcoor = np.zeros([nnodes, 2])
        im = np.int(elements[el, 2])
        par0 = mats[im, 0]
        par1 = mats[im, 1]
        for j in range(nnodes):
            IELCON[j] = elements[el, j+3]
            elcoor[j, 0] = nodes[IELCON[j], 1]
            elcoor[j, 1] = nodes[IELCON[j], 2]
        kloc = uelbeam2DU(elcoor, par0, par1)
        dme = DME[el, :ndof]
        for row in range(ndof):
            glob_row = dme[row]
            if glob_row != -1:
                for col in range(ndof):
                    glob_col = dme[col]
                    if glob_col != -1:
                        KG[glob_row, glob_col] = KG[glob_row, glob_col] +\
                            kloc[row, col]

    return KG
```

**uelbeam2D** uses the nodal point coordinates and the material parameters to compute the local stiffness matrix transformed to the global reference system.

**Question: Add comments to explains the different steps in the following subroutine. In particular identify the computation of the rotational transformation matrix $\lambda$.**

```python
def uelbeam2DU(coord, I, Emod):
    """2D-2-noded beam element
       without axial deformation

    Parameters
    ----------
    coord : ndarray
      Coordinates for the nodes of the element (2, 2).
    A : float
      Cross section area.
    Emod : float
      Young modulus (>0).

    Returns
    -------
    kl : ndarray
      Local stiffness matrix for the element (4, 4).

    """
    vec = coord[1, :] - coord[0, :]
    nx = vec[0]/np.linalg.norm(vec)
    ny = vec[1]/np.linalg.norm(vec)
    L = np.linalg.norm(vec)
    Q = np.array([
        [-ny, nx,   0,  0,  0, 0],
        [0,  0, 1.0,  0,  0, 0],
        [0,  0,   0, -ny, nx, 0],
        [0,  0,  0,  0,  0, 1.0]])
    kl = (I*Emod/(L*L*L)) * np.array([
        [12.0, 6, -12.0, 6*L],
        [6,  4*L*L, -6*L, 2*L*L],
        [-12.0,  -6*L, 12.0, -6*L],
        [6*L,  2*L*L, -6*L, 4*L*L]])
    kG = np.dot(np.dot(Q.T, kl), Q)
    return kG
```

**loadassem** forms the vector of nodal loads.

```python
def loadasem(loads, IBC, neq, nl):
    """Assembles the global Right Hand Side Vector RHSG

    Parameters
    ----------
    loads : ndarray
      Array with the loads imposed in the system.
    IBC : ndarray (int)
      Array that maps the nodes with number of equations.
    neq : int
      Number of equations in the system after removing the nodes
      with imposed displacements.
    nl : int
      Number of loads.

    Returns
    -------
    RHSG : ndarray
      Array with the right hand side vector.

    """
    RHSG = np.zeros([neq])
    for i in range(nl):
        il = int(loads[i, 0])
        ilx = IBC[il, 0]
        ily = IBC[il, 1]
        ilT = IBC[il, 2]
        if ilx != -1:
            RHSG[ilx] = loads[i, 1]
        if ily != -1:
            RHSG[ily] = loads[i, 2]
        if ilT != -1:
            RHSG[ilT] = loads[i, 3]

    return RHSG
```

The main program still retains the same structure as follows:

* Reads the model
* Builds the DME() operator
* Assembles the global system of equations
* Solves for the global displacements $UG$

```python
nodes, mats, elements, loads = readin()
DME, IBC, neq = DME(nodes, elements)
KG = assembly(elements, mats, nodes, neq, DME)
RHSG = loadasem(loads, IBC, neq, 1)
UG = np.linalg.solve(KG, RHSG)
print(UG)
```

    [  2.25000000e+01   2.00000000e+01   1.27557539e-16  -1.25918779e-15
       2.00000000e+01   4.88970566e-16]

### Proposed problems
#### Problem 1
Implement a subroutine to compute the nodal forces in each element and verify the equilibrium of the system.

#### Problem 2
Find the lateral stiffness of the structure using the relation:

$$k=\frac P\delta$$

#### Problem 3
Retrofit the structure in such a way that the lateral stiffness increases by a factor of 2.0.

#### Problem 4
Fix the framed structure shown in the figure by adding elements and/or imposing appropiate displacement boundary conditions. (Note: you must create a new set of input files.)

### References

* Bathe, Klaus-Jürgen. (2006) Finite element procedures. Klaus-Jurgen Bathe. Prentice Hall International.

* Juan Gómez, Nicolás Guarín-Zapata (2018). SolidsPy: 2D-Finite Element Analysis with Python, <https://github.com/AppliedMechanics-EAFIT/SolidsPy>.

```python
from IPython.core.display import HTML
def css_styling():
    styles = open('./nb_style.css', 'r').read()
    return HTML(styles)
css_styling()
```

<link href='http://fonts.googleapis.com/css?family=Fenix' rel='stylesheet' type='text/css'>
<link href='http://fonts.googleapis.com/css?family=Alegreya+Sans:100,300,400,500,700,800,900,100italic,300italic,400italic,500italic,700italic,800italic,900italic' rel='stylesheet' type='text/css'>
<link href='http://fonts.googleapis.com/css?family=Source+Code+Pro:300,400' rel='stylesheet' type='text/css'>

```python

```