text:
# セクション一個だけのダイナミクスを導出

必要なものをインポート

```python
import sympy as sy
from sympy.printing.numpy import NumPyPrinter
from sympy import julia_code
from sympy.utilities.codegen import codegen
import tqdm
import os
from pathlib import Path

from kinematics import Local
#from kinematics_mac2 import Local

from dynamics import operate_V, operate_tilde, operate_T2
from dynamics import Dynamics

```

```python
# パラメータ
m = 0.13
r = 0.0125
Ixx = 1/4 * r**2
G_v = sy.Matrix([[0, 0, -9.81]]).T
Ki = 1700  # 弾性係数
Di = 110  # ダンピング
```

変数を宣言

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

# アクチュエータベクトルの微分
l1_dot, l2_dot, l3_dot = sy.symbols("l1_dot, l2_dot, l3_dot")
q_dot = sy.Matrix([[l1_dot, l2_dot, l3_dot]]).T

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

運動学関係

```python
kinema = Local()

P = kinema.P(q, xi)  # 位置ベクトル
R = kinema.R(q, xi)  # 回転行列

# 位置のアクチュエータ微分
dPdl1 = sy.diff(P, l1)
dPdl2 = sy.diff(P, l2)
dPdl3 = sy.diff(P, l3)

# 回転行列のアクチュエータ微分
dRdl1 = sy.diff(R, l1)
dRdl2 = sy.diff(R, l2)
dRdl3 = sy.diff(R, l3)
```

慣性行列

```python
# 回転方向の慣性行列
M_omega_1_1 = Ixx * sy.integrate(
    operate_T2(dRdl1.T * dRdl1),
    (xi, 0, 1)
)
M_omega_1_2 = Ixx * sy.integrate(
    operate_T2(dRdl1.T * dRdl2),
    (xi, 0, 1)
)
M_omega_1_3 = Ixx * sy.integrate(
    operate_T2(dRdl1.T * dRdl3),
    (xi, 0, 1)
)
M_omega_2_1 = Ixx * sy.integrate(
    operate_T2(dRdl2.T * dRdl1),
    (xi, 0, 1)
)
M_omega_2_2 = Ixx * sy.integrate(
    operate_T2(dRdl2.T * dRdl2),
    (xi, 0, 1)
)
M_omega_2_3 = Ixx * sy.integrate(
    operate_T2(dRdl2.T * dRdl3),
    (xi, 0, 1)
)
M_omega_3_1 = Ixx * sy.integrate(
    operate_T2(dRdl3.T * dRdl1),
    (xi, 0, 1)
)
M_omega_3_2 = Ixx * sy.integrate(
    operate_T2(dRdl3.T * dRdl2),
    (xi, 0, 1)
)
M_omega_3_3 = Ixx * sy.integrate(
    operate_T2(dRdl3.T * dRdl3),
    (xi, 0, 1)
)
M_omega = sy.Matrix([
    [M_omega_1_1, M_omega_1_2, M_omega_1_3],
    [M_omega_2_1, M_omega_2_2, M_omega_2_3],
    [M_omega_3_1, M_omega_3_2, M_omega_3_3],
])

```

```python
# 並進運動の回転行列
M_v_1_1 = m * sy.integrate(
    dPdl1.T * dPdl1,
    (xi, 0, 1)
)
M_v_1_2 = m * sy.integrate(
    dPdl1.T * dPdl2,
    (xi, 0, 1)
)
M_v_1_3 = m * sy.integrate(
    dPdl1.T * dPdl3,
    (xi, 0, 1)
)
M_v_2_1 = m * sy.integrate(
    dPdl2.T * dPdl1,
    (xi, 0, 1)
)
M_v_2_2 = m * sy.integrate(
    dPdl2.T * dPdl2,
    (xi, 0, 1)
)
M_v_2_3 = m * sy.integrate(
    dPdl2.T * dPdl3,
    (xi, 0, 1)
)
M_v_3_1 = m * sy.integrate(
    dPdl3.T * dPdl1,
    (xi, 0, 1)
)
M_v_3_2 = m * sy.integrate(
    dPdl3.T * dPdl2,
    (xi, 0, 1)
)
M_v_3_3 = m * sy.integrate(
    dPdl3.T * dPdl3,
    (xi, 0, 1)
)
M_v = sy.Matrix([
    [M_v_1_1, M_v_1_2, M_v_1_3],
    [M_v_2_1, M_v_2_2, M_v_2_3],
    [M_v_3_1, M_v_3_2, M_v_3_3],
])
```

```python
M = M_omega + M_v  # 慣性行列
```

遠心力とコリオリ

```python
# 慣性行列のアクチュエータ微分
M_1_1_diff_by_l1 = sy.diff(M[0, 0], l1)
M_1_1_diff_by_l2 = sy.diff(M[0, 0], l2)
M_1_1_diff_by_l3 = sy.diff(M[0, 0], l3)

M_1_2_diff_by_l1 = sy.diff(M[0, 1], l1)
M_1_2_diff_by_l2 = sy.diff(M[0, 1], l2)
M_1_2_diff_by_l3 = sy.diff(M[0, 1], l3)

M_1_3_diff_by_l1 = sy.diff(M[0, 2], l1)
M_1_3_diff_by_l2 = sy.diff(M[0, 2], l2)
M_1_3_diff_by_l3 = sy.diff(M[0, 2], l3)

M_2_1_diff_by_l1 = sy.diff(M[1, 0], l1)
M_2_1_diff_by_l2 = sy.diff(M[1, 0], l2)
M_2_1_diff_by_l3 = sy.diff(M[1, 0], l3)

M_2_2_diff_by_l1 = sy.diff(M[1, 1], l1)
M_2_2_diff_by_l2 = sy.diff(M[1, 1], l2)
M_2_2_diff_by_l3 = sy.diff(M[1, 1], l3)

M_2_3_diff_by_l1 = sy.diff(M[1, 2], l1)
M_2_3_diff_by_l2 = sy.diff(M[1, 2], l2)
M_2_3_diff_by_l3 = sy.diff(M[1, 2], l3)

M_3_1_diff_by_l1 = sy.diff(M[2, 0], l1)
M_3_1_diff_by_l2 = sy.diff(M[2, 0], l2)
M_3_1_diff_by_l3 = sy.diff(M[2, 0], l3)

M_3_2_diff_by_l1 = sy.diff(M[2, 1], l1)
M_3_2_diff_by_l2 = sy.diff(M[2, 1], l2)
M_3_2_diff_by_l3 = sy.diff(M[2, 1], l3)

M_3_3_diff_by_l1 = sy.diff(M[2, 2], l1)
M_3_3_diff_by_l2 = sy.diff(M[2, 2], l2)
M_3_3_diff_by_l3 = sy.diff(M[2, 2], l3)

M_dot = [
    [
        [M_1_1_diff_by_l1, M_1_1_diff_by_l2, M_1_1_diff_by_l3,],
        [M_1_2_diff_by_l1, M_1_2_diff_by_l2, M_1_3_diff_by_l3,],
        [M_1_3_diff_by_l1, M_1_3_diff_by_l2, M_1_3_diff_by_l3,],
    ],
    [
        [M_2_1_diff_by_l1, M_2_1_diff_by_l2, M_2_1_diff_by_l3,],
        [M_2_2_diff_by_l1, M_2_2_diff_by_l2, M_2_3_diff_by_l3,],
        [M_2_3_diff_by_l1, M_2_3_diff_by_l2, M_2_3_diff_by_l3,],
    ],
    [
        [M_3_1_diff_by_l1, M_3_1_diff_by_l2, M_3_1_diff_by_l3,],
        [M_3_2_diff_by_l1, M_3_2_diff_by_l2, M_3_3_diff_by_l3,],
        [M_3_3_diff_by_l1, M_3_3_diff_by_l2, M_3_3_diff_by_l3,],
    ],
]  # 混乱するのでまとめる
```

```python
def Gamma(i, j, k):
    """慣性行列のクリストッフェル記号"""
    return 1/2 * (M_dot[k][j][i] + M_dot[k][i][j] - M_dot[i][j][k])

C_1_1 = sum([Gamma(0, 0, i) * q_dot[i, 0] for i in range(3)])
C_1_2 = sum([Gamma(0, 1, i) * q_dot[i, 0] for i in range(3)])
C_1_3 = sum([Gamma(0, 2, i) * q_dot[i, 0] for i in range(3)])

C_2_1 = sum([Gamma(1, 0, i) * q_dot[i, 0] for i in range(3)])
C_2_2 = sum([Gamma(1, 1, i) * q_dot[i, 0] for i in range(3)])
C_2_3 = sum([Gamma(1, 2, i) * q_dot[i, 0] for i in range(3)])

C_3_1 = sum([Gamma(2, 0, i) * q_dot[i, 0] for i in range(3)])
C_3_2 = sum([Gamma(2, 1, i) * q_dot[i, 0] for i in range(3)])
C_3_3 = sum([Gamma(2, 2, i) * q_dot[i, 0] for i in range(3)])

C = sy.Matrix([
    [C_1_1, C_1_2, C_1_3,],
    [C_2_1, C_2_2, C_2_3,],
    [C_3_1, C_3_2, C_3_3,],
])  # コリオリ・遠心力
```

重力ベクトル

```python
# 線速度ヤコビアン
J_v_1 = R.T * dPdl1
J_v_2 = R.T * dPdl2
J_v_3 = R.T * dPdl3

G_1 = m * sy.integrate(
    J_v_1.T * R.T * G_v,
    (xi, 0, 1)
)
G_2 = m * sy.integrate(
    J_v_2.T * R.T * G_v,
    (xi, 0, 1)
)
G_3 = m * sy.integrate(
    J_v_3.T * R.T * G_v,
    (xi, 0, 1)
)
G = sy.Matrix([[G_1, G_2, G_3]]).T  # 重力ベクトル
```

その他

```python
K = sy.diag([Ki, Ki, Ki], unpack=True)
D = sy.diag([Di, Di, Di], unpack=True)
```

式を整理（時間かかる）

```python
M = sy.simplify(M)
C = sy.simplify(C)
G = sy.simplify(G)
```

結果をファイルに保存

```python
cwd = str(Path().resolve())
#base = cwd + "/../derived/mac2"
base = cwd + "/../derived/ikko_dake"
```

```python
# numpyスタイルの数式を生成
dir_name = base + '/eqs/numpy_style'
os.makedirs(dir_name, exist_ok=True)

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

name = dir_name + "/Phi0" + ".py"
f = open(name, 'w')
f.write(numpy_word)
f.write(NumPyPrinter().doprint(P))
f.close()

name = dir_name + "/Theta0" + ".py"
f = open(name, 'w')
f.write(numpy_word)
f.write(NumPyPrinter().doprint(R))
f.close()

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

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

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

f = open(dir_name + "/K" + ".py", 'w')
f.write(numpy_word)
f.write(NumPyPrinter().doprint(K))
f.close()

f = open(dir_name + "/D" + ".py", 'w')
f.write(numpy_word)
f.write(NumPyPrinter().doprint(D))
f.close()

```

```python
# じゅりあのコード生成
dir_name = base + '/eqs/julia_style'
os.makedirs(dir_name, exist_ok=True)

julia_word = "function f(q::Vector{T}, q_dot::Vector{T}, xi::T) where T\n    l1 = q[1]\n    l2 = q[2]\n    l3 = q[3]\n    l1_dot = q_dot[1]\n    l2_dot = q_dot[2]\n    l3_dot = q_dot[3]\n    \n    "

name = dir_name + "/Phi0" + ".jl"
f = open(name, 'w')
f.write("module " + "Phi0" + "\n")
f.write(julia_word)
f.write(julia_code(P))
f.write("\nend\nend")
f.close()

name = dir_name + "/Theta0" + ".jl"
f = open(name, 'w')
f.write("module " + "Theta0" + "\n")
f.write(julia_word)
f.write(julia_code(R))
f.write("\nend\nend")
f.close()

for i in range(3):
    for j in range(3):
        f = open(dir_name + "/M" + str(i) + "_" + str(j) + ".jl", 'w')
        f.write("module " + "M" + str(i) + "_" + str(j) + "\n")
        f.write(julia_word)
        f.write(julia_code(M[i, j]))
        f.write("\nend\nend")
        f.close()

for i in range(3):
    for j in range(3):
        f = open(dir_name + "/C" + str(i) + "_" + str(j) + ".jl", 'w')
        f.write("module " + "C" + str(i) + "_" + str(j) + "\n")
        f.write(julia_word)
        f.write(julia_code(C[i, j]))
        f.write("\nend\nend")
        f.close()

for i in range(3):
    f = open(dir_name + "/G" + str(i) + ".jl", 'w')
    f.write("module " + "G" + str(i) + "\n")
    f.write(julia_word)
    f.write(julia_code(G[i, 0]))
    f.write("\nend\nend")
    f.close()

f = open(dir_name + "/K" + ".jl", 'w')
f.write("module " + "G" + "\n")
f.write(julia_word)
f.write(julia_code(K))
f.write("\nend\nend")
f.close()

f = open(dir_name + "/D" + ".jl", 'w')
f.write("module " + "G" + "\n")
f.write(julia_word)
f.write(julia_code(D))
f.write("\nend\nend")
f.close()

```

```python
# Cコード生成
dir_name = base + '/eqs/c_src/'
os.makedirs(dir_name, exist_ok=True)

def gen_c(f, name, dir_name):
    [(c_name, c_code), (h_name, c_header)] = codegen(
        name_expr=(name, f),
        language="C",
        project= name + "project",
        to_files=False
    )

    f = open(dir_name + c_name, 'w')
    f.write(c_code)
    f.close()

    f = open(dir_name + h_name, 'w')
    f.write(c_header)
    f.close()

dir_name = base + '/eqs/c_src/Phi0/'
os.makedirs(dir_name, exist_ok=True)
gen_c(P, "Phi0", dir_name)

dir_name = base + '/eqs/c_src/Theta0/'
os.makedirs(dir_name, exist_ok=True)
gen_c(R, "Theta0", dir_name)

dir_name = base + '/eqs/c_src/M/'
os.makedirs(dir_name, exist_ok=True)
gen_c(M, "M", dir_name)

dir_name = base + '/eqs/c_src/C/'
os.makedirs(dir_name, exist_ok=True)
gen_c(C, "C", dir_name)

dir_name = base + '/eqs/c_src/G/'
os.makedirs(dir_name, exist_ok=True)
gen_c(G, "G", dir_name)
```

# 状態方程式の導出

```python
detM = M[0,0]*M[1,1]*M[2,2] + M[0,1]*M[1,2]*M[2,0] + M[0,2]*M[1,0]*M[2,1] -\
    (M[0,2]*M[1,1]*M[2,0] + M[0,1]*M[1,0]*M[2,2] + M[0,0]*M[1,2]*M[2,1])

_M = sy.Matrix([
    [
        M[1,1]*M[2,2] - M[1,2]*M[2,1],
        -(M[0,1]*M[2,2] - M[0,2]*M[2,1]),
        M[0,1]*M[1,2] - M[0,2]*M[1,1]
    ],
    [
        -(M[1,0]*M[2,2] - M[1,2]*M[2,0]),
        M[0,0]*M[2,2] - M[0,2]*M[2,0],
        -(M[0,0]*M[1,2] - M[0,2]*M[1,0])
    ],
    [
        M[1,0]*M[2,1] - M[1,1]*M[2,0],
        -(M[0,0]*M[2,1] - M[0,1]*M[2,0]),
        M[0,0]*M[1,1] - M[0,1]*M[1,0]
    ],
])

invM = _M / detM

# ヒステリシスベクトル
h1, h2, h3 = sy.symbols("h1, h2, h3")
H = sy.Matrix([[h1, h2, h3]]).T

# トルクベクトル
tau1, tau2, tau3 = sy.symbols("tau1, tau2, tau3")
tau = sy.Matrix([[tau1, tau2, tau3]]).T

q_dot_dot = invM*(-(C+D)*q_dot - K*q - G -H -tau)
alpha_h = 23.705
_fx = invM*(-(C+D)*q_dot - K*q - G + H)  # ドリフト項
fx = sy.Matrix([[
    l1_dot, l2_dot, l3_dot, _fx[0,0], _fx[1,0], _fx[2,0], alpha_h*l1, alpha_h*l2, alpha_h*l3
]]).T  # ドリフト項

X = sy.Matrix([[l1, l2, l3, l1_dot, l2_dot, l3_dot, h1, h2, h3]]).T
A = fx.jacobian(X)

```

```python
# 式整理
invM = sy.simplify(invM)
```

```python
dir_name = base + '/eqs/c_src/invM/'
os.makedirs(dir_name, exist_ok=True)

for i in range(3):
    for j in range(3):
        gen_c(invM[i,j], "invM_"+str(i)+"_"+str(j), dir_name)
```

```python
#q_dot_dot = sy.simplify(q_dot_dot)
```

```python
dir_name = base + '/eqs/c_src/q_dot_dot/'
os.makedirs(dir_name, exist_ok=True)

for i in range(3):
    gen_c(q_dot_dot[i, 0], "q_dot_dot_"+str(i), dir_name)
```

```python
#fx = sy.simplify(fx)
```

```python
dir_name = base + '/eqs/c_src/fx/'
os.makedirs(dir_name, exist_ok=True)

for i in range(9):
    gen_c(fx[i, 0], "fx_"+str(i), dir_name)
```

```python
#A = sy.simplify(A)
```

```python
dir_name = base + '/eqs/c_src/A/'
os.makedirs(dir_name, exist_ok=True)

for i in range(9):
    for j in range(9):
        gen_c(A[i, j], "A"+"_"+str(i)+"_"+str(j), dir_name)
```


text:
###### Content under Creative Commons Attribution license CC-BY 4.0, code under MIT license (c)2014 L.A. Barba, C. Cooper, G.F. Forsyth, A. Krishnan.

# Phugoid Motion

Welcome to [**"Practical Numerical Methods with Python!"**](http://openedx.seas.gwu.edu/courses/GW/MAE6286/2014_fall/about) This course is a collaborative, online, open education project, where we aim to give a foundation in scientific computing. The focus is on numerical solution of problems modeled by ordinary and partial differential equations.

This IPython Notebook introduces the problem we'll be studying in the **first module** of the course: the _phugoid model of glider flight_. We'll start with some background, explaining the physics, and working out the mathematical model.

First, we'll look at an idealized motion where there is no drag, resulting in a simple harmonic motion. We can plot some interesting trajectories that will pique your imagination. In the next notebook, you'll learn to numerically integrate the differential equation using Euler's method. But hang on ... first things first.

The term "phugoid" is used in aeronautics to refer to a motion pattern where an aircraft oscillates up and down —nose-up and climb, then nose-down and descend— around an equilibrium trajectory.  The aircraft oscillates in altitude, speed and pitch, with only small (neglected) variations in the angle of attack, as it repeatedly exchanges kinetic and potential energy.

A low-amplitude phugoid motion can be just a nuisance, as the aircraft does not exceed the stall angle of attack and nothing bad happens. But the mode can also be unstable leading to a stall or even a loop!

Look at this video showing a Cessna single-engine airplane in phugoid motion:

```
from IPython.display import YouTubeVideo
YouTubeVideo('ysdU4mnRYdM')
```

That doesn't look too good! What's happening?

It can get a lot worse when an aircraft enters one of these modes that is unstable. For example, one of [NASA's Helios Solar Powered Aircraft](http://www.nasa.gov/centers/dryden/history/pastprojects/Helios/) prototype broke up in mid air due to extreme phugoid oscillations!

Helios was a proof-of-concept solar electric-powered flying wing that broke the world altitude record for a non-rocket-powered aircraft in August 2001. But in June 26, 2003, it broke something else. The aircraft entered phugoid motion after encountering turbulence near the Hawaiian Island of Kauai. The high speed in the oscillatory movement exceeded the design limits, and it ended up wrecked in the Pacific Ocean. Luckily, the Helios was remotely operated, and nobody got hurt.

## The physics of phugoids

The phugoid oscillation has the aircraft pitching up and down, as it decelerates and accelerates. The trajectory might look like a sinusoid, as in the figure below. The assumption is that the forward velocity of the aircraft, $v$, varies in such a way that the angle of attack remains (nearly) constant, which means that we can assume a constant lift coefficient.

####Figure 1. Trajectory of an aircraft in phugoid motion.

In the descending portion of the trajectory, the aircraft's velocity increases as it proceeds from a peak to the minimum height—gaining kinetic energy at the expense of potential energy. The contrary happens in the upward segment, as its velocity decreases there.

We measure the pitch angle (between the aircraft's longitudinal axis and the horizontal) as positive when the aircraft's nose is pointing up. In the portion of the trajectory below the center-line, where it curves upwards, the pitch angle $\theta$ is increasing: $\dot{\theta}>0$. And where the trajectory curves down, the pitch angle is decreasing: $\dot{\theta}<0$, as shown in the figure.

Let's remind ourselves of the forces affecting an aircraft in a downward glide. Look at the figure below: we show the flight path, the forces on the glider (no thrust), and the _glide angle_ or flight path angle, $\gamma$, between the flight path and the horizontal.

####Figure 2. Forces on a glider.

The force of lift, $L$ —created by the airflow around the wings— is perpendicular to the trajectory, and the force of drag, $D$, is parallel to the trajectory. Both forces are expressed in terms of coefficients of lift and drag, $C_L$ and $C_D$, respectively, that depend on the wing design and _angle of attack_—the angle between the wing chord and the flight path.

If you are not familiar with airplane aerodynamics, you might be getting confused with some terms here ... and all those angles! But be patient and look things up, if you need to. We're giving you a quick summary here.

Lift and drag are proportional to a surface area, $S$, and the dynamic pressure: $1/2 \rho v^2$, where $\rho$ is the density of air, and $v$ the forward velocity of the aircraft. The equations for lift and drag are:

$$\begin{eqnarray}
L &=& C_L S \times \frac{1}{2} \rho v^2 \\
D &=& C_D S \times \frac{1}{2} \rho v^2
\end{eqnarray}$$

If the glider were in equilibrium, the forces would balance each other. We can equate the forces in the directions perpendicular and parallel to the trajectory, as follows:

$$\begin{equation}
L = W \cos \gamma \quad \text{and} \quad D = W \sin \gamma
\end{equation}$$

where $W$ repesents the weight of the glider.

In the figure, we've drawn the angle $\gamma$ as the _glide angle_, formed between the direction of motion and the horizontal. We are not bothered with the _sign_ of the angle, because we draw a free-body diagram and take the direction of the forces into account in writing our balance equations. But later on, we will need to be careful with the sign of the angles. It can cause you a real headache to keep this straight, so be patient!

It looks like we've set this up to do a little bit of mathematics. Are you ready?

But before, a short glimpse of the history.

## Lanchester's Aerodonetics

"Phugoid theory" was first described by the British engineer Frederick W. Lanchester in _"Aerodonetics"_ (1909). This book is so old that it is now in the public domain, so you can actually download [from Google Books](http://books.google.com/books?id=6hxDAAAAIAAJ&dq=%22phugoid%20theory%20deals%20with%20the%20longitudinal%20stability%22&pg=PA37#v=onepage&q=%22phugoid%20theory%20deals%20with%20the%20longitudinal%20stability%22&f=false) a PDF file of a scan, or read it online.

Lanchester defines phugoid theory as the study of longitudinal stability of a flying machine (aerodone). He first considered the simplification where drag and moment of inertia are neglected. Then he included these effects, obtaining an equation of stability. In addition to describing many experiments by himself and others, Lanchester also reports on _"numerical work ... done by the aid of an ordinary 25-cm slide rule."_ Go figure!

### Ideal case of zero drag

In this section, we follow the derivation given by Milne-Thompson (1966), which we find a little bit easier than that of the original in "Aerodonetics"!

An aircraft flying in a steady, straight horizontal flight has a lift equal to its weight. The velocity in this condition is sometimes called _trim velocity_ ("trim" is what pilots do to set the controls to just stay in a steady flight). Let's use $v_t$ for the trim velocity, and from $L=W$ deduce that:

$$\begin{equation}
W = C_L S \times\frac{1}{2} \rho v_t^2
\end{equation}$$

The weight $W$ is constant for the aircraft, but the lift at any other flight condition depends on the flight speed, $v$. We can use the expression for the weight in terms of $v_t$ to obtain the ratio $L/W$ at any other flight velocity, as follows:

$$\begin{equation}
\frac{L}{W}= \frac{v^2}{v_t^2}
\end{equation}$$

Imagine that the aircraft experienced a little upset, a wind gust, and it finds itself off the "trim" level, in a curved path with an instantaneous angle $\theta$. In the sketch below, we exaggerate the curved trajectory of flight to help you visualize what we'll do next. The angle $\theta$ (using the same name as Milne-Thompson) is between the _trajectory_ and the horizontal, positive up.

####Figure 3. Curved trajectory of the aircraft going up.

A balance of forces now has to take into account that our reference frame is moving with the aircraft, in a rotating frame: we have a centrifugal force. The balance in the direction of lift is thus:

$$\begin{equation}
L- W \cos \theta = \frac{W}{g} \frac{v^2}{R}
\end{equation}$$

where $R$ is the radius of curvature of the trajectory, and $g$ the acceleration of gravity. Recall that the centrifugal acceleration is $v^2/R$. Rearrange this by dividing the equation by the weight, and use the expression we found for $L/W$, above. The following equation results:

$$\begin{equation}
\frac{v^2}{v_t^2}-\cos \theta = \frac{v^2}{g R}
\end{equation}$$

Recall that we simplified the problem assuming that there is no friction, which means that the total energy is constant (the lift does no work). If $z$ represents the depth below a reference horizontal line, the energy per unit mass is (kinetic plus potential energy):

$$\begin{equation}
\frac{1}{2}v^2-g z = \text{constant}
\end{equation}$$

To get rid of that pesky constant, we can choose the reference horizontal line at the level that makes the constant energy equal to zero, so $v^2 = 2 g z$. That helps us re-write the phugoid equation in terms of $z$ as follows:

$$\begin{equation}
\frac{z}{z_t}-\cos \theta = \frac{2z}{R}
\end{equation}$$

Let $ds$ represent a small arc-length of the trajectory. We can write

$$\begin{equation}
\frac{1}{R} = \frac{d\theta}{ds} \quad \text{and}\quad  \sin\theta = -\frac{dz}{ds}
\end{equation}$$

Employing the chain rule of calculus,

$$\begin{equation}
\frac{1}{R} = \frac{d\theta}{ds} = \frac{dz}{ds}\frac{d\theta}{dz} = -\sin \theta\frac{d\theta}{dz}
\end{equation}$$

Multiply the phugoid equation by $\frac{1}{2\sqrt{z}}$ to get:

$$\begin{equation}
\frac{\sqrt{z}}{2z_t} - \frac{\cos\theta}{2\sqrt{z}} = \frac{\sqrt{z}}{R}
\end{equation}$$

Substituting for $1/R$ on the right hand side and bringing the cosine term over to the right, we get:

$$\begin{equation}
\frac{\sqrt{z}}{2z_t} = \frac{\cos \theta}{2 \sqrt{z}} - \sqrt{z} \sin \theta \frac{d\theta}{dz}
\end{equation}$$

The right-hand-side is an exact derivative! We can rewrite it as:

$$\begin{equation}
\frac{d}{dz} \left(\sqrt{z}\cos\theta \right) = \frac{\sqrt{z}}{2z_t}
\end{equation}$$

Integrating this equation, we add an arbitrary constant, chosen as $C\sqrt{z_t}$ which (after dividing through by $\sqrt{z}$) gives:

$$\begin{equation}
\cos \theta = \frac{1}{3}\frac{z}{z_t} + C\sqrt{\frac{z_t}{z}}
\end{equation}$$

Taking the derivative of both sides of equation (15) and applying the relations from equation (10) yields:

$$\begin{equation}
\frac{z_t}{R} = \frac{1}{3} - \frac{C}{2}\sqrt{\frac{z_t^3}{z^3}}
\end{equation}$$

Make sure you have followed the derivation, and perhaps write it out on paper!

## Phugoid Curves

Equation (15) is non-linear, which usually means we are hard-pressed to write a clean expression for the variable of interest, $z$.  In fact, Lanchester himself said that he was unable to _"reduce this expression to a form suitable for co-ordinate plotting."_ If the great polymath couldn't do it, we can't either!

But Lanchester _was_ able to plot a suitable approximation of the phugoid flight path using what he called the "trammel" method.  If you're interested in seeing how he did it, his explanation begins on page [48 of Aerodonetics](http://books.google.com/books?id=6hxDAAAAIAAJ&pg=PA49&lpg=PA48&dq=aerodonetics+the+use+of+the+trammel&source=bl&ots=lB6EVKYQuT&sig=aVE2kiDWZoWftaWczMIrcYftMOs&hl=en&sa=X&ei=gTD_U82fGYjzgwT3moGwCQ&ved=0CCAQ6AEwAA#v=onepage&q=aerodonetics%20the%20use%20of%20the%20trammel&f=false).  It's a trip.

Lanchester used Equations (15) and (16) to solve for the constant $C$ and the radius of curvature $R$ and then iteratively plotted small arcs of the phugoid path. By hand.

We wrote a neat little code that duplicates the manual trammel method, but it might be a bit much for you to absorb in the first lesson.  If you want to look it over, you are more than welcome to.  If you are just starting with Python, skip it for the moment and we'll return to it at the end of this module.

###Plotting the flight path

As we mentioned, we wrote a Python code to reproduce programmatically what Lanchester did graphically. Here's a neat feature of IPython Notebooks: you can run external programs with the magical keyword ... wait for it ... `run`. And the jargon of IPython _is_ to call this "magic." In fact, there are a bunch of [magic functions](http://ipython.org/ipython-doc/dev/interactive/tutorial.html) that you will learn about. They will make you a happy camper.

Let's do it:

```
%run phugoid.py
%matplotlib inline
```

This code cell loaded our simulated-trammel code, `phugoid.py`. The code defined a function for you in the background, called `plot_flight_path`, taking three inputs: $z_t$, $z$ and $\theta$.

Look again at Equation (15), where we take the positive square root. There are several possibilities, depending on the value that the constant $C$ takes.

* There are no physical solutions for $C>2/3$, because it would result in $\cos\theta>1$.

* If $C=2/3$, then the solution is a horizontal straight line, because $\cos\theta=1$, $\theta=0$ and $R=\infty$.

* Any value of $C$ for which $0 < C < \frac{2}{3}$ will produce "trochoidal"-like paths.  What does this look like? Let's use our custom function `plot_flight_path` to find out!

```
#zt = 64, z = 16, theta=0
plot_flight_path(64, 16, 0)
```

Cool!  Note that the plot title tells us what the calculated value of $C$ was for our input conditions.  We have a value of $C$ between $0$ and $\frac{2}{3}$ and our path is trochoidal, like we announced it would be.

* For negative values of $C$, the resultant flight path consists of a series of loops.  Let's try it out!

```
plot_flight_path(64,16,numpy.pi)
```

You can play around with the input values and see what kind of behavior results.  Just note that any value of $C > \frac{2}{3}$ will result in $\cos \theta > 1$, which doesn't exist.  Python will probably throw a few errors if you hit that condition, but just try again!

* The last case is $C = 0$.  Take another look at Equation (16) and plug in $C = 0$, what should happen? It looks like it will just reduce to

$$R = 3z_t$$

It's a constant radius of curvature!  In fact, this solution is a series of semi-circles, with a cusp between them. One way to force $C = 0$ that we can figure out from Equation (15), is to make:

$$z = 3z_t\ \ \ ,\ \ \  \theta = 0$$

```
plot_flight_path(16,48,0.)
```

That looks an awful lot like a quarter circle.  And what's the radius of the arc?  It's $$r = 48 = 3z_t.$$

We can also get a semi-circle out of our simulated trammel by changing to another configuration where $C$ is (near) zero. Here's one example:

```
plot_flight_path(64,16,numpy.pi/2)
```

That is so nice. We have reproduced the trajectories that Lanchester found more than a hundred years ago, painstakingly drawing them by hand with a contraption called a "trammel." It must have taken him days!

Here is how the different phugoid curves are drawn in von Kármán's book, _Aerodynamics_ (1957). He never says _how_ he drew them, but we're guessing by hand, also. We did pretty good!

####Figure 4. Phugoid curves in von Kármán (1957).

In the next notebook of this series, we'll look at the differential equation that arises when you consider small perturbations on the horizontal phugoid, and we'll learn to numerically integrate that to get the flight paths.

## References

1. Lanchester, F. W. _Aerodonetics_, D. van Nostrand Company: New York, 1909. On the public domain. [Get it from Google Books](http://books.google.com/books?id=6hxDAAAAIAAJ&pg=PP1#v=onepage&q&f=false).

2. Milne-Thompson, L. M. _Theoretical Aerodynamics_, Dover 2012 reprint of the revised 1966 edition. [Read on Google Books](http://books.google.com/books?id=EMfCAgAAQBAJ&lpg=PP1&pg=PP1#v=onepage&q&f=false) (see section 18.5)

3. Sinha, N. K. and Ananthkrishnan, N. _Elementary Flight Dynamics with an introduction to Bifurcation and Continuation Methods_, CRC Press, 2013. [Read on Google Books](http://books.google.com/books?id=yXL6AQAAQBAJ&lpg=PP1&pg=PP1#v=onepage&q&f=false) (see chapter 5)

4. von Kármán, T. _Aerodynamics_, Dover 2004 reprint of the 1957 2nd edition. (see pages 149–151)

## About this course

This course is a collaborative project in open education. Three professors across the world are teaching connected courses, developing and reviewing course materials, and interacting with the community of learners that follow the course online. They are:

* Lorena A. Barba, the George Washington University, United States
* Carlos Jerez, Pontificia Universidad Católica de Chile
* Ian Hawke, Southampton University, United Kingdom

---

######The cell below loads the style of this notebook.

```
# Execute this cell to load the notebook's style sheet, then ignore it
from IPython.core.display import HTML
css_file = '../../styles/numericalmoocstyle.css'
HTML(open(css_file, "r").read())
```

<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=Arvo:400,700,400italic' rel='stylesheet' type='text/css'>
<link href='http://fonts.googleapis.com/css?family=PT+Mono' rel='stylesheet' type='text/css'>
<link href='http://fonts.googleapis.com/css?family=Shadows+Into+Light' rel='stylesheet' type='text/css'>
<link href='http://fonts.googleapis.com/css?family=Nixie+One' rel='stylesheet' type='text/css'>
<style>

@font-face {
    font-family: "Computer Modern";
    src: url('http://mirrors.ctan.org/fonts/cm-unicode/fonts/otf/cmunss.otf');
}

#notebook_panel { /* main background */
    background: rgb(245,245,245);
}

div.cell { /* set cell width */
    width: 750px;
}

div #notebook { /* centre the content */
    background: #fff; /* white background for content */
    width: 1000px;
    margin: auto;
    padding-left: 0em;
}

#notebook li { /* More space between bullet points */
margin-top:0.8em;
}

/* draw border around running cells */
div.cell.border-box-sizing.code_cell.running {
    border: 1px solid #111;
}

/* Put a solid color box around each cell and its output, visually linking them*/
div.cell.code_cell {
    background-color: rgb(256,256,256);
    border-radius: 0px;
    padding: 0.5em;
    margin-left:1em;
    margin-top: 1em;
}

div.text_cell_render{
    font-family: 'Alegreya Sans' sans-serif;
    line-height: 140%;
    font-size: 125%;
    font-weight: 400;
    width:600px;
    margin-left:auto;
    margin-right:auto;
}

/* Formatting for header cells */
.text_cell_render h1 {
    font-family: 'Nixie One', serif;
    font-style:regular;
    font-weight: 400;
    font-size: 45pt;
    line-height: 100%;
    color: rgb(0,51,102);
    margin-bottom: 0.5em;
    margin-top: 0.5em;
    display: block;
}
.text_cell_render h2 {
    font-family: 'Nixie One', serif;
    font-weight: 400;
    font-size: 30pt;
    line-height: 100%;
    color: rgb(0,51,102);
    margin-bottom: 0.1em;
    margin-top: 0.3em;
    display: block;
}

.text_cell_render h3 {
    font-family: 'Nixie One', serif;
    margin-top:16px;
	font-size: 22pt;
    font-weight: 600;
    margin-bottom: 3px;
    font-style: regular;
    color: rgb(102,102,0);
}

.text_cell_render h4 {    /*Use this for captions*/
    font-family: 'Nixie One', serif;
    font-size: 14pt;
    text-align: center;
    margin-top: 0em;
    margin-bottom: 2em;
    font-style: regular;
}

.text_cell_render h5 {  /*Use this for small titles*/
    font-family: 'Nixie One', sans-serif;
    font-weight: 400;
    font-size: 16pt;
    color: rgb(163,0,0);
    font-style: italic;
    margin-bottom: .1em;
    margin-top: 0.8em;
    display: block;
}

.text_cell_render h6 { /*use this for copyright note*/
    font-family: 'PT Mono', sans-serif;
    font-weight: 300;
    font-size: 9pt;
    line-height: 100%;
    color: grey;
    margin-bottom: 1px;
    margin-top: 1px;
}

.CodeMirror{
        font-family: "PT Mono";
        font-size: 90%;
}

</style>

ext: ipynb

max_stars_repo_name: jptrinastic/numerical-mooc

max_issues_repo_name: jptrinastic/numerical-mooc

max_forks_repo_name: jptrinastic/numerical-mooc


text:
```python
from sympy import *
init_printing(use_latex=True)
```

```python
import networkx as nx
```

```python
V = range(1, 12+1)
```

```python
E = [(1,2),(2,3),(1,4),(1,6),(1,12),(2,5),(2,7),(3,8),(3,10),(4,11),(4,9),(5,6),
     (6,7),(7,8),(8,9),(9,10),(10,11),(11,12),(5,12),(5,9),(6,10),(7,11),(8,12)]
```

```python
g = nx.Graph()
g.add_nodes_from(V)
g.add_edges_from(E)
```

```python
import bokeh.plotting as plt
plt.output_notebook()

def nx_draw(g, layout=nx.circular_layout, node_color="white", text_color="black"):
    pos = layout(g)
    xs, ys = [], []
    labels = [ str(v) for v in g.nodes() ]
    vx, vy = zip(*[ pos[v] for v in g.nodes() ])
    for (a, b) in g.edges():
        x0, y0 = pos[a]
        x1, y1 = pos[b]
        xs.append([x0, x1])
        ys.append([y0, y1])
    f = plt.figure(width=300, height=300, x_axis_type=None, y_axis_type=None,
                   outline_line_color=None, toolbar_location=None)
    f.multi_line(xs, ys, line_color="black")
    f.circle(vx, vy, size=16, line_color="black", fill_color=node_color)
    f.text(vx, vy, text=labels, text_color=text_color, text_font_size="10px",
           text_align="center", text_baseline="middle")
    return f
```

```python
plt.show(nx_draw(g))
```

```python
Vx = [ var('x%d' % i) for i in V ]
Ex = [ (Vx[i-1], Vx[j-1]) for i, j in E ]
F3 = [ xi**3 - 1 for xi in Vx ]
Fg = [ xi**2 + xi*xj + xj**2 for xi, xj in Ex ]
Fx = F3 + Fg
```

```python
G = groebner(Fx, order='lex')
```

```python
colors = var('red,green,blue')
roots_of_unity = roots(Dummy()**3 - 1, multiple=True)
color_map = dict(zip(roots_of_unity, colors))
```

```python
solutions = solve(G, *Vx)
```

```python
colorings = [ [ color_map[zeta] for zeta in solution ] for solution in solutions ]
```

```python
plots = []
for i, coloring in enumerate(colorings):
    f = nx_draw(g, node_color=[ str(color) for color in coloring ], text_color="white")
    if i % 2 == 0:
        plots.append([f])
    else:
        plots[-1].append(f)
plt.show(plt.gridplot(plots, toolbar_location=None))
```

```python

```


text:
# Bunny ICP

## Notebook Setup
The following cell will install Drake, checkout the manipulation repository, and set up the path (only if necessary).
- On Google's Colaboratory, this **will take approximately two minutes** on the first time it runs (to provision the machine), but should only need to reinstall once every 12 hours.

More details are available [here](http://manipulation.mit.edu/drake.html).

```python
import importlib
import os, sys
from urllib.request import urlretrieve

if 'google.colab' in sys.modules and importlib.util.find_spec('manipulation') is None:
    urlretrieve(f"http://manipulation.csail.mit.edu/setup/setup_manipulation_colab.py",
                "setup_manipulation_colab.py")
    from setup_manipulation_colab import setup_manipulation
    setup_manipulation(manipulation_sha='c1bdae733682f8a390f848bc6cb0dbbf9ea98602', drake_version='0.25.0', drake_build='releases')

# python libraries
import numpy as np

from manipulation import running_as_notebook

# Start a single meshcat server instance to use for the remainder of this notebook.
from meshcat.servers.zmqserver import start_zmq_server_as_subprocess
server_args = []
if 'google.colab' in sys.modules:
    server_args = ['--ngrok_http_tunnel']
proc, zmq_url, web_url = start_zmq_server_as_subprocess(server_args=server_args)

import meshcat.geometry as g
import meshcat.transformations as tf

from pydrake.all import RigidTransform, RotationMatrix

import open3d as o3d
import meshcat

from IPython.display import clear_output
clear_output()

from manipulation import FindResource
```

## Problem Description
In the lecture, we learned about the Iterative Closest Point (ICP) algorithm. In this exercise, you will implement the ICP algorithm to solve the standard Stanford Bunny problem!

**These are the main steps of the exercise:**
1. Implement the ```least_squares_transform``` function to optimize transformation given correspondence
2. Implement the ```icp``` algorithm using the functions implemented above.

Let's first visualize the point clouds of Stanford bunny in meshcat!

```python
# Visualize Stanford Bunny
pcd = o3d.io.read_point_cloud(FindResource("models/bunny/bun_zipper_res2.ply"))
pointcloud_model = np.asarray(pcd.points).transpose()

# First, clean the origin a bit to define nominal pose.
X = np.array([[1., 0., 0., 0.0],
              [0., np.cos(np.pi/2), -np.sin(np.pi/2), 0.],
              [0., np.sin(np.pi/2), np.cos(np.pi/2), -0.05]])
Xtemp = RigidTransform(X)
X = np.array([[np.cos(np.pi/2), -np.sin(np.pi/2), 0, 0.],
              [np.sin(np.pi/2), np.cos(np.pi/2), 0., 0.],
              [0., 0., 1., 0.]])
X = RigidTransform(X).multiply(Xtemp)

pointcloud_model = X.multiply(pointcloud_model).T

# Next, set up a a distrubance
X = np.array([[1., 0., 0., -1e-1],
              [0., np.cos(np.pi/6), -np.sin(np.pi/6), 1e-1],
              [0., np.sin(np.pi/6), np.cos(np.pi/6), 1e-1]])
X = RigidTransform(X)
pointcloud_scene = X.multiply(pointcloud_model.T).T

def generate_color_mat(color_vec, shape):
  color_mat = np.tile(np.array(color_vec).astype(np.float32).reshape(3,1), (1, shape[1]))
  return color_mat

vis = meshcat.Visualizer(zmq_url)
vis.delete()
vis["/Background"].set_property('visible', False)
#vis["/Cameras/default/"].set_transform(tf.translation_matrix([0, 0, 1]))
vis["/Cameras/default/rotated/<object>"].set_property("zoom", 10.5)

vis["red_bunny"].set_object(g.PointCloud(pointcloud_model.T, generate_color_mat([1, 0, 0], pointcloud_model.T.shape), size=0.01))
vis["blue_bunny"].set_object(g.PointCloud(pointcloud_scene.T, generate_color_mat([0, 0, 1], pointcloud_scene.T.shape), size=0.01))
```

## Point cloud registration with known correspondences

In this section, you will follow the [derivation](http://manipulation.csail.mit.edu/pose.html#section3) to solve the optimization problem below.

\begin{align}
    \min_{p\in\mathbb{R}^3,R\in\mathbb{R}^{3\times3}} \quad & \sum_{i=1}^{N_s} \| p + R
    \hspace{.1cm} {^Op^{m_{c_i}}} - p^{s_i}\|^2, \\ s.t. \quad & RR^T =
    I, \quad \det(R)=1\end{align}

The goal is to find the transform that registers the point clouds of the model and the scene, assuming the correspondence is known.  You may refer to the implementation from [colab](https://colab.research.google.com/github/RussTedrake/manipulation/blob/master/pose.ipynb#scrollTo=AHfxMwrvb1mz) and the explanation from [textbook](http://manipulation.csail.mit.edu/pose.html#section4).

In the cell below, implement the ```least_squares_transform``` nethod.

```python
def least_squares_transform(scene, model) -> RigidTransform:
    '''
    Calculates the least-squares best-fit transform that maps corresponding
    points scene to model.
    Args:
      scene: Nx3 numpy array of corresponding points
      model: Nx3 numpy array of corresponding points
    Returns:
      X_BA: A RigidTransform object that maps point_cloud_A on to point_cloud_B
            such that
                        X_BA.multiply(model) ~= scene,
    '''
    X_BA = RigidTransform()
    ##################
    # your code here
    ##################
    return X_BA
```

## Point correspondence from closest point
The ```least_squares_transform``` function assumes that the point correspondence is known. Unfortunately, this is often not the case, so we will have to estimate the point correspondence as well. A common heuristics for estimating point correspondence is the closest point/nearest neighbor.

We have implemented the closest neighbors using [Open3d's implementation](http://www.open3d.org/docs/release/python_api/open3d.geometry.KDTreeFlann.html), which uses [k-d trees](https://en.wikipedia.org/wiki/K-d_tree).

```python
def nearest_neighbors(scene, model):
    '''
    Find the nearest (Euclidean) neighbor in model for each
    point in scene
    Args:
        scene: Nx3 numpy array of points
        model: Mx3 numpy array of points
    Returns:
        distances: (N, ) numpy array of Euclidean distances from each point in
            scene to its nearest neighbor in model.
        indices: (N, ) numpy array of the indices in model of each
            scene point's nearest neighbor - these are the c_i's
    '''
    distances = np.empty(scene.shape[0], dtype=float)
    indices = np.empty(scene.shape[0], dtype=int)

    kdtree = o3d.geometry.KDTreeFlann(model.T)
    for i in range(model.shape[0]):
        nn = kdtree.search_knn_vector_3d(scene[i,], 1)
        indices[i] = nn[1][0]
        distances[i] = np.linalg.norm(scene[i,:] - model[indices[i],:])

    return distances, indices
```

## Iterative Closest Point (ICP)
Now you should be able to register two point clouds iteratively by first finding/updating the estimate of point correspondence with ```nearest_neighbors``` and then computing the transform using ```least_squares_transform```. You may refer to the explanation from [textbook](http://manipulation.csail.mit.edu/pose.html#section4).

**In the cell below, complete the implementation of ICP algorithm using the  ```nearest_neighbors``` and ```least_squares_transform``` methods from above.**

```python
def icp(scene, model, max_iterations=20, tolerance=1e-3, anim=None):
    '''
    Perform ICP to return the correct relative transform between two set of points.
    Args:
        scene: Nx3 numpy array of points
        model: Mx3 numpy array of points
        max_iterations: max amount of iterations the algorithm can perform.
        tolerance: tolerance before the algorithm converges.
    Returns:
      X_BA: A RigidTransform object that maps point_cloud_A on to point_cloud_B
            such that
                        X_BA.multiply(model) ~= scene,
      mean_error: Mean of all pairwise distances.
      num_iters: Number of iterations it took the ICP to converge.
    '''
    X_BA = RigidTransform()

    mean_error = 0
    num_iters = 0
    prev_error = 0

    while True:
        num_iters += 1

        # your code here
        ##################

        mean_error = np.inf # Modify to add mean error.
        ##################

        if abs(mean_error - prev_error) < tolerance or num_iters >= max_iterations:
            break

        prev_error = mean_error

        if anim is not None:
          with anim.at_frame(vis, 5 * num_iters) as frame:
            frame["red_bunny"].set_transform(X_BA.GetAsMatrix4())

    return X_BA, mean_error, num_iters
```

Now you should be able to visualize the registration of the Stanford bunny! Have fun!

```python
from meshcat.animation import Animation

anim = Animation()
icp(pointcloud_scene, pointcloud_model, max_iterations=30, tolerance=1e-5, anim=anim)

vis.set_animation(anim)
vis["/Animations/default"].set_property("timeScale", 10.0)
```

## How will this notebook be Graded?##

If you are enrolled in the class, this notebook will be graded using [Gradescope](www.gradescope.com). You should have gotten the enrollement code on our announcement in Piazza.

For submission of this assignment, you must do two things.
- Download and submit the notebook `bunny_icp.ipynb` to Gradescope's notebook submission section, along with your notebook for the other problems.
- Copy and Paste your answer to the kinematic singularity problem to Gradescope's written submission section.

We will evaluate the local functions in the notebook to see if the function behaves as we have expected. For this exercise, the rubric is as follows:
- [3 pts] `least_squares_transform` must be implemented correctly.
- [3 pts] `icp` must be implemented correctly.

```python
from manipulation.exercises.pose.test_icp import TestICP
from manipulation.exercises.grader import Grader

Grader.grade_output([TestICP], [locals()], 'results.json')
Grader.print_test_results('results.json')
```

```python

```

max_stars_repo_name: JafarAbdi/manipulation

max_issues_repo_name: JafarAbdi/manipulation

max_forks_repo_name: JafarAbdi/manipulation


text:
# Sampling GFG parameters
Faces from the GFG platform can be parameterized with specific configurations of action units (AUs) and temporal/onset parameters. This notebook describes how we sample these parameters.

## 1. Sampling of AUs
In previous articles ([Jack et al., 2016](http://psycnet.apa.org/journals/xge/145/6/708.html?uid=2016-17953-001), [Delis et al., 2016](http://jov.arvojournals.org/article.aspx?articleid=2529071)), AU samples were generated by sampling a combination of AUs from a binomial distribution which was parameterized as follows:

\begin{align}
B(n, p)
\end{align}

In which $n$ refers to the number of "trials" and $p$ to the probability of "success" for a given trial. In the aforementioned articles, the authors use $n=5$ and $p=0.6$ and limit the range from $1$ to $6$. While setting the minimum to 1 makes sense (you want at least one AU to animate the face), setting the maximum to 6 is unnecessary because with $n=5$ the maximum is 5. Also, the articles mention that the median number of AUs is 3, which makes sense because the median of a binomial distribution with parameters $n$ and $p$ is $np$ ($5 \times 0.6 = 3$).

Anyway, let's acutally implement such a sampling scheme. First, let's load in the choices of AU available to us:

```python
from scipy.io import loadmat
au_labels = loadmat('../GFG_python/au_labels.mat')['au_labels'][0]
au_labels = [au[0] for au in au_labels]

print("Number of AUs: %i" % len(au_labels))
print(au_labels)
```

    Number of AUs: 42
    ['AU1', 'AU1-2', 'AU2', 'AU2L', 'AU4', 'AU5', 'AU6', 'AU6L', 'AU6R', 'AU7L', 'AU7R', 'AU9', 'AU10Open', 'AU10LOpen', 'AU10ROpen', 'AU11L', 'AU11R', 'AU12', 'AU25-12', 'AU12L', 'AU12R', 'AU13', 'AU14', 'AU14L', 'AU14R', 'AU15', 'AU16Open', 'AU17', 'AU20', 'AU20L', 'AU20R', 'AU22', 'AU23', 'AU24', 'AU25', 'AU26', 'AU27i', 'AU38', 'AU39', 'AU43', 'AU7', 'AU12-6']

Now, let's define a function that randomly samples a number of AUs, which is determined by a draw from a binomial distribution with $n=5$ and $p=0.6$:

```python
import numpy as np

def sample_AUs(au_labels, n=5, p=0.6):
    """ Draws a random sample of AUs.

    Parameters
    ----------
    au_labels : numpy array
        Array with AU-labels
    n : int
        Parameter n of binomial distribution
    p : float
        Parameter p of binomial distribution

    Returns
    -------
    these_AUs : numpy array
        Array with selected AUs.
    """
    this_n = np.random.binomial(n=5, p=0.6)
    while this_n == 0:  # Resample if number of AUs to draw is 0
        this_n = np.random.binomial(n=5, p=0.6)

    these_AUs = np.random.choice(au_labels, size=this_n, replace=False)
    return these_AUs

print("Selected AUs: %s" % (sample_AUs(au_labels),))
```

    Selected AUs: ['AU14R' 'AU6' 'AU10ROpen']

## 2. Sampling of


text:
 INF285 - Computación Científica

 Pendulum, double pendulum and chaos

  [S]cientific [C]omputing [T]eam

 Version: 1.02

## Table of Contents
* [Source](#source)
* [The Pendulum](#pendulum)
* [Double Pendulum and chaos](#doublependulum)
* [Challenge: Adding friction and forcing](#challenge)
* [Acknowledgements](#acknowledgements)

```python
from ipywidgets import interact, fixed, IntSlider, FloatSlider, Checkbox
import sympy as sym
sym.init_printing()
import numpy as np
import ipywidgets as widgets

from scipy.integrate import odeint
import matplotlib.pyplot as plt
import matplotlib.cm as cm

import matplotlib
matplotlib.rc('xtick', labelsize=20)
matplotlib.rc('ytick', labelsize=20)
from matplotlib import rc
rc('text', usetex=True)
from mpl_toolkits.mplot3d import Axes3D
from matplotlib.patches import Wedge
import matplotlib.patches as mpatches
from matplotlib.collections import PatchCollection
```

# Sources
[Back to TOC](#toc)

- See Textbook: Numerical Analysis, Timothy Sauer, 2nd Edition, page 305.
- https://en.wikipedia.org/wiki/Pendulum_(mathematics)
- https://en.wikipedia.org/wiki/Double_pendulum
- https://scienceworld.wolfram.com/physics/DoublePendulum.html
- https://demonstrations.wolfram.com/DoublePendulum/
- **World Pendulum Alliance**: http://wpa.tecnico.ulisboa.pt/~wpa.daemon/
- **World Pendulum Alliance at USM**: http://wpa.tecnico.ulisboa.pt/~wpa.daemon/hei-partners/p10-universidad-tecnica-federico-santa-maria-utfsm/

# The Pendulum
[Back to TOC](#toc)

```python
plt.figure(figsize=(8,8))
ax=plt.gca()
theta0=np.pi/4
x=np.sin(theta0)
y=-np.cos(theta0)
plt.plot([0, x],[0, y],'-k')
plt.scatter(x, y, s=200, marker='o', c='b')
plt.scatter(0, 0, s=200, marker='o', c='k')
plt.xlim([-1.5,1.5])
plt.ylim([-1.5,0.5])
plt.grid(True)
patches=[]
wedge = mpatches.Wedge((0, 0), 0.7, 270, 270+45, ec="none")
patches.append(wedge)
collection = PatchCollection(patches, cmap=plt.cm.hsv, alpha=0.3)
ax.add_collection(collection)
plt.text(0.1, -0.4, r'$\theta$', fontsize=20)
plt.text(0.8, -0.7, r'$m$', fontsize=20)
plt.text(0.35, -0.25, r'$l$', fontsize=20)
plt.show()
```

In the previous Figure we showed the components that will be included in the modeling of The Pendulum without fricction under the influence of gravity.
This means we have the pivot denoted by the black point at coordinates $(0,0)$, the mass $m$ handging from the rigid rod of length $l$, and the angl $\theta$ generated between the vertical line below the pivot and the rigid rod.

Differential equation for the evolution of the angle of a pendulum.
\begin{align*}
    \ddot{\theta}(t) &=-\dfrac{g}{l}\,\sin(\theta(t))\\
    \theta(0)        &= \theta_0\\
    \dot{\theta}(0)  &= \omega_0
\end{align*}
This is actually an Initial Value Problem, i.e. it is a differential equation that models the evolution of the angle between the vertical axis and a rigid pendulum.

## Translating the equation for the pendulum into a code:

```python
def myFirstPendulum(y,t,g,l,m):
    theta, omega = y
    return np.array([omega,-g/l*np.sin(theta)])
```

## Now solving the differential equation

```python
def solve_pendulum(N=1000, T=10,y0=np.array([3*np.pi/2, 0.0]),args=(9.8,1,1)):
    t = np.linspace(0, T, N)
    sol = odeint(myFirstPendulum, y0, t, args=args)
    return t, sol

'''def solve_pendulum0(N=1000):
    y0 = [3*np.pi/2, 0.0]
    t = np.linspace(0, 10, N)
    sol = odeint(myFirstPendulum, y0, t, args=(9.8,0.2,0.1))
    thetas = sol[:,0]
    return t, sol'''

t, sol = solve_pendulum()
thetas=sol[:,0]
omegas=sol[:,1]

plt.figure(figsize=(5,5))
plt.plot(t, thetas, 'b', label=r'$\theta$(t)')
plt.plot(t, omegas, 'g', label=r'$\omega$(t)')
plt.legend(loc='lower right',fontsize=20)
plt.xlabel('t')
plt.grid()
plt.title('What are we seeing?', fontsize=20)
plt.show()
```

## Now, this is a pendulum 'moving'! (with interact)

```python
def plot_pendulum(k,thetas,t):
    plt.figure(figsize=(10,5))

    ax1 = plt.subplot(121)
    ax2 = plt.subplot(122)

    x   = np.sin(thetas[k])
    y   = -np.cos(thetas[k])
    ax1.plot([0, x],[0, y],'-k')
    ax1.scatter(x, y, s=200, marker='o', c='b')
    ax1.set_xlim([-1.5,1.5])
    ax1.set_ylim([-1.5,1.5])
    ax1.grid(True)
    ax1.set_title("Time t= %.2f" % (t[k]), fontsize=20)

    ax2.plot(t[:k], thetas[:k], 'b', label=r'$\theta$(t)')
    ax2.legend(loc='lower right',fontsize=20)
    ax2.set_xlabel('t', fontsize=20)
    ax2.set_xlim([0,t[-1]])
    ax2.set_ylim([np.min(thetas)*0.9,np.max(thetas)*1.1])
    ax2.grid(True)

    plt.show()
```

```python
interact(plot_pendulum, k=widgets.IntSlider(min=0, max=1000, step=10, value=0), thetas=fixed(thetas), t=fixed(t))
```

    interactive(children=(IntSlider(value=0, description='k', max=1000, step=10), Output()), _dom_classes=('widget…

    <function __main__.plot_pendulum(k, thetas, t)>

## In this case we can change the initial angle $\theta_0$ and the initial angular velocity $\omega_0$. We only plot $\theta(t)$.

```python
def plot_pendulum2(k,theta0, omega0):

    plt.figure(figsize=(10,5))
    # We need to solve the whole problem again for each different initial angle,
    # since we are changing the initial condition.
    t, sol = solve_pendulum(y0=np.array([theta0, omega0]))
    thetas = sol[:,0]

    ax1 = plt.subplot(121)
    ax2 = plt.subplot(122)

    x   = np.sin(thetas[k])
    y   = -np.cos(thetas[k])
    ax1.plot([0, x],[0, y],'-k')
    ax1.scatter(x, y, s=200, marker='o', c='b')
    ax1.set_xlim([-1.5,1.5])
    ax1.set_ylim([-1.5,1.5])
    ax1.grid(True)
    ax1.set_title("Time t= %.2f" % (t[k]), fontsize=20)

    ax2.plot(t[:k], thetas[:k], 'b', label=r'$\theta$(t)')
    ax2.legend(loc='lower right',fontsize=20)
    ax2.set_xlabel('t', fontsize=20)
    ax2.set_xlim([0,t[-1]])
    ax2.set_ylim([np.min(thetas)*0.9,np.max(thetas)*1.1])
    ax2.grid(True)

    plt.show()
```

```python
interact(plot_pendulum2,k=widgets.IntSlider(min=0, max=999, step=10, value=0),
                        theta0=widgets.FloatSlider(min=0, max=2*np.pi, step=2*np.pi/50, value=3*np.pi/2),
                        omega0=widgets.FloatSlider(min=-5, max=5, step=0.01, value=0))
```

    interactive(children=(IntSlider(value=0, description='k', max=999, step=10), FloatSlider(value=4.7123889803846…

    <function __main__.plot_pendulum2(k, theta0, omega0)>

## Now, we model $M$ pendulums with different rod length $l$.

```python
def plot_pendulum3(k, theta0, omega0, M, FLAG_3D=False):
    plt.figure(figsize=(16,8))

    colors = cm.rainbow(np.linspace(0, 1, M))

    ax1 = plt.subplot(121)
    if FLAG_3D:
        ax2 = plt.subplot(122, projection='3d')
    else:
        ax2 = plt.subplot(122)

    Ls=np.linspace(0.05,0.2,10)[::-1]
    for i in np.arange(M):
        l=Ls[i]
        t, sol = solve_pendulum(y0=np.array([theta0, omega0]), args=(9.8,l,1))
        thetas = sol[:,0]

        # We are just dividing by 0.2 to scale the rods for plottting purposes.
        x   = np.sin(thetas[k])*l/0.2
        y   = -np.cos(thetas[k])*l/0.2
        ax1.plot([0, x],[0, y],'-k',alpha=0.2)
        ax1.scatter(x, y, s=200, marker='o', color=colors[i])
        ax1.set_xlim([-1.5,1.5])
        ax1.set_ylim([-1.5,1.5])
        ax1.grid(True)
        ax1.set_title("Time t= %.2f" % (t[k]), fontsize=20)

        if FLAG_3D:
            ax2.plot(t[:k], t[:k]*0+l, thetas[:k])
        else:
            ax2.plot(t[:k], thetas[:k], color=colors[i])
            ax2.set_ylim([np.min(thetas)*0.9, np.max(thetas)*1.1])
            ax2.grid(True)

    if FLAG_3D:
        ax2.view_init(elev=60,azim=35)
        ax2.set_ylabel(r'$l$', fontsize=20)
        ax2.set_zlabel(r'$\theta$', fontsize=20)
    ax2.set_xlabel(r'$t$', fontsize=20)

    plt.show()
```

```python
k_widget3      = IntSlider(min=0, max=1000-1, step=10, value=0)
theta0_widget3 = FloatSlider(min=0, max=2*np.pi, step=2*np.pi/50, value=3*np.pi/2)
omega0_widget3 = FloatSlider(min=-5, max=5, step=0.1, value=0)
M_widget3      = IntSlider(min=1, max=10, step=1, value=5)

interact(plot_pendulum3, k=k_widget3,
         theta0=theta0_widget3, omega0=omega0_widget3,
         M=M_widget3,
         FLAG_3D=Checkbox(value=False, description='Show 3D plot'))
```

    interactive(children=(IntSlider(value=0, description='k', max=999, step=10), FloatSlider(value=4.7123889803846…

    <function __main__.plot_pendulum3(k, theta0, omega0, M, FLAG_3D=False)>

# Double Pendulum and chaos
[Back to TOC](#toc)

Source: https://demonstrations.wolfram.com/DoublePendulum/ and https://scienceworld.wolfram.com/physics/DoublePendulum.html

In this case the components are pretty much the same, the main difference is that now we have a second pendulum that it attached to the mass $m_1$, Figure below for an sketch of how the second pendulum is included.

```python
plt.figure(figsize=(8,8))
ax=plt.gca()
theta0=np.pi/4
theta1=np.pi/3
x1=np.sin(theta0)
y1=-np.cos(theta0)
x2=x1+0.7*np.sin(theta1)
y2=y1-0.7*np.cos(theta1)
plt.plot([0, x1],[0, y1],'-k')
plt.plot([x1, x2],[y1, y2],'-k')
plt.scatter(x1, y1, s=200, marker='o', c='b')
plt.scatter(x2, y2, s=200, marker='o', c='r')
plt.scatter(0, 0, s=200, marker='o', c='k')
plt.xlim([-1.5,2.5])
plt.ylim([-1.5,0.5])
plt.grid(True)
patches=[]
wedge1 = mpatches.Wedge((0, 0), 0.7, 270, 270+45, ec='none')
wedge2 = mpatches.Wedge((x, y), 0.7, 270, 270+60, ec='none')
patches.append(wedge1)
patches.append(wedge2)
collection = PatchCollection(patches, cmap=plt.cm.hsv, alpha=0.3)
ax.add_collection(collection)
plt.text(0.1, -0.4, r'$\theta_1$', fontsize=20)
plt.text(x1+0.1, y1-0.3, r'$\theta_2$', fontsize=20)
plt.text(0.8, -0.7, r'$m_1$', fontsize=20)
plt.text(x2+0.1, y2, r'$m_2$', fontsize=20)
plt.text(0.35, -0.25, r'$l_1$', fontsize=20)
plt.text(0.5*(x1+x2)+0.1, 0.5*(y1+y2), r'$l_2$', fontsize=20)
plt.show()
```

The equations will be omitted but we refer to the interested reader to look at https://demonstrations.wolfram.com/DoublePendulum/ and https://scienceworld.wolfram.com/physics/DoublePendulum.html.

```python
def doublePendulum(y, t, l1, l2, m1, m2, g):
    th1  = y[0]
    th2  = y[1]
    th1p = y[2]
    th2p = y[3]
    th1pp = -((g*(2*m1 + m2)*np.sin(th1) + g*m2*np.sin(th1 - 2*th2) +
               2*m2*(l2*th2p**2 + l1*th1p**2*np.cos(th1 - th2))*np.sin(th1 - th2))
              /(2*l1*(m1 + m2 - m2*np.cos(th1 - th2)**2)))
    th2pp = (((m1 + m2)*(l1*th1p**2 + g*np.cos(th1))+
              l2*m2*th2p**2*np.cos(th1 - th2))*
             np.sin(th1 - th2))/(l2*(m1 + m2 - m2*np.cos(th1 - th2)**2))
    return np.array([th1p,th2p,th1pp,th2pp])
```

```python
def plotDoblePendulum(k=0, T=10, N=1000, th10=np.pi, ome10=2, th20=-np.pi/2, ome20=-2):
    l1 = 1
    l2 = 1
    m1 = 1
    m2 = 1
    g  = 9.8

    t  = np.linspace(0,T,N)

    y0 = np.array([th10, th20, ome10, ome20])
    sol = odeint(doublePendulum, y0, t, args=(l1, l2, m1, m2, g))

    # Plotting trajectories
    theta1 = sol[:k,0]
    theta2 = sol[:k,1]
    x1     = l1*np.sin(theta1)
    y1     = -l1*np.cos(theta1)
    x2     = x1+l2*np.sin(theta2)
    y2     = y1-l2*np.cos(theta2)

    plt.figure(figsize=(16,8))

    ax1 = plt.subplot(121)
    ax2 = plt.subplot(122)

    ax1.plot(x1,y1,'-ob', alpha=0.05)
    ax1.plot(x2,y2,'-or', alpha=0.05)

    # Plotting function values
    ax2.plot(t[:k], theta1, 'b', label=r'$\theta_1$(t)')
    ax2.plot(t[:k], theta2, 'r', label=r'$\theta_2$(t)')
    ax2.legend(loc='lower right',fontsize=20)
    ax2.set_xlabel(r'$t$', fontsize=20)
    ax2.set_xlim([0,t[-1]])
    ax2.grid(True)

    # Plotting masses
    theta1 = sol[k,0]
    theta2 = sol[k,1]
    x1     = l1*np.sin(theta1)
    y1     = -l1*np.cos(theta1)
    x2     = x1+l2*np.sin(theta2)
    y2     = y1-l2*np.cos(theta2)
    ax1.plot([0, x1],[0, y1],'-k')
    ax1.plot([x1, x2],[y1, y2],'-k')
    ax1.scatter(x1, y1, s=200, marker='o', c='b')
    ax1.scatter(x2, y2, s=200, marker='o', c='r')

    # Extra plottting configurations
    ax1.grid(True)
    ax1.set_title("Time t= %.2f" % (t[k]), fontsize=20)
    #plt.axis('equal')
    ax1.set_xlim([-2.5,2.5])
    ax1.set_ylim([-2.5,2.5])

    plt.show()

k_widget = IntSlider(min=0, max=1000-1, step=10, value=0)
```

## Running the double pendulum numerical simulation
Here, we added the trajectory of both pendulums.
In the previous case, i.e. only one pendulum, the trajectory was always within a circle, however the second pendulum does not have to follow that!

```python
interact(plotDoblePendulum,k=k_widget,T=(10,50,10), N=fixed(1000),
        th10=(-np.pi,np.pi,np.pi/8),
        ome10=(-5,5,0.1),
        th20=(-np.pi,np.pi,np.pi/8),
        ome20=(-5,5,0.1))
```

    interactive(children=(IntSlider(value=0, description='k', max=999, step=10), IntSlider(value=10, description='…

    <function __main__.plotDoblePendulum(k=0, T=10, N=1000, th10=3.141592653589793, ome10=2, th20=-1.5707963267948966, ome20=-2)>

## Showing Chaos in a double pendulum!
The main idea here is to run two simulations where the only difference between the numerical simulation is that only one initial conditions was perturbed respet to the otehr one.
This perturnation will be denoted as $\delta$ in the code.
See the next cell with comments.

```python
def solveDoblePendulum(T=10 , N=100, y0=np.array([np.pi/2,np.pi/2,2,-2]), args=(1,1,1,1,9.8)):

    l1, l2, m1, m2, g = args

    t  = np.linspace(0,T,N)

    #y0 = np.array([np.pi/2,np.pi/2,2,-2])
    sol = odeint(doublePendulum, y0, t, args=(l1, l2, m1, m2, g))
    return t, sol

def plotFastDoblePendulum(axT, axF, t, sol, T, k=0, args=(1,1,1,1,9.8), color_1st_mass='b', color_2nd_mass='r'):

    l1, l2, m1, m2, g = args

    theta1 = sol[:k,0]
    theta2 = sol[:k,1]

    # Plotting angles
    axF.plot(t[:k],theta1,'-', c=color_1st_mass)
    axF.plot(t[:k],theta2,'-', c=color_2nd_mass)
    axF.grid(True)
    axF.set_title("Time t= %.2f" % (t[k]), fontsize=20)
    axF.set_xlim([0, T])

    # Plotting Trajectories
    x1     = l1*np.sin(theta1)
    y1     = -l1*np.cos(theta1)
    x2     = x1+l2*np.sin(theta2)
    y2     = y1-l2*np.cos(theta2)
    axT.plot(x1,y1,'-o', alpha=0.05, c=color_1st_mass)
    axT.plot(x2,y2,'-o', alpha=0.05, c=color_2nd_mass)

    # Plotting masses Trajectories
    theta1 = sol[k,0]
    theta2 = sol[k,1]
    x1     = l1*np.sin(theta1)
    y1     = -l1*np.cos(theta1)
    x2     = x1+l2*np.sin(theta2)
    y2     = y1-l2*np.cos(theta2)
    axT.plot([0, x1],[0, y1],'-k')
    axT.plot([x1, x2],[y1, y2],'-k')
    axT.scatter(x1, y1, s=200, marker='o', c=color_1st_mass)
    axT.scatter(x2, y2, s=200, marker='o', c=color_2nd_mass)

    # Extra plottting configurations
    axT.grid(True)
    axT.set_title("Time t= %.2f" % (t[k]), fontsize=20)
    axT.set_xlim([-2.5,2.5])
    axT.set_ylim([-2.5,2.5])
```

Here we add the perturbation $\delta$.
Please play with the values and analyze the output!

```python
delta = -0.1

y01=np.array([np.pi/2,np.pi/2,2,-2])
y02=np.array([np.pi/2,np.pi/2,2,-2+delta])

N = 1000
T = 20
args = (1,1,1,1,9.8)
t1, sol1 = solveDoblePendulum(T=T, N=N, y0=y01, args=args)
t2, sol2 = solveDoblePendulum(T=T, N=N, y0=y02, args=args)

def plotSeveralPendulums(k=10):

    plt.figure(figsize=(15,10))
    ax1 = plt.subplot(231)
    ax2 = plt.subplot(232)
    ax3 = plt.subplot(233)
    ax4 = plt.subplot(234)
    ax5 = plt.subplot(235)
    ax6 = plt.subplot(236)

    plotFastDoblePendulum(ax1, ax4, t1, sol1, T, k, args=args)
    plotFastDoblePendulum(ax2, ax5, t1, sol1, T, k, args=args)
    plotFastDoblePendulum(ax2, ax5, t2, sol2, T, k, args=args, color_1st_mass='m', color_2nd_mass='g')
    plotFastDoblePendulum(ax3, ax6, t2, sol2, T, k, args=args, color_1st_mass='m', color_2nd_mass='g')

    min_y = min(np.min(sol1[:]),np.min(sol2[:]))
    max_y = max(np.max(sol1[:]),np.max(sol2[:]))

    ax4.set_ylim([min_y, max_y])
    ax5.set_ylim([min_y, max_y])
    ax6.set_ylim([min_y, max_y])

    plt.show()
```

Notice that the original double pendulum is on the first column of plots, the perturbed double pendulum is on the third column, and we include the data for both of the in the second (central) column of plots.

```python
interact(plotSeveralPendulums,k=(0,N-1,10))
```

    interactive(children=(IntSlider(value=10, description='k', max=999, step=10), Output()), _dom_classes=('widget…

    <function __main__.plotSeveralPendulums(k=10)>

The conclusion here is that even very small perturbation in a double pendulum show very different trajectories of the masses!
This is the beggining of chaos! See https://en.wikipedia.org/wiki/Chaos_theory

# Challenge: Adding friction and forcing
[Back to toc](#toc)

Add friction and a forcing term to the pendulums!
1. The current model of the pendulums generate pendulums that will not stop since they don't loss energy, however a real life pendulum will lose energy.
2. Also, one could have a pendulum inmerse in a medium where there exists external forces, such as wind, etc. How woul you include this?
3. Last and not least, one could consider in having a heavy pendulum hanging from a more or less long rod. Morever, consider that pendulum is perturbed a little bit from the steady state. How can you use the knowledge adquire is this jupyter notebook to measure the gravity constant from real data obtained from a real pendulum?

# Acknowledgements
[Back to toc](#toc)

* _Material originally created by professor Claudio Torres_ (`ctorres@inf.utfsm.cl`) _. DI UTFSM. October 2019._
* _Update July 2021 - v1.01 - C.Torres_ : Adding double pendulum.
* _Update July 2021 - v1.02 - C.Torres_ : Adding reference to World Pendulum Alliance.

```python

```

max_stars_repo_path: SC1v2/Bonus - 11 - Pendulum, double pendulum and chaos.ipynb


text:
```python
from IPython.display import Image
from IPython.core.display import HTML
from sympy import *; x,h,y,t,w = symbols("x h y t w"); A = Function("A")
Image(url= "https://i.imgur.com/auyZwus.png")
```

```python
L  = Function("L")
f = Function("f")
x,a = symbols('x a')
L(x)
```

$\displaystyle L{\left(x \right)}$

```python
Eq(L(x) , f(a) + diff(f(a),a)*(x-a))
```

$\displaystyle L{\left(x \right)} = \left(- a + x\right) \frac{d}{d a} f{\left(a \right)} + f{\left(a \right)}$

```python
expr = x**4 - x**2 +2
Eq(f(x), expr)
```

$\displaystyle f{\left(x \right)} = x^{4} - x^{2} + 2$

```python
x1 = 1
Eq(f(x), expr).subs(x,x1)
```

$\displaystyle f{\left(1 \right)} = 2$

```python
fa = 2        # f(a)
f = lambdify(x, expr, "math")
df = diff(f(x),x)
```

```python
Eq(Derivative(f(x)),df).subs(x,x1)
```

$\displaystyle \left. \frac{d}{d x} \left(x^{4} - x^{2} + 2\right) \right|_{\substack{ x=1 }} = 2$

```python
dfa= df.subs(x,x1)
Eq(L(x) , fa+ dfa*(x-x1))
```

$\displaystyle L{\left(x \right)} = 2 x$

```python
Image(url= "https://i.imgur.com/dYuwIib.png")
```


text:
```python
from statsmodels.tsa.seasonal import seasonal_decompose
import matplotlib.pyplot as plt
import scipy.stats as st
import seaborn as sns
import pandas as pd
from scipy.stats import norm, uniform, expon, t, probplot
import scipy.stats as st
from scipy.integrate import quad
from sympy.solvers import solve
from sympy import Symbol
import numpy as np
from pandas import Series, DataFrame
from statsmodels.graphics.tsaplots import plot_acf
from statsmodels.tsa.stattools import acf
from pandas.plotting import lag_plot
from statsmodels.tsa.arima_process import ArmaProcess
```

```python
np.abs(np.roots([0.1, -0.5, 0.5, 1]))
```

    array([3.25642854, 3.25642854, 0.9430113 ])

```python
ar1 = np.array([1, -.5, .5, .1])
simulated_data = ArmaProcess(ar1, ma=[1]).generate_sample(nsample=200)
plt.plot(simulated_data)
plt.xlabel("Zeit")
plt.ylabel("Wert")
plt.title("AR(3) Prozess")
plt.show()
```

```python
lag = 25
acf_theor =ArmaProcess(ar = [1, -.5, .5, .1], ma=[1]).acf(lag)
x = np.arange(lag)
plt.bar(x, acf_theor , width=.2, color="black")
plt.plot(x, acf_theor, "ro", markerfacecolor="C1")
plt.xlabel("Index")
plt.ylabel("ACF")
plt.title("ACF eines AR(3) Prozesses")
plt.show()
```

```python
import statsmodels.api as sm
dta = sm.datasets.sunspots.load_pandas().data
dta.index = pd.Index(sm.tsa.datetools.dates_from_range('1700', '2008'))
del dta["YEAR"]
dta.plot()
plt.show()
```

```python
dta_sq = (np.sqrt(dta) - 1)
dta_sq.plot()
plt.xlabel("Jahre")
plt.ylabel("Wurzel der Sonnenflecken")
plt.show()
```

```python
from statsmodels.graphics.tsaplots import plot_pacf, plot_acf
fig = plt.figure()
ax1 = fig.add_subplot(311)
plot_acf(dta_sq,lags=50)
plot_pacf(dta_sq,lags=50)
plot_pacf(dta,lags=50)
plt.show()
```

```python
from statsmodels.tsa.arima_model import ARMA
model = ARMA(dta_sq, order=(9,0)).fit()
```

    C:\Users\flori\Anaconda3\lib\site-packages\statsmodels\tsa\base\tsa_model.py:165: ValueWarning: No frequency information was provided, so inferred frequency A-DEC will be used.
      % freq, ValueWarning)

```python
plt.plot(dta_sq)
plt.plot(dta_sq["SUNACTIVITY"] - model.resid)
plt.show()
```

```python
from statsmodels.graphics.api import qqplot
fig = plt.figure()
ax1 = fig.add_subplot(121)
model.resid.hist(edgecolor="black", bins=100, ax=ax1)
ax2 = fig.add_subplot(122)
qqplot(model.resid, line="q", ax=ax2)
plt.show()
```

```python
from pandas import DataFrame
np.random.seed(4)
ar1 = np.array([1, -.7])
sim = DataFrame(ArmaProcess(ar1, ma=[1]).generate_sample(nsample=150))
mod = ARMA(sim, order=(1,0)).fit()
fig, ax = plt.subplots(figsize=(12, 8))
ax = sim.loc[0:].plot(ax=ax)
fig = mod.plot_predict(130, 200, dynamic=True, ax=ax, plot_insample=True)
plt.show()
```

```python
fig, ax = plt.subplots()
ax = dta_sq.loc['1950':].plot(ax=ax)
fig = model.plot_predict('1990', '2014', dynamic=True, ax=ax, plot_insample=True)
plt.show()
```

```python

```

max_stars_repo_name: florianbaer/STAT

max_issues_repo_name: florianbaer/STAT

max_forks_repo_name: florianbaer/STAT


text:
MCMC-basics
===========

Step One : Monte-Carlo (25 minutes)
-----------------------------------

#### Motivating example : which (somehow low dimension) unit d-ball has the largest volume?

### Motivating example:

* Given an integer $d$ smaller than $15$, we are interested in the volume of the unit ball of dimension $d$.

* Intuition:
    * the circle ($d=2$) has a volume of $\pi$.
    * the sphere ($d=3$) has a volume of $\frac{4}{3}\pi$.
    * does it keep growing with $d$?

* if you want an exact value of the volume of the unit n-ball, it is: $$V_{n}={\frac {\pi ^{\frac {n}{2}}}{\Gamma \left({\frac {n}{2}}+1\right)}}$$ where $\Gamma$ is Euler function - just to say that the aim here is not to get the real value but to have fun with Monte-Carlo Methods.

Monte-Carlo (MC) methods:
-------------------------

* used to estimate an expectation.
* based upon an application of the Strong Law of Large Numbers for estimation ...
* and on the Limit Central Theorem for error control.
* Several variance-reduction techniques are available in the literature.

Notations:
----------

* Let us have $E\subset\mathbb{R}^{d}\rightarrow\mathbb{R}$
* and some function  $\varphi(.)$ from $E$
* and finally $\mathbf{X}$ is a $d-$dimensional random vector (with known
density $f_{\mathbf{X}}$).

* meaning we are restricted to a 1-dimensional output

These methods:
* allow the estimation of an expectation of form:

\begin{equation}
I=\mathbb{E}[\varphi(\mathbf{X})]
\end{equation}

* or on the integral form:

\begin{equation}
I=\int_{E}\varphi(\mathbf{x})f_{\mathbf{X}}(\mathbf{x})d\mathbf{x}
\end{equation}

* (these are the same things)
* (given that the integral exists)

Crude MC:
---------

* The main idea of this method is to generate a large number of *i.i.d.*
vectors with density $f_{\mathbf{X}}$, then to estimate $I$ with
the empirical mean of the $N$ values.

* The Strong Law of Large Numbers allows to get an unbiased estimator of $I$:

\begin{equation}
\hat{I}_N=\frac{1}{N}\sum_{i=1}^{N}\varphi(\mathbf{X}^{i})
\end{equation}

 * where $\mathbf{X}^{i}$ are i.i.d with $f_{\mathbf{X}}$.
 * $\hat{I}_N$ is a r.v. with expectation $I$.

 * $\hat{I}_N$ will sometimes represent the realization of this estimator given a sample $\mathbf{x}^{(i..N)}$, it can lead to some confusion.
 * should it be $\hat{I}_{N,n}$? (open question on notation) ...

### Example of Crude MC: estimating $\pi$

* We take E the unit square:
\begin{equation}
E = [-1,1]^{2}
\end{equation}

* We define the function $\varphi$ as "is my point in the unit circle"?
$$\varphi(\mathbf{x}) = 1_{\lVert \mathbf{x} \lVert_2 \leq 1}$$

* we set  $\mathbf{X} \sim \mathcal{U} [-1,1]^2$ so that
$$
\begin{aligned}
f_{\mathbf{X}}(\mathbf{x}) &= f_{\mathbf{X}}(x_1,x_2)  \\
&= \frac{1}{2} 1_{\mathbf{x_1} \in [-1,1]} \frac{1}{2} 1_{\mathbf{x_2} \in [-1,1]}   \\
&= \frac{1}{4} 1_{\mathbf{x} \in E}
\end{aligned}$$
* (the support of $f_{\mathbf{X}}$ is E so it eases the notations).

$$
\begin{aligned}I &=\int_{E}\varphi(\mathbf{x})f_{\mathbf{X}}(\mathbf{x})d\mathbf{x} \\
                 &=\int_{E} 1_{\lVert x \lVert_2 \leq 1}\frac{1}{4} 1_{\mathbf{x} \in E}  d\mathbf{x}  \\
                 &=\frac{\pi}{4}
\end{aligned}
$$

* since the surface of the circle of radius 1 is $\pi$.

* thus $\hat{\pi} = 4 \hat{I}$

* to get (a realization of) $\hat{I}_N$ we will just throw uniformly $N$ random points in $E$, count the number of which that lies in the unit circle. (A realization of) $\hat{I}_N$ is that number divided by $N$.

```python
%matplotlib inline
import numpy as np
import matplotlib.pyplot as plt
N = 10000
x = np.random.uniform(-1,1,(N,2))
in_circle = np.sqrt(np.square(x[:,0])+np.square(x[:,1]))<=1

plt.figure(figsize=(10, 10))
plt.axis([-1.1, 1.1, -1.1, 1.1])
plt.scatter(x[in_circle,0],x[in_circle,1],c='cyan')
plt.scatter(x[~in_circle,0],x[~in_circle,1],c='black')
plt.show()
I_hat = np.sum(in_circle)/N
print("(a realization of) I_hat_N is : {}.".format(I_hat))
print("(a realization of) pi_hat_N is : {}.".format(I_hat*4))
```

* The variance of the estimator of $\mathbb{E}[\varphi(\mathbf{X})]$ (that is the variance of $\hat{I}_N$):
is:
\begin{equation}
\mbox{Var }[\hat{I}_N]=\frac{1}{N}\mbox{Var}[\varphi(\mathbf{X})]
\end{equation}

* and it can be estimated by:
\begin{equation}
\widehat{\mbox{Var }}[\hat{I}]=\frac{1}{N-1}\left[\frac{1}{N}\sum_{i=1}^{N}\varphi^{2}(\mathbf{X}^{i})-\hat{I}^{2}_N\right]
\end{equation}

* still, be careful about "is it the r.v. or the realization of this r.v."

So an estimation of the variance $I_hat_N$ is:

```python
N = 10000
x_1 = np.random.uniform(-1,1,(N,2))
in_circle = np.sqrt(np.square(x_1[:,0])+np.square(x_1[:,1]))<=1
I_hat = np.sum(in_circle)/N
# note that we use another sample for the 2 parts of the estimator. a bit overkill with our sample size.
x_2 = np.random.uniform(-1,1,(N,2))
in_circle_square = np.sqrt(np.square(x_2[:,0])+np.square(x_2[:,1]))<=1
squarephi = np.sum(in_circle_square)/N
# take a moment to understand why the function is the same when squared

Var_hat=(squarephi-I_hat*I_hat)/(N-1)
print("(a realization of) I_hat_N is : {}.".format(I_hat))
print("Var_hat is : {}.".format(Var_hat))
print()
print("the associated estimation pi_hat_N is : {}.".format(I_hat*4))
print("By composition, the estimation of the variance of pi_hat_N is : {}.".format(Var_hat*4*4))
```

    (a realization of) I_hat_N is : 0.779.
    Var_hat is : 1.7697669766976696e-05.

    the associated estimation pi_hat_N is : 3.116.
    By composition, the estimation of the variance of pi_hat_N is : 0.00028316271627162713.

One can build confidence intervals thanks to TLC:
* ugly plug in of the estimation of tha variance as the original variance since our $N$ is "large".

```python
I_sup = I_hat + 1.96 * np.sqrt(Var_hat)
I_inf = I_hat -1.96 * np.sqrt(Var_hat)

print("there is a 95% probability that pi/4 is in [{:f},{:f}]".format(I_inf,I_sup))
print("pi/4=",np.pi/4)
print(np.pi/4>I_inf and np.pi/4<I_sup)

```

    there is a 95% probability that pi/4 is in [0.781223,0.792577]
    pi/4= 0.7853981633974483
    True

```python
%%time

from numba import jit, int32, float32
@jit(int32(int32))
def getI_fast(N):
    x_i = np.random.uniform(-1,1,(N,2))
    in_circle = np.sqrt(np.square(x_i[:,0])+np.square(x_i[:,1]))<=1
    I_hat=np.sum(in_circle)/N
    Var_hat=(I_hat-I_hat*I_hat)/(N-1) # not using 2 samples anymore
    I_sup = I_hat + 1.96 * np.sqrt(Var_hat)
    I_inf = I_hat -1.96 * np.sqrt(Var_hat)
    return np.pi/4>I_inf and np.pi/4<I_sup

M = 10000
N = 200000
in_c = 0
for _ in range(M):
    I_hat_loc = getI_fast(10000)
    in_c+=I_hat_loc

print(in_c/M)
```

    0.9501
    Wall time: 2.68 s

Todo: calcul N balls 1..10, avec 95 est. On voit variance up; besoin de techniques de base pour réduire variance.

```python
x = np.random.uniform(-1,1,(10,2))
print(x)
norm = np.apply_along_axis(np.square,1,x)
norm
```

    [[ 0.97313042 -0.50290659]
     [-0.16794071 -0.82656998]
     [ 0.69388437 -0.05316349]
     [-0.3728245   0.15579747]
     [-0.1871149  -0.86876272]
     [-0.10747036 -0.84243932]
     [-0.54462127  0.1176867 ]
     [ 0.37111461  0.00616314]
     [-0.97239711  0.36186081]
     [ 0.17249894  0.48630984]]

    array([[  9.46982823e-01,   2.52915040e-01],
           [  2.82040837e-02,   6.83217933e-01],
           [  4.81475523e-01,   2.82635656e-03],
           [  1.38998111e-01,   2.42728507e-02],
           [  3.50119859e-02,   7.54748666e-01],
           [  1.15498778e-02,   7.09704000e-01],
           [  2.96612332e-01,   1.38501593e-02],
           [  1.37726054e-01,   3.79842391e-05],
           [  9.45556142e-01,   1.30943246e-01],
           [  2.97558851e-02,   2.36497264e-01]])

```python
## faster to use nditer than jit ... ?

def getdballvolume(N,d):
    volume_unit = 2**d
    x = np.random.uniform(-1,1,(N,d))
    norm = 0
    for xloc in np.nditer(x, flags=['external_loop'], order='F'):
        norm += np.square(xloc)
    in_ball = np.sqrt(norm)<=1
    I_hat=np.sum(in_ball)/N
    Var_hat=(I_hat-I_hat*I_hat)/(N-1)
    return I_hat* 2**d, Var_hat* 2**d* 2**d
```

```python
plt.figure(figsize=(16,10))

li, lv ={},{}
nloop = 40

plt.scatter(1,1,c='k',alpha=1) # arbitrary choice

for d in np.arange(1,15):
    li[d] = []
    lv[d] = []
    for loop in range(nloop):
        i,v=getdballvolume(20000,d)
        li[d].append(i)
        lv[d].append(v)
    plt.scatter(np.tile(d,nloop),li[d],c='k',alpha=.5)

plt.axhline(0)
plt.show()
```

```python
plt.figure(figsize=(16,10))

ax_1_15 =  np.arange(1,15)

plt.plot( ax_1_15,[np.mean(li[d]) for d in  ax_1_15])
plt.scatter(ax_1_15,[np.mean(li[d]) for d in ax_1_15])
plt.plot( ax_1_15,[np.mean(li[d])+1.96*np.sqrt(np.mean(lv[d])) for d in ax_1_15],c="r")
plt.plot( ax_1_15,[np.mean(li[d])-1.96*np.sqrt(np.mean(lv[d])) for d in ax_1_15],c="r")
plt.axhline(0)
plt.show()
```

```python
%%time

dmax=20
li, lv ={},{}
li[0]=0
lv[0]=0
nloop = 5
for d in np.arange(1,dmax):
    li[d] = []
    lv[d] = []
    for loop in range(nloop):
        i,v=getdballvolume(int(10e6),d)
        li[d].append(i)
        lv[d].append(v)

plt.figure(figsize=(16,10))
plt.plot(range(dmax),[np.mean(li[d]) for d in range(dmax)])
plt.scatter(range(dmax),[np.mean(li[d]) for d in range(dmax)])
plt.plot(range(dmax),[np.mean(li[d])+1.96*np.sqrt(np.mean(lv[d])) for d in range(dmax)],c="r")
plt.plot(range(dmax),[np.mean(li[d])-1.96*np.sqrt(np.mean(lv[d])) for d in range(dmax)],c="r")
plt.axhline(0)
plt.show()
```

max_stars_repo_name: DrAnaximandre/presentations

max_issues_repo_name: DrAnaximandre/presentations

max_forks_repo_name: DrAnaximandre/presentations