# 1.6 信息论

### 熵

考虑一个离散变量 $x$。

对于该变量的一个观测值，我们需要一个量来衡量从这个观测量中得到了多少关于 $x$ 的信息。
从主观意义上来说，一个不常见的观测值所提供的新信息要比常见的观测值要多。

我们的信息量 $h$ 依赖于一个概率分布 $p(x)$，$h(x)$ 需要满足这样的性质：
- 关于 $p(x)$ 是单调（递减）的，高概率对应的信息量低，低概率对应的信息量高；
- 假设两个事件 $x， y$ 是不相关的，即 $p(x, y)=p(x)p(y)$，那么从事件 $(x, y)$ 中获得的信息量应该等于单独从 $x$ 和 $y$ 事件中获得的信息量之和，即 $h(x, y)=h(x) + h(y)$；

我们将 $h$ 和 $p$ 之间的关系表示为 $h(p)$，从上面的例子中，我们可以看到

$$
h(pq) = h(p) h(q)
$$

归纳法不难给出 $h(p^n) = nh(p)$，所以

$$
h(p^{n/m}) = nh(p^{1/m}) = \frac n m h(p^{m/m}) = \frac n m h(p)
$$

因此由连续性，我们有 $h(p^x) = x h(p)$。

对任意正数 $p,q$，总有 $q^x = p$ 成立，则我们有

$$
\frac{h(p)}{\ln (p)} = \frac{h(q^x)}{\ln (p^x)} = \frac{xh(q)}{x\ln (q)} = \frac{h(q)}{\ln (q)}
$$

从而 $h(p) \propto \ln p$，即 $h$ 与 $p$ 的对数成正比。

一个可能的定义为

$$
h(x) = - \log_2 p(x)
$$

负号保证信息量为正且满足单调递减的要求，在这里，我们选取了 2 作为对数的底，因此 $h(x)$ 的单位为比特（`bit`）。

有了对于单一的 $x$ 的信息量，更一般的想法是衡量一组 $x$ 的信息量，因此，我们考虑对 x 求期望，得到熵（`entropy`）：

$$
H[x]=-\sum_x p(x) \log_2 p(x)
$$

因为 $\lim_{p\to 0} plnp = 0$，所以如果 $p(x)=0$，那么 $p(x)\ln p(x)=0$

### 编码长度和熵

现在假设我们要传送状态变量给一个接收器。

考虑一个有 8 个状态的变量 $x$，每个状态都是等概率的。为了表示这个状态，我们至少需要 3 比特的信息来表示状态，此时熵值为

$$
H[x] = - 8 \times \frac 1 8 \log_2 \frac 1 8 = 3~\text{bits}
$$

再考虑不等概率的情况，假设状态和概率为

|a|b|c|d|e|f|g|h|
|---|---|---|---|---|---|---|---|
|$\frac{1}{2}$|$\frac{1}{4}$|$\frac{1}{8}$|$\frac{1}{16}$|$\frac{1}{64}$|$\frac{1}{64}$|$\frac{1}{64}$|$\frac{1}{64}$|

熵为

$$
H[x] = - \frac{1}{2} \log_2 \frac{1}{2} - \frac{1}{4}\log_2 \frac{1}{4} - \frac{1}{8} \log_2 \frac{1}{8} - \frac{1}{16} \log_2 \frac{1}{16} - \frac{4}{64} \log_2 \frac{1}{64} = 2~\text{bits}
$$

不等概率的熵要比等概率的熵要小。

考虑字符的霍夫曼编码：

|a|b|c|d|e|f|g|h|
|---|---|---|---|---|---|---|---|
|0|10|110|1110|111100|111101|111110|111111|

其平均字符长度为：

$$
\frac{1}{2} \times 1 + \frac{1}{4} \times 2 + \frac{1}{8} \times 3 + \frac{1}{16} \times 4 + \frac{4}{64} \times 6 = 2~\text{bits}
$$

因此，熵和霍夫曼编码长度之间存在一定的关系。

`Shannon` 在 1948 年的 `noiseless coding theorem` 说明，这样的熵是编码长度的下界。

### 自然对数熵

现在考虑自然对数表示的熵。此时熵的单位是 `nat` 而不是 `bit`。

我们之前用平均信息量的方式引入了熵，事实上熵的概念是从物理上引入的。

假设我们将 $N$ 个相同的小球放入一些容器中，使得第 $i$ 个容器中有 $n_i$ 个球，$\sum_i n_i = N$。

我们按顺序从第 1 个容器开始往里面放入小球，第 1 个有 $N$ 种选择，第 2 个有 $N-1$ 种选择……总共有 $N!$ 种选择。

但是放入每个容器中的 $n_i$ 个球是不区分顺序的，有 $n_i!$ 种可能性放置这些小球，因此总的可能方法有

$$
W = \frac{N!}{\prod_i n_i!}
$$

定义熵为

$$
H = \frac{1}{N} \ln W = \frac 1 N\ln N! - \frac 1 N \sum_{i} \ln n_i !
$$

当 $N \to \infty$ 时，由 `Stirling` 近似公式：

$$
\ln N! \approx N \ln N - N
$$

我们有

$$
H = \lim_{N\to \infty} \frac 1 N (N\ln N - N) - \frac 1 N \sum_{i} (n_i \ln n_i - n_i)
= -\lim_{N\to \infty} \sum_i \left(\frac{n_i}{N}\right) \ln \left(\frac{n_i}{N}\right)
= -\sum_i p_i \ln p_i
$$

$p_i = \lim_{N\to\infty} \left(\frac{n_i}{N}\right)$ 是小球放入第 i 个容器的概率。

在物理上，容器中小球的分布叫做微观状态，各个容器中的小球数目叫做宏观状态。

对于一个离散随机变量 $X$ 的取值 $x_i$ 当作一个容器，即有 $p(X=x_i)=p_i$，则它的熵为：

$$
H[p] = - \sum_i p(x_i) \ln p(x_i)
$$

一般来说，一个离散分布的点越多，分布越均匀，那么对应的熵也越大。

如果其中有一个 $p_i=1$，那么熵取到最小值 0，最大值可以用 Lagrange 乘子求解，Lagrange 函数为

$$
\tilde H = - \sum_i p(x_i) \ln p(x_i) + \lambda \left(\sum_i p(x_i) - 1\right)
$$

最大化这个函数得到：

$$
\frac{\partial \tilde H}{\partial p(x_i)} = -1 -\ln p(x_i) + \lambda = 0
$$

得到最大熵应该满足一个均匀离散分布 $p(x_i)=p(x_j), \forall i,j$。

若状态总数为 $M$，则 $p(x_i) = \frac 1 M$，此时，我们有 $H = \ln M$。

为了验证的确是个最大值，我们可以计算它的二阶导数来验证。

### 连续变量的熵

对于连续变量，我们将 $x$ 划分为长度为 $\Delta$ 的小段，积分中值定理告诉我们，在第 $i$ 小段 $(i\Delta, (i+1)\Delta)$ 中存在 $x_i$，使得：

$$
\int_{i\Delta}^{i+1\Delta} p(x) dx = p(x_i) \Delta
$$

我们可以将落在第 $i$ 小段的 $x$ 对应为 $x_i$，则观测到 $x_i$ 的概率为 $p(x_i) \Delta$，这给出了一个关于 $x_i$ 的离散分布，其熵为：

$$
H_{\Delta} = - \sum_i p(x_i) \Delta \ln(p(x_i) \Delta) = - \sum_i p(x_i) \Delta \ln(p(x_i)) - \ln\Delta
$$

其中 $p(x_i) \Delta= 1$，现在忽略最后的常数项，然后考虑 $\Delta \to0$ 的情况，我们有

$$
-\lim_{\Delta \to 0} \left\{ \sum_i p(x_i) \Delta \ln(p(x_i) \Delta) \right\} = -\int p(x) \ln p(x) dx
$$

这一项叫做微分熵（`differential entropy`），它与离散熵的差别在于一个 $\ln \Delta$，在 $\Delta \to0$ 时会发散的部分。

对于多元向量 $\mathbf x$，微分熵定义为

$$
H[\mathbf x]=-\int p(\mathbf x) \ln p(\mathbf x) d\mathbf x
$$

在离散分布的例子中，我们看到最大化熵的分布是均匀离散分布。

为了最大化连续分布的熵，我们需要加上 $p(x)$ 的一阶矩和二阶矩都固定的条件。

考虑一维情况，我们的问题为

$$
\begin{align}
\max_{p(x)}~& -\int p(x) \ln p(x) dx \\
s.t.~& 1 = \int_{-\infty}^{\infty} p(x) dx  \\
& \mu = \int_{-\infty}^{\infty} xp(x) dx\\
& \sigma^2 = \int_{-\infty}^{\infty} (x-\mu)^2p(x) dx\\
\end{align}
$$

`Lagrange` 函数为

$$
-\int p(x) \ln p(x) dx + \lambda_1 \left(\int_{-\infty}^{\infty} p(x) dx - 1\right) + \lambda_2 \left(\int_{-\infty}^{\infty} xp(x) dx - \mu\right) + \lambda_3 \left(\int_{-\infty}^{\infty} (x-\mu)^2p(x) dx - \sigma^2) \right)
$$

令这个泛函对 $p(x)$ 的导数为 `0`，得到

$$
1-\ln p(x)+\lambda_1 + \lambda_2 x + \lambda_3 (x-\mu)^2 = 0
$$

从而

$$
p(x) = \exp\left\{1+\lambda_1 + \lambda_2 x + \lambda_3 (x-\mu)^2\right\}
$$

将这个形式带入约束条件，不难得到

$$
p(x) = \frac{1}{(2\pi\sigma^2)^{1/2}} \exp\left\{-\frac{(x-\mu)^2}{2\sigma^2}\right\}
$$

即最大熵的分布是高斯分布。

注意到，我们并没有对 $p(x)$ 做非负的约束，但是最大化给出的结果是一个非负的结果。

高斯分布的熵为：

$$
\frac{1}{2} \left\{1+\ln(2\pi\sigma^2)\right\}
$$

从这个表达式我们可以看出，与离散熵不同，连续熵可以为负值：$\sigma^2 < \frac{1}{2\pi e}, H(x) < 0$，原因是概率密度函数 p(x) 没有小于 1 的限制，因此 $-p(x)\ln p(x)$ 不能保证非负。

### 条件熵

假设我们有 $\mathbf{x,y}$ 的联合分布，假设 $\mathbf x$ 已知，那么 $\mathbf y$ 的包含的信息量应当由 $-\ln p(\mathbf{y|x})$ 来衡量，因此我们可以定义其条件期望为给定 $\mathbf x$ 下 $\mathbf y$ 的条件熵（`conditional entropy`）：

$$
H[\mathbf{y|x}] = -\iint p(\mathbf{x,y}) \ln(\mathbf{y|x}) d\mathbf xd\mathbf y
$$

可以计算出，我们有：

$$
H[\mathbf{x, y}] = H[\mathbf{y|x}] + H[\mathbf{x}]
$$

因此，用来描述 $\mathbf{x,y}$ 的信息量等于描述 $\mathbf x$ 的信息量加上给定 $\mathbf x$ 的条件下描述 $\mathbf y$ 所需的信息量。

## 1.6.1 相对熵和互信息

假设我们有一个未知分布 $p(\mathbf x)$，然后对它建模得到了一个概率分布 $q(\mathbf x)$。

用 $q(\mathbf x)$ 的分布来传送 $x$ 所用的信息量（$q(\mathbf x)$），比用真实的分布 $p(\mathbf x)$ 传送所用的信息量（$p(\mathbf x)$）要多一些，多出来的部分为：

$$
\begin{align}
KL(p\|q)
&= - \int p(\mathbf x)\ln(q(\mathbf x)) dx - \left(-\int p(\mathbf x)\ln p(\mathbf x)\right)\\
&= - \int p(\mathbf x)\ln \left\{\frac{q(\mathbf x)}{p(\mathbf x)}\right\}
\end{align}
$$

这叫做相对熵（`relative entropy`）或者 `Kullback-Leibler` 散度（`Kullback-Leibler divergence`）。

`K-L` 散度不是对称的，即 $KL(p\|q) \neq KL(q\|p)$。

`K-L` 散度满足 $KL(p\|q) \geq 0$，当且仅当 $p(\mathbf x)=q(\mathbf x)$ 时取等号。

### 凸函数

如果一个函数的弦（`chord`）始终在函数上或者函数上方，那么这个函数就是凸函数。

一个凸函数的例子如图所示：

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

%matplotlib inline

fig, ax = plt.subplots()

xx = np.linspace(-3, 4, 100)
yy = xx ** 2

ax.plot(xx, yy, 'r')
ax.set_xlim(-4, 5)
ax.set_yticks([])
ax.set_ylim(-10, 20)

ax.set_xticks([-2, 0.5, 3])
ax.set_xticklabels([r"$a$", r"$x_\lambda$", r"$b$"], fontsize="xx-large")

for i in [-2, 3]:
    ax.plot([i, i], [-10, i ** 2], 'g--')

ax.plot([-2, 3], [4, 9], 'b')
ax.plot([0.5, 0.5], [-10, 6.5], 'g')
ax.annotate(
    "chord",
    fontsize="xx-large",
    xytext=(-3, 12),
    xy=(-0.2, 6),
    arrowprops=dict(arrowstyle="->",
                    connectionstyle="arc3")
           )

plt.show()
```

在 $x=a$ 和 $x=b$ 的区间内的任意点 $x$ 都可以写成 $\lambda a +(1-\lambda)b, 0\leq\lambda \leq 1$，对应的弦上的值为 $\lambda f(a) +(1-\lambda) f(b)$，函数值为 $f(\lambda a +(1-\lambda)b)$。凸性等价于

$$
f(\lambda a +(1-\lambda)b) \leq \lambda f(a) +(1-\lambda) f(b)
$$

这也等价于函数的二阶导数非负（如果二阶导数存在）。

证明如下：

- 必要性

假设凸性的式子成立，令 $\lambda = \frac 1 2, b = a + 2\epsilon$，则有：

$$
\begin{align}
\frac 1 2f(a)+\frac 1 2f(b) & \geq f\left(\frac 1 2a + \frac 1 2b\right) \\
& = \frac 1 2 f\left((\frac 1 2a + \frac 1 2b\right) + \frac 1 2 f\left((\frac 1 2a + \frac 1 2b\right) \\
& = \frac 1 2 f(a+\epsilon) + \frac{1}{2} f(b-\epsilon)
\end{align}
$$

从而

$$
f(b) - f(b-\epsilon) \geq  f(a+\epsilon) - f(a)
$$

取 $\epsilon\to 0$，我们有

$$
f'(b) \geq f'(a)
$$

从而

$$
f''(x) \geq 0
$$

- 充分性

假设二阶导数非负：$f''(x) \geq 0$，那么由泰勒展开，存在一个 $x^\star$ 使得

$$
f(x) = f(x_0) + f'(x_0)(x-x_0) + \frac 1 2 f''(x^\star) (x-x_0)^2 \geq f(x_0) + f'(x_0)(x-x_0)
$$

令 $x_0 = \lambda a +(1-\lambda b), x = a$，则有

$$
f(a) \geq f(x_0) + f'(x_0)((1-\lambda)(a-b))
$$

令 $x_0 = \lambda a +(1-\lambda b), x = b$，则有

$$
f(b) \geq f(x_0) + f'(x_0)(\lambda(a-b))
$$

合并两个不等式，去掉 $f'(x_0)$ 的项：

$$
\lambda f(a) +(1-\lambda) \geq f(b) f(\lambda a +(1-\lambda)b)
$$

凸函数的例子：

- $x\ln x$
- $x^2$

如果等式只在 $\lambda = 1$ 和 $\lambda = 0$ 时成立，则称 $f(x)$ 是严格凸函数。

如果 $f(x)$ 是凸函数，那么 $-f(x)$ 是凹函数。

### Jensen 不等式

使用数学归纳法不难证明，对于任意凸函数 $f(x)$，`Jensen` 不等式成立：

$$
f\left(\sum_{i=1}^M \lambda _i x_i\right) \leq \sum_{i=1}^M\lambda_i f(x_i)
$$

其中 $\lambda_i \geq 0, \sum_i \lambda_i = 1$。等式成立的条件为 $f$ 是线性，或者所有的 $x_i$ 都相等。

证明如下：

$M = 1$ 时显然成立；假设对于某个 $M$ 不等式成立，那么对于 $M+1$ 来说：

$$
f\left(\sum_{i=1}^{M+1} \lambda_i x_i\right) = f\left(\lambda_{M+1} x_{M+1} + \sum_{i=1}^M \lambda _i x_i\right) = f\left(\lambda_{M+1} x_{M+1} + (1-\lambda_{M+1})\sum_{i=1}^M \eta_i x_i\right)
$$

其中

$$
\eta_i = \frac{\lambda_i}{1-\lambda_{M+1}}
$$

考虑凸函数性质，我们有

$$
f\left(\sum_{i=1}^{M+1} \lambda_i x_i\right) = f\left(\lambda_{M+1} x_{M+1} + (1-\lambda_{M+1})\sum_{i=1}^M \eta_i x_i\right) \leq \lambda_{M+1} f(x_{M+1}) + (1-\lambda_{M+1}) f\left(\sum_{i=1}^M \eta_i x_i\right)
$$

再应用假设条件，我们有
$$
f\left(\sum_{i=1}^{M+1} \lambda_i x_i\right) \leq \lambda_{M+1} f(x_{M+1}) + (1-\lambda_{M+1}) f\left(\sum_{i=1}^M \eta_i x_i\right) \leq \lambda_{M+1} f(x_{M+1}) + \sum_{i=1}^M (1-\lambda_{M+1})\eta_i f\left(x_i\right) = \sum_{i=1}^{M+1} \lambda_i f(x_i)
$$

如果我们认为 $\lambda_i$ 是某个离散分布取 $x_i$ 的概率，那 `Jenson` 不等式可以表示为

$$
f\left(\mathbb E[\mathbf x]\right) \leq \mathbb E[f(\mathbf x)]
$$

不等式成立的条件为 $x$ 为常数，或者 $f$ 是线性的。

更一般的，我们有：

$$
f\left(\mathbb E[y(\mathbf x)]\right) \leq \mathbb E[f(y(\mathbf x))]
$$

对于连续形式，我们有

$$
f\left(\int y(\mathbf x)p(\mathbf x)d\mathbf x\right) \leq \int f(y(\mathbf x))p(\mathbf x)d\mathbf x
$$

### 应用 Jensen 不等式

将 `Jensen` 不等式应用到 `K-L` 散度（$-\ln x$ 是凸函数）上：

$$
KL(p||q)=-\int p(\mathbf x)\ln\left\{\frac{q(\mathbf x)}{p(\mathbf x)}\right\} d\mathbf x
\geq -\ln \int \left\{p(\mathbf x) \frac{q(\mathbf x)}{p(\mathbf x)}\right\}
= -\ln \int q(\mathbf x)d\mathbf x = 0
$$

因为 $-\ln x$ 是严格凸的，因此等式只能在 $\frac{q(\mathbf x)}{p(\mathbf x)}$ 为常数时成立，考虑到两者都是概率分布（积分都为 1），所以只能有 $p(\mathbf x) = q(\mathbf x)$ 成立。

因此，我们可以将 `K-L` 散度来衡量两个分布的差异。

另一方面，我们看到，当我们使用真实的数据分布传送数据，所用信息量的期望是最小的。

### 最小化相对熵

假设我们有一组从未知分布 $p(\mathbf x)$ 中产生的数据，我们的模型为 $q(\mathbf{x|\theta})$，一个决定参数 $\mathbf \theta$ 的方法就是最小化 $p(\mathbf x)$ 和 $q(\mathbf{x|\theta})$ 的 `K-L` 散度。

然而我们并不知道 $p(x)$ 的真实分布，一个近似的方法是使用已观测的数据进行模拟：

$$
KL(p|q) \approx \sum_{n=1}^N (-\ln q(\mathbf{x}_n | \mathbf{\theta}) + \ln p(\mathbf x_n))
$$

由于右边的部分与 $\mathbf\theta$ 无关，因此，最小化 `K-L` 散度就相当于最大化似然函数。

### 互信息

考虑两个变量 $\mathbf{x,y}$ 的分布，如果它们独立，则 $p(\mathbf{x,y}) = p(\mathbf{x})p(\mathbf{y})$，如果他们不独立，则我们用 $p(\mathbf{x,y})$ 和 $p(\mathbf{x})p(\mathbf{y})$ 的 `K-L` 散度来衡量它们接近独立的程度：

$$
I[\mathbf{x,y}] \equiv KL(p(\mathbf{x,y})||p(\mathbf{x})p(\mathbf{y}))
= - \iint p(\mathbf{x,y}) \ln \frac{p(\mathbf{x})p(\mathbf{y})}{p(\mathbf{x,y})} d\mathbf xd\mathbf y
$$

这叫做 $\mathbf x$ 和 $\mathbf y$ 的互信息（`mutual information`）。从 `K-L` 散度的性质中我们可以看出，$I[\mathbf{x,y}] \geq 0$，当且仅当 $\mathbf{x,y}$ 是独立的时候等号成立。

另外，我们从定义式中可以得到：

$$
I[\mathbf{x,y}] = H[\mathbf x] - H[\mathbf{x|y}] = H[\mathbf y] - H[\mathbf{y|x}]
$$

所以我们可以将互信息看成是给定 $\mathbf y$ 后 $\mathbf x$ 信息量的减少值。


###### Content under Creative Commons Attribution license CC-BY 4.0, code under MIT license (c)2014 L.A. Barba, G.F. Forsyth, C.D. Cooper. Based on [CFD Python](https://github.com/barbagroup/CFDPython), (c)2013 L.A. Barba, also under CC-BY.

# Space & Time

## Introduction to numerical solution of PDEs

Welcome to *Space and Time: Introduction to finite-difference solutions of PDEs*, the second module of ["Practical Numerical Methods with Python"](http://openedx.seas.gwu.edu/courses/GW/MAE6286/2014_fall/about).

In the first module, we looked into numerical integration methods for the solution of ordinary differential equations (ODEs), using the phugoid model of glider flight as a motivation. In this module, we will study the numerical solution of *partial differential equations (PDEs)*, where the unknown is a multi-variate function. The problem could depend on time, $t$, and one spatial dimension $x$ (or more), which means we need to build a discretization grid with each independent variable.

We will start our discussion of numerical PDEs with 1-D linear and non-linear convection equations, the 1-D diffusion equation, and 1-D Burgers' equation. We hope you will enjoy them!

## 1D linear convection

The *one-dimensional linear convection equation* is the simplest, most basic model that can be used to learn something about numerical solution of PDEs. It's surprising that this little equation can teach us so much! Here it is:

\begin{equation}\frac{\partial u}{\partial t} + c \frac{\partial u}{\partial x} = 0\end{equation}

The equation represents a *wave* propagating with speed $c$ in the $x$ direction, without change of shape. For that reason, it's sometimes called the *one-way wave equation* (sometimes also the *advection equation*).

With an initial condition $u(x,0)=u_0(x)$, the equation has an exact solution given by:

\begin{equation}u(x,t)=u_0(x-ct).
\end{equation}

Go on: check it. Take the time and space derivative and stick them into the equation to see that it holds!

Look at the exact solution for a moment ... we know two things about it:

1. its shape does not change, being always the same as the initial wave, $u_0$, only shifted in the $x$-direction; and
2. it's constant along so-called **characteristic curves**, $x-ct=$constant. This means that for any point in space and time, you can move back along the characteristic curve to $t=0$ to know the value of the solution.

####Characteristic curves for positive wave speed.

Why do we call the equations *linear*? PDEs can be either linear or non-linear. In a linear equation, the unknown function $u$ and its derivatives appear only in linear terms, in other words, there are no products, powers, or transcendental functions applied on them.

What is the most important feature of linear equations? Do you remember? In case you forgot: solutions can be superposed to generate new solutions that still satisfy the original equation. This is super useful!

## Finite-differences

In the previous lessons, we discretized time derivatives; now we have derivatives in both space *and* time, so we need to discretize with respect to *both* these variables.

Imagine a *space-time* plot, where the coordinates in the vertical direction represent advancing in time—for example, from $t^n$ to $t^{n+1}$—and the coordinates in the horizontal direction move in space: consecutive points are $x_{i-1}$, $x_i$, and $x_{i+1}$.  This creates a grid where a point has both a temporal and spatial index. Here is a graphical representation of the space-time grid:

\begin{matrix}
t^{n+1} & \rightarrow & \bullet  && \bullet  && \bullet  \\
t^n & \rightarrow & \bullet  && \bullet  && \bullet  \\
& &  x_{i-1} && x_i && x_{i+1}
\end{matrix}

For the numerical solution of $u(x,t)$, we'll use subscripts to denote the spatial position, like $u_i$, and superscripts to denote the temporal instant, like $u^n$.  We would then label the solution at the top-middle point in the grid above as follows:
$u^{n+1}_{i}$.

Each grid point below has an index $i$, corresponding to the spatial position and increasing to the right, and an index $n$, corresponding to the time instant and increasing upwards.  A small grid segment would have the following values of the numerical solution at each point:

\begin{matrix}
& &\bullet & & \bullet & &  \bullet \\
& &u^{n+1}_{i-1} & & u^{n+1}_i & & u^{n+1}_{i+1} \\
& &\bullet & & \bullet & &  \bullet \\
& &u^n_{i-1} & & u^n_i & & u^n_{i+1} \\
& &\bullet & & \bullet & &  \bullet \\
& &u^{n-1}_{i-1} & & u^{n-1}_i & & u^{n-1}_{i+1} \\
\end{matrix}

Another way to explain our discretization grid is to say that it is built with constant steps in time and space, $\Delta t$ and $\Delta x$, as follows:

\begin{eqnarray}
x_i &=& i\, \Delta x \quad \text{and} \quad t^n= n\, \Delta t \nonumber \\
u_i^n &=& u(i\, \Delta x, n\, \Delta t)
\end{eqnarray}

### Discretizing our model equation

Let's see how to discretize the 1-D linear convection equation in both space and time.  By definition, the partial derivative with respect to time changes only with time and not with space; its discretized form changes only the $n$ indices.  Similarly, the partial derivative with respect to $x$ changes with space not time, and only the $i$ indices are affected.

We'll discretize the spatial coordinate $x$ into points indexed from $i=0$ to $N$, and then step in discrete time intervals of size $\Delta t$.

From the definition of a derivative (and simply removing the limit), we know that for $\Delta t$ sufficiently small:

\begin{equation}\frac{\partial u}{\partial x}\approx \frac{u(x+\Delta x)-u(x)}{\Delta x}\end{equation}

This formula could be applied at any point $x_i$. But note that it's not the only way that we can estimate the derivative. The geometrical interpretation of the first derivative $\partial u/ \partial x$ at any point is that it represents the slope of the tangent to the curve $u(x)$. In the sketch below, we show a slope line at $x_i$ and mark it as "exact." If the formula written above is applied at $x_i$, it approximates the derivative using the next spatial grid point: it is then called a _forward difference_ formula.

But as shown in the sketch below, we could also estimate the spatial derivative using the point behind $x_i$, in which case it is called a _backward difference_. We could even use the two points on each side of $x_i$, and obtain what's called a _central difference_ (but in that case the denominator would be $2\Delta x$).

####Three finite-difference approximations at $x_i$.

We have three possible ways to represent a discrete form of $\partial u/ \partial x$:

* Forward difference: uses $x_i$ and $x_i + \Delta x$,
* Backward difference: uses $x_i$ and $x_i- \Delta x$,
* Central difference: uses two points on either side of $x_i$.

The sketch above also suggests that some finite-difference formulas might be better than others: it looks like the *central difference* approximation is closer to the slope of the "exact" derivative. Curious if this is just an effect of our exaggerated picture? We'll show you later how to make this observation rigorous!

The three formulas are:

\begin{eqnarray}
\frac{\partial u}{\partial x} & \approx & \frac{u(x_{i+1})-u(x_i)}{\Delta x} \quad\text{Forward}\\
\frac{\partial u}{\partial x} & \approx & \frac{u(x_i)-u(x_{i-1})}{\Delta x} \quad\text{Backward}\\
\frac{\partial u}{\partial x} & \approx & \frac{u(x_{i+1})-u(x_{i-1})}{2\Delta x} \quad\text{Central}
\end{eqnarray}

Euler's method is equivalent to using a forward-difference scheme for the time derivative. Let's stick with that, and choose the backward-difference scheme for the space derivative.  Our discrete equation is then:

\begin{equation}\frac{u_i^{n+1}-u_i^n}{\Delta t} + c \frac{u_i^n - u_{i-1}^n}{\Delta x} = 0, \end{equation}

where $n$ and $n+1$ are two consecutive steps in time, while $i-1$ and $i$ are two neighboring points of the discretized $x$ coordinate. With given initial conditions, the only unknown in this discretization is $u_i^{n+1}$.  We solve for this unknown to get an equation that lets us step in time, as follows:

\begin{equation}u_i^{n+1} = u_i^n - c \frac{\Delta t}{\Delta x}(u_i^n-u_{i-1}^n)\end{equation}

We like to make drawings of a grid segment, showing the grid points that influence our numerical solution. This is called a **stencil**. Below is the stencil for solving our model equation with the finite-difference formula we wrote above.

####Stencil for the "forward-time/backward-space" scheme.

## And compute!

Alright. Let's get a little Python on the road. First: we need to load our array and plotting libraries, as usual. And if you noticed in the [*Bonus!* notebook for Module 1](http://nbviewer.ipython.org/github/numerical-mooc/numerical-mooc/blob/master/lessons/01_phugoid/01_04_Second_Order_Methods.ipynb), we taught you a neat trick to set some global plotting parameters with the `rcParams` module. We like to do that.

```
import numpy
import matplotlib.pyplot as plt
%matplotlib inline
from matplotlib import rcParams
rcParams['font.family'] = 'serif'
rcParams['font.size'] = 16
```

As a first exercise, we'll solve the 1D linear convection equation with a *square wave* initial condition, defined as follows:

\begin{equation}
u(x,0)=\begin{cases}2 & \text{where } 0.5\leq x \leq 1,\\
1 & \text{everywhere else in } (0, 2)
\end{cases}
\end{equation}

We also need a boundary condition on $x$: let $u=1$ at $x=0$. Our spatial domain for the numerical solution will only cover the range $x\in (0, 2)$.

####Square wave initial condition.

Now let's define a few variables; we want to make an evenly spaced grid of points within our spatial domain. In the code below, we define a variable called `nx` that will be the number of spatial grid points, and a variable `dx` that will be the distance between any pair of adjacent grid points. We also can define a step in time, `dt`, a number of steps, `nt`, and a value for the wave speed: we like to keep things simple and make $c=1$.

```
nx = 41  # try changing this number from 41 to 81 and Run All ... what happens?
dx = 2./(nx-1)
nt = 25
dt = .02
c = 1.      #assume wavespeed of c = 1
```

We also need to set up our initial conditions. Here, we use the NumPy function `ones()` defining an array which is `nx` elements long with every value equal to $1$. How useful! We then *change a slice* of that array to the value $u=2$, to get the square wave, and we print out the initial array just to admire it. Make sure you understand what is going on with the indices of the slicing of `u` here!

```
u = numpy.ones(nx)      #numpy function ones()
u[.5/dx : 1/dx+1]=2  #setting u = 2 between 0.5 and 1 as per our I.C.s
print(u)
```

    [ 1.  1.  1.  1.  1.  1.  1.  1.  1.  1.  2.  2.  2.  2.  2.  2.  2.  2.
      2.  2.  2.  1.  1.  1.  1.  1.  1.  1.  1.  1.  1.  1.  1.  1.  1.  1.
      1.  1.  1.  1.  1.]

Now let's take a look at those initial conditions we've built with a handy plot.

```
plt.plot(numpy.linspace(0,2,nx), u, color='#003366', ls='--', lw=3)
plt.ylim(0,2.5);
```

It does look pretty close to what we expected. But it looks like the sides of the square wave are not perfectly vertical. Is that right? Think for a bit.

Now it's time to write some code for the discrete form of the convection equation using our chosen finite-difference scheme.

For every element of our array `u`, we need to perform the operation:

$$u_i^{n+1} = u_i^n - c \frac{\Delta t}{\Delta x}(u_i^n-u_{i-1}^n)$$

We'll store the result in a new (temporary) array `un`, which will be the solution $u$ for the next time-step.  We will repeat this operation for as many time-steps as we specify and then we can see how far the wave has traveled.

We first initialize the placeholder array `un` to hold the values we calculate for the $n+1$ timestep, using once again the NumPy function `ones()`.

Then, we may think we have two iterative operations: one in space and one in time (we'll learn differently later), so we may start by nesting a spatial loop inside the time loop, as shown below. You see that the code for the finite-difference scheme is a direct expression of the discrete equation:

```
for n in range(1,nt):
    un = u.copy()
    for i in range(1,nx):

        u[i] = un[i]-c*dt/dx*(un[i]-un[i-1])
```

**Note**—We will learn later that the code as written above is quite inefficient, and there are better ways to write this, Python-style. But let's carry on.

Now let's inspect our solution array after advancing in time with a line plot.

```
plt.plot(numpy.linspace(0,2,nx), u, color='#003366', ls='--', lw=3)
plt.ylim(0,2.5);
```

That's funny. Our square wave has definitely moved to the right, but it's no longer in the shape of a top-hat.  **What's going on?**

##### Dig deeper

The solution differs from the expected square wave because the discretized equation is an approximation of the continuous differential equation that we want to solve. There are errors: we knew that. But the modified shape of the initial wave is something curious. Maybe it can be improved by making the grid spacing finer. Why don't you try it? Does it help?

## Spatial truncation error

Recall the finite-difference approximation we are using for the spatial derivative:

\begin{equation}\frac{\partial u}{\partial x}\approx \frac{u(x+\Delta x)-u(x)}{\Delta x}\end{equation}

We obtain it by using the definition of the derivative at a point, and simply removing the limit, in the assumption that $\Delta x$ is very small. But we already learned with Euler's method that this introduces an error, called the *truncation error*.

Using a Taylor series expansion for the spatial terms now, we see that the backward-difference scheme produces a first-order method, in space.

\begin{equation}
\frac{\partial u}{\partial x}(x_i) = \frac{u(x_i)-u(x_{i-1})}{\Delta x} + \frac{\Delta x}{2} \frac{\partial^2 u}{\partial x^2}(x_i) - \frac{\Delta x^2}{6} \frac{\partial^3 u}{\partial x^3}(x_i)+ \cdots
\end{equation}

The dominant term that is neglected in the finite-difference approximation is of $\mathcal{O}(\Delta x)$. We also see that the approximation *converges* to the exact derivative as $\Delta x \rightarrow 0$. That's good news!

In summary, the chosen "forward-time/backward space" difference schem is first-order in both space and time: the truncation errors are $\mathcal{O}(\Delta t, \Delta x)$. We'll come back to this!

## Non-linear convection

Let's move on to the non-linear convection equation, using the same methods as before. The 1-D convection equation is:

\begin{equation}\frac{\partial u}{\partial t} + u \frac{\partial u}{\partial x} = 0\end{equation}

The only difference with the linear case is that we've replaced the constant wave speed $c$ by the variable speed $u$. The equation is non-linear because now we have a product of the solution and one of its derivatives: the product $u\,\partial u/\partial x$. This changes everything!

We're going to use the same discretization as for linear convection: forward difference in time and backward difference in space. Here is the discretized equation:

\begin{equation}\frac{u_i^{n+1}-u_i^n}{\Delta t} + u_i^n \frac{u_i^n-u_{i-1}^n}{\Delta x} = 0\end{equation}

Solving for the only unknown term, $u_i^{n+1}$, gives an equation that can be used to advance in time:

\begin{equation}u_i^{n+1} = u_i^n - u_i^n \frac{\Delta t}{\Delta x} (u_i^n - u_{i-1}^n)\end{equation}

There is very little that needs to change from the code written so far. In fact, we'll even use the same square-wave initial condition. But let's re-initialize the variable `u` with the initial values, and re-enter the numerical parameters here, for convenience (we no longer need $c$, though).

```
##problem parameters
nx = 41
dx = 2./(nx-1)
nt = 10
dt = .02

##initial conditions
u = numpy.ones(nx)
u[.5/dx : 1/dx+1]=2

x = numpy.linspace(0,2,nx)

```

 How does it look?

```
plt.plot(numpy.linspace(0,2,nx), u, color='#003366', ls='--', lw=3)
plt.ylim(0,2.5);
```

Changing just one line of code in the solution of linear convection, we are able to now get the non-linear solution: the line that corresponds to the discrete equation now has `un[i]` in the place where before we just had `c`. So you could write something like:
```Python
for n in range(1,nt):
  un = u.copy()
  for i in range(1,nx):
    u[i] = un[i]-un[i]*dt/dx*(un[i]-un[i-1])
```

We're going to get more clever than that use NumPy to update all values of the spatial grid in one fell swoop. We don't really need to write a line of code that gets executed *for each* value of $u$ on the spatial grid. Python can update them all at once! Study the code below, and compare it with the one above. Here is a helpful sketch, to illustrate the array operation—also called a "vectorized" operation—for $u_i-u_{i-1}$.

####Sketch to explain vectorized stencil operation.

```
for n in range(1, nt):
    un = u.copy()
    u[1:] = un[1:]-un[1:]*dt/dx*(un[1:]-un[0:-1])
    u[0] = 1.0
```

```
plt.plot(numpy.linspace(0,2,nx), u, color='#003366', ls='--', lw=3)
plt.ylim(0,2.5);
```

Hmm. That's quite interesting: like in the linear case, we see that we have lost the sharp sides of our initial square wave, but there's more. Now, the wave has also lost symmetry! It seems to be lagging on the rear side, while the front of the wave is steepening. Is this another form of numerical error, do you ask? No! It's physics!

##### Dig deeper

Think about the effect of having replaced the constant wave speed $c$ by the variable speed given by the solution $u$. It means that different parts of the wave move at different speeds. Make a sketch of an initial wave and think about where the speed is higher and where it is lower ...

---

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

```
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>


# Analiza i projektiranje računalom: 1. domaća zadaća

Implementirajte  razred  "Matrica"  (ili  neku  sličnu  programsku  cjelinu)  koji  omogućava  jednostavnije rukovanje objektima tipa dvodimenzionalne matrice. Za tu namjenu ostvarite osnovne elemente:

- podatke o dimenzijama matrice i dinamički zauzete elemente matrice tipa *double*. Smještaj sadržaja matrice u memoriji organizirati po izboru, uz dinamičko zauzimanje memorije.
- potrebne konstruktore i destruktore, po potrebi metode za mijenjanje dimenzija matrice.
- operator ili metoda pridruživanja (funkcionalnost `A = B`).
- metoda koja čita matricu iz tekstne datoteke u kojoj je matrica smještena tako da jedan redak matrice odgovara  jednom  retku  u  datoteci.  Na  osnovu  broja  elemenata  u  prvom  retku  određuje  se  broj stupaca  matrice,  a  redaka  ima  koliko  ima  i  redaka  u  datoteci.  Brojevi  u  jednom  retku  mogu  biti odvojeni razmakom ili tabulatorom. Vidi primjer ulazne datoteke na kraju!
- metoda koja ispisuje matricu u datoteku u istom formatu, te metodu koja ispisuje matricu na ekran.
- metode  za  postavljanje  i  dohvat  jednog  elementa  matrice.  Može  biti  implementirano  i  istom metodom, uz korištenje funkcije koja vraća referencu i/ili nadgradnjom operatora `[]` (C++) ili nekim drugim postupkom po želji.
- metode  za  zbrajanje,  oduzimanje,  množenje  i  transponiranje  matrica,  te  metode  koje  obavljaju  C operatore `+=` i `-=`. Preporuča se da budu implementirane kao nadgrađeni operatori.
- metoda za množenje matrice sa skalarom. Može biti ostvarena i kao operator.
- operator `==` za usporedbu matrica.

Primjer uporabe uz nadgrađene operatore:

```
Matrica A("A.txt"), B("B.txt"), C;
C = ~A;
C += A * 0.5 * B * (A – 2 * B);
double x = C[0][0];
C[1][1] = x;
```

Primjer ulazne datoteke matrice (elementi matrice 4x3 po retcima):

```
12.5 3.0 9 2
4 5 6 7
8 9 10 11
```

Osim gore navedenih, potrebno je ostvariti i sljedeće elemente:

- metode koje izvode **supstituciju  unaprijed** i **supstituciju  unatrag**. Metode neka kao ulazni parametar (slobodni vektor s desne strane sustava) primaju vektor čija  je duljina jednaka dimenziji kvadratne matrice, a koji je i sam ostvaren objektom razreda Matrica. Također trebaju vraćati vektor kao objekt tipa Matrica (u kojemu će biti upisano rješenje odgovarajućeg postupka).
- metodu (metode) koje izvode **LU** i **LUP dekompoziciju** (kvadratne) matrice koristeći isti memorijski prostor za spremanje rezultantnih matrica L i U. Izbor odgovarajuće metode može se riješiti nekim dodatnim kontrolnim parametrom. Obratiti pažnju na mogućnost pojave nule kao stožernog (pivot) elementa (metode moraju imati neki mehanizam otkrivanja i prijave pogreške korisniku!).
- metoda za računanje **inverzije** kvadratne matrice uz pomoć LUP dekompozicije. Inverzna matrica se računa stupac po stupac, s jednom LUP dekompozicijom in supstitucija unaprijed i unatrag, kako je pokazano na predavanjima (*u skripti: str. 3-30*). Metoda se može definirati kao unarni operator nad matricom. Posebnu  pažnju  obratiti  na  slučaj kada je matrica singularna (pojava nule za stožerni element).
- metoda za računanje **determinante** matrice. Ako se matrica A rastavi na L, U i P  matrice, onda se determinanta matrice A može izračunati kao umnožak determinanti pojedinih matrica: $det(A) = det(P^{-1})det(L)det(U)$. Pri tome, determinanta permutacijske matrice jednaka je 1 ako nije napravljena niti jedna permutacija redaka ili parni broj permutacija, odnosno -1 ako je napravljen neparan broj permutacija redaka. S druge strane, determinanta trokutastih matrica jednaka je umnošku elemenata na dijagonali. Dakle, determinantu matrice A možemo onda izračunati kao: $A = (-1)^S \left( \prod^n_{i=1}{l_{ii}} \right) \left( \prod^n_{i=i}{u_{ii}} \right)$, pri čemu S predstavlja broj permutacija koji je napravljen, *l* element L matrice i u element U matrice.
- nije obvezatno, ali može pomoći: metode koje manipuliraju stupcima matrice (postavljaju i vraćaju određeni stupac matrice pomoću objekta istoga tipa), jer se operacije te vrste često koriste u opisanim postupcima.

**Važno**: obratite pažnju na situacije u kojima se kao stožerni element može pojaviti nula ili neka jako mala vrijednost (na bilo kojem mjestu u vašoj implementaciji!). Program treba 'preživjeti' pojavu takvih okolnosti i prijaviti grešku. U tom slučaju ne smije se ispisati rješenje koje matematički nema smisla.

## Priprema za izvođenje

```python
import os

CD_KEY = "--HW01_IN_ROOT"
```

```python
if (
    CD_KEY not in os.environ
    or os.environ[CD_KEY] is None
    or len(os.environ[CD_KEY]) == 0
    or os.environ[CD_KEY] == "false"
):
    %cd ..
else:
    print(os.getcwd())

os.environ[CD_KEY] = "true"
```

    /mnt/data/projekti/faks/AIPR/dz/dz-01

### Učitavanje paketa

```python
import copy

from IPython.display import display, Markdown

from src.matrices.matrix import Matrix
from src.matrices.exceptions import MatrixIsSingular, NotSolvable
```

## Zadatci

### Zadatak 1

Kakva treba biti usporedba *double* varijabli kako bi uspoređivanje dalo očekivane rezultate?

**Odgovor**: Umjesto konkretnih vrijednosti moramo gledati kakva je udaljenost vrijednosti od 0.

Isprobajte operator `==` s elementima matrice kao necijelim brojevima, pomnožite i podijelite sa realnim brojem i usporedite s originalom.

**Odgovor**:

```python
t1_coefficient = 3.14
t1_matrix = Matrix.from_array(
    [
        [1.1, 2.1, 3.1],
        [4.1, 5.1, 6.1],
        [7.1, 8.1, 9.1]
    ]
)
```

```python
t1_matrix_ = t1_matrix * t1_coefficient
t1_matrix_ /= t1_coefficient
```

```python
t1_string_particle = "su" if (t1_matrix == t1_matrix_) else "nisu"

display(
    Markdown(
        "<br>Matrice<br><br>" +\
        t1_matrix.latex("square_bracket") +\
        "<br>i<br><br>" +\
        t1_matrix_.latex("square_bracket") +\
        "<br>" +\
        ("su" if (t1_matrix == t1_matrix_) else "nisu") +\
        " identične.<br>"
    )
)
```

Matrice

\begin{bmatrix}
	1.100 & 2.100 & 3.100 \\
	4.100 & 5.100 & 6.100 \\
	7.100 & 8.100 & 9.100 \\
\end{bmatrix}
i

\begin{bmatrix}
	1.100 & 2.100 & 3.100 \\
	4.100 & 5.100 & 6.100 \\
	7.100 & 8.100 & 9.100 \\
\end{bmatrix}
su identične.

### Zadatak 2

Riješite sustav

$$
\begin{equation}
    \begin{bmatrix}
        3 &  9 &  6 \\
        4 & 12 & 12 \\
        1 & -1 &  1
    \end{bmatrix}
    \vec{x} =
    \begin{bmatrix}
        12 \\
        12 \\
        1
    \end{bmatrix}
\end{equation}
$$

Odredite može li se riješiti LU odnosno LUP dekompozicijom.

**Odgovor**:

```python
t2_matrix = Matrix.from_array(
    [
        [3, 9, 6],
        [4, 12, 12],
        [1, -1, 1]
    ]
)
t2_values = Matrix.from_array([
    [12],
    [12],
    [1]
])
t2_lu_matrix = None

try:
    t2_lu_matrix = t2_matrix.lu()
    t2_equation_values = copy.deepcopy(t2_values)
except NotSolvable:
    display(
        Markdown(
            "<br>Matrica<br><br> $$ \\\\" +\
            t2_matrix.latex("square_bracket") +\
            "$$ <br> nije rješiva LU dekompozicijom."
        )
    )

    try:
        t2_lu_matrix, t2_p_matrix = t2_matrix.lup()
        t2_equation_values = t2_p_matrix @ t2_values
    except NotSolvable:
        display(
            Markdown(
                "<br>Matrica<br><br> $$ \\\\" +\
                t2_matrix.latex("square_bracket") +\
                "$$ <br> nije rješiva ni LUP dekompozicijom."
            )
        )
```

Matrica

 $$ \\\begin{bmatrix}
	3 & 9 & 6 \\
	4 & 12 & 12 \\
	1 & -1 & 1 \\
\end{bmatrix}$$
 nije rješiva LU dekompozicijom.

```python
if t2_lu_matrix is not None:
    t2_solution = t2_lu_matrix.solve(t2_equation_values)

    display(
        Markdown(
            "Rješenje sustava<br> $$ \\\\" +\
            t2_matrix.latex("square_bracket") +\
            "\\vec{x} = " +\
            t2_values.latex("square_bracket") +\
            "$$ je $$\\\\ \\vec{x} = " +\
            t2_solution.T.latex("square_bracket") +\
            "$$<br>"
        )
    )
else:
    display(Markdown("<br>Zbog toga što ni jedna dekompozicija nije uspjela, sustav nije rješiv.<br>"))
```

Rješenje sustava
 $$ \\\begin{bmatrix}
	3 & 9 & 6 \\
	4 & 12 & 12 \\
	1 & -1 & 1 \\
\end{bmatrix}\vec{x} = \begin{bmatrix}
	12 \\
	12 \\
	1 \\
\end{bmatrix}$$ je $$\\ \vec{x} = \begin{bmatrix}
	3 \\
	1 \\
	-1 \\
\end{bmatrix}$$

### Zadatak 3

Matricu

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

rastavite na LU odnosno LUP.

```python
t3_matrix = Matrix.from_array(
    [
        [1, 2, 3],
        [4, 5, 6],
        [7, 8, 9]
    ]
)
```

**Odgovor**:

```python
try:
    t3_lu_matrix, t3_p_matrix = t3_matrix.lup()

    display(
        Markdown(
            "LU matrica matrice $$ \\\\" +\
            t3_matrix.latex("square_bracket") +\
            "$$ <br> je $$ \\\\" +\
            t3_lu_matrix.latex("square_bracket") +\
            "$$<br>"
        )
    )
except NotSolvable:
    display(
        Markdown(
            "Matrica $$ \\\\" +\
            t3_matrix.latex("square_bracket") +\
            "$$ <br> je singularna.<br>"
        )
    )
```

Matrica $$ \\\begin{bmatrix}
	1 & 2 & 3 \\
	4 & 5 & 6 \\
	7 & 8 & 9 \\
\end{bmatrix}$$
 je singularna.

Ako je ovom matricom predstavljen sustav jednadžbi, može li se sustav riješiti? (sami definirajte slobodni vektor).

**Odgovor**: Sustav nije riješiv jer je matrica singularna.

### Zadatak 4

Sustav

$$
\begin{equation}
    \begin{bmatrix}
    0.000001 & 3000000 & 2000000 \\
     1000000 & 2000000 & 3000000 \\
     2000000 & 1000000 & 2000000
    \end{bmatrix} \vec{x} =
    \begin{bmatrix}
        12000000.000001 \\
        14000000 \\
        10000000
    \end{bmatrix}
\end{equation}
$$

riješite LU te LUP dekompozicijom.

```python
t4_matrix = Matrix.from_array(
    [
        [0.000001, 3000000, 2000000],
        [1000000, 2000000, 3000000],
        [2000000, 1000000, 2000000]
    ]
)
t4_values = Matrix.from_array(
    [
        [12000000.000001],
        [14000000],
        [10000000]
    ]
)
```

**Odgovor**:

```python
try:
    t4_lu_matrix = t4_matrix.lu()
    t4_lu_solution = t4_lu_matrix.solve(t4_values)

    display(
        Markdown(
            "Rješenje sustava $$ \\\\" +\
            t4_matrix.latex("square_bracket", 6) +\
            " \\vec{x} = " +\
            t4_values.latex("square_bracket", 6) +\
            "$$ LU dekompozicijom je $$ \\\\ \\vec{x} = " +\
            t4_lu_solution.T.latex("square_bracket") +\
            "$$<br>"

        )
    )
except NotSolvable:
    display(
        Markdown(
            "Matrica $$ \\\\" +\
            t4_matrix.latex("square_bracket", 8) +\
            "$$ nije riješiva LU dekompozicijom."
        )
    )

```

Rješenje sustava $$ \\\begin{bmatrix}
	0.000001 & 3000000 & 2000000 \\
	1000000 & 2000000 & 3000000 \\
	2000000 & 1000000 & 2000000 \\
\end{bmatrix} \vec{x} = \begin{bmatrix}
	12000000.000001 \\
	14000000 \\
	10000000 \\
\end{bmatrix}$$ LU dekompozicijom je $$ \\ \vec{x} = \begin{bmatrix}
	1.000 \\
	1.999 \\
	3.001 \\
\end{bmatrix}$$

```python
try:
    t4_lu_matrix_, t4_p_matrix_ = t4_matrix.lup()
    t4_lup_solution = t4_lu_matrix_.solve(t4_p_matrix_ @ t4_values)

    display(
        Markdown(
            "Rješenje sustava $$ \\\\" +\
            t4_matrix.latex("square_bracket", 6) +\
            " \\vec{x} = " +\
            t4_values.latex("square_bracket", 6) +\
            "$$ LUP dekompozicijom je $$ \\\\ \\vec{x} =" +\
            t4_lup_solution.T.latex("square_bracket") +\
            "$$<br>"
        )
    )
except NotSolvable as ns:
    display(
        Markdown(
            "Matrica $$ \\\\" +\
            t4_matrix.latex("square_bracket", 8) +\
            "$$ nije riješiva LUP dekompozicijom."
        )
    )

```

Rješenje sustava $$ \\\begin{bmatrix}
	0.000001 & 3000000 & 2000000 \\
	1000000 & 2000000 & 3000000 \\
	2000000 & 1000000 & 2000000 \\
\end{bmatrix} \vec{x} = \begin{bmatrix}
	12000000.000001 \\
	14000000 \\
	10000000 \\
\end{bmatrix}$$ LUP dekompozicijom je $$ \\ \vec{x} =\begin{bmatrix}
	1.000 \\
	2.000 \\
	3.000 \\
\end{bmatrix}$$

Objasnite razliku u rješenjima! (očituje se prilikom uporabe *double* varijabli)

**Odgovor**: LUP dekompozicija je nešto numerički stabilnija od LU dekompozicije, pa smo zato dobili ljepše brojke za neuredan skup jednadžbi.

### Zadatak 5

Sustav

$$
\begin{equation}
    \begin{bmatrix}
        0 & 1 & 2 \\
        2 & 0 & 3 \\
        3 & 5 & 1
    \end{bmatrix} \vec{x} =
    \begin{bmatrix}
        6 \\
        9 \\
        3
    \end{bmatrix}
\end{equation}
$$

riješite odgovarajućom metodom.

```python
t5_matrix = Matrix.from_array(
    [
        [0, 1, 2],
        [2, 0, 3],
        [3, 5, 1]
    ]
)
t5_values = Matrix.from_array(
    [
        [6],
        [9],
        [3]
    ]
)
```

**Odgovor**:

```python
try:
    t5_lu_matrix, t5_p_matrix = t5_matrix.lup()
    t5_solution = t5_lu_matrix.solve(t5_p_matrix @ t5_values)

    display(
        Markdown(
            "Rješenje sustava $$ \\\\" +\
            t5_matrix.latex("square_bracket") +\
            " \\vec{x} = " +\
            t5_values.latex("square_bracket") +\
            "$$ je $$ \\\\ \\vec{x} =" +\
            t5_solution.T.latex("square_bracket") +\
            "$$<br>"
        )
    )
except NotSolvable:
    display(
        Markdown(
            "Matrica $$ \\\\" +\
            tt_matrix.latex("square_bracket") +\
            "$$ <br> je singularna.<br>"
        )
    )
```

Rješenje sustava $$ \\\begin{bmatrix}
	0 & 1 & 2 \\
	2 & 0 & 3 \\
	3 & 5 & 1 \\
\end{bmatrix} \vec{x} = \begin{bmatrix}
	6 \\
	9 \\
	3 \\
\end{bmatrix}$$ je $$ \\ \vec{x} =\begin{bmatrix}
	-0.000 \\
	0.000 \\
	3.000 \\
\end{bmatrix}$$

Objasnite razliku između dobivenog i točnog rješenja.

**Odgovor**: Ako se malo zagledamo, vidimo da je trivijalno rješenje

$$
\begin{equation}
    \vec{x} =
    \begin{bmatrix}
        0 \\
        0 \\
        3
    \end{bmatrix}
\end{equation}
$$

Razlog zašto smo imali problema su nule na dijagonali. Radi tih nula smo morali koristiti LUP dekompoziciju - već bi se u prvom koraku program srušio kad bi pokušao dijeliti s $A_{00}$. Ali čak i kad smo koristili LUP dekompoziciju, pivoti su bili dovoljno nekvalitetni (prosti brojevi različiti od $2$) da nam daju neprecizno rješenje.

Možemo probati rješiti sustav matricama realnih brojeva:

```python
t5_matrix_ = copy.deepcopy(t5_matrix).float()
t5_values_ = copy.deepcopy(t5_values).float()
```

```python
try:
    t5_lu_matrix_, t5_p_matrix_ = t5_matrix_.lup()
    t5_solution_ = t5_lu_matrix_.solve(t5_p_matrix_ @ t5_values_).float()

    display(
        Markdown(
            "Rješenje sustava $$ \\\\" +\
            t5_matrix_.latex("square_bracket") +\
            " \\vec{x} = " +\
            t5_values_.latex("square_bracket") +\
            "$$ je $$ \\\\ \\vec{x} =" +\
            t5_solution_.T.latex("square_bracket") +\
            "$$<br>"
        )
    )
except NotSolvable:
    display(
        Markdown(
            "Matrica $$ \\\\" +\
            tt_matrix_.latex("square_bracket") +\
            "$$ <br> je singularna.<br>"
        )
    )
```

Rješenje sustava $$ \\\begin{bmatrix}
	0 & 1 & 2 \\
	2 & 0 & 3 \\
	3 & 5 & 1 \\
\end{bmatrix} \vec{x} = \begin{bmatrix}
	6 \\
	9 \\
	3 \\
\end{bmatrix}$$ je $$ \\ \vec{x} =\begin{bmatrix}
	0 \\
	0 \\
	3.000 \\
\end{bmatrix}$$

Vidimo da smo sad dobili precizno rješenje.

### Zadatak 6

Rješavanje sustava

$$
\begin{equation}
    \begin{bmatrix}
      4000000000 &   1000000000 &   3000000000 \\
               4 &            2 &            7 \\
    0.0000000003 & 0.0000000005 & 0.0000000002
    \end{bmatrix} \vec{x} =
    \begin{bmatrix}
          9000000000 \\
                  15 \\
        0.0000000015
    \end{bmatrix}
\end{equation}
$$

moglo bi zadati problema vašoj implementaciji. O čemu to ovisi?

**Odgovor**: O preciznosti brojevnih varijabli koje se koriste unutar matrica.

Kako je moguće izbjeći ovaj problem, transformacijom zadanog sustava tako da rješenje ostane nepromijenjeno? (Napomena: postavite vrijednost epsilona za ovaj primjer na $10^{-6}$)

**Odgovor**: Najjednostavniji način bilo bi stvaranje transformacijske matrice kojom bi se reskalirao problem na nešto manje egzotično. Na primjer, ako definiramo transformacijsku matricu

$$
\begin{equation}
    \vec{T} =
    \begin{bmatrix}
        10^{-9} & 0 &      0 \\
              0 & 1 &      0 \\
              0 & 0 & 10^{10}
    \end{bmatrix}
\end{equation}
$$

Možemo problem transformirati u

$$
\vec{T}\vec{A}\vec{x} = \vec{T}\vec{y}
$$

tj.

$$
\begin{equation}
    \begin{bmatrix}
    4 & 1 & 3 \\
    4 & 2 & 7 \\
    3 & 5 & 2
    \end{bmatrix} \vec{x} =
    \begin{bmatrix}
          9 \\
         15 \\
         15
    \end{bmatrix}
\end{equation}
$$

čime ćemo dobiti preciznije rješenje. Demonstrirajmo to.

```python
t6_matrix = Matrix.from_array(
    [
        [4000000000, 1000000000, 3000000000],
        [4, 2, 7],
        [0.0000000003, 0.0000000005, 0.0000000002]
    ]
)
t6_matrix.epsilon = 1e-6
t6_values = Matrix.from_array(
    [
        [9000000000],
        [15],
        [0.0000000015]
    ]
)
t6_transformation = Matrix.from_array(
    [
        [1e-9, 0, 0],
        [0, 1, 0],
        [0, 0, 1e10]
    ]
)
```

**Demonstracija**:

```python
try:
    t6_lu_matrix, t6_p_matrix = t6_matrix.lup()
    t6_solution = t6_lu_matrix.solve(t6_p_matrix @ t6_values)

    display(
        Markdown(
            "Rješenje sustava $$ \\\\" +\
            t6_matrix.latex("square_bracket", 10) +\
            "\\vec{x} = " +\
            t6_values.latex("square_bracket", 10) +\
            "$$<br> je $$ \\\\" +\
            t6_solution.T.latex("square_bracket") +\
            "$$<br>"
        )
    )
except NotSolvable:
    display(
        Markdown(
            "Matrica $$ \\\\" +\
            t6_matrix.latex("square_bracket", 10) +\
            "$$ nije rješiva."
        )
    )
```

Matrica $$ \\\begin{bmatrix}
	4000000000 & 1000000000 & 3000000000 \\
	4 & 2 & 7 \\
	0.0000000003 & 0.0000000005 & 0.0000000002 \\
\end{bmatrix}$$ nije rješiva.

Sada ćemo napraviti isto ali primijeniti transformaciju.

```python
t6_matrix_ = t6_transformation @ t6_matrix
t6_values_ = t6_transformation @ t6_values
```

Sada imamo sustav

```python
display(
    Markdown(
        "$$ \\\\" +\
        t6_matrix_.latex("square_bracket") +\
        "\\vec{x} = " +\
        t6_values_.latex("square_bracket") +\
        "$$<br>"
    )
)
```

$$ \\\begin{bmatrix}
	4 & 1 & 3 \\
	4 & 2 & 7 \\
	3 & 5 & 2 \\
\end{bmatrix}\vec{x} = \begin{bmatrix}
	9 \\
	15 \\
	15 \\
\end{bmatrix}$$

koji je lakše riješiti:

```python
try:
    t6_lu_matrix_, t6_p_matrix_ = t6_matrix_.lup()
    t6_solution_ = t6_lu_matrix_.solve(t6_p_matrix_ @ t6_values_)

    display(
        Markdown(
            "Rješenje sustava $$ \\\\" +\
            t6_matrix_.latex("square_bracket") +\
            "\\vec{x} = " +\
            t6_values_.latex("square_bracket") +\
            "$$<br> je $$ \\\\" +\
            t6_solution_.T.latex("square_bracket") +\
            "$$<br>"
        )
    )
except NotSolvable:
    display(
        Markdown(
            "Matrica $$ \\\\" +\
            t6_matrix_.latex("square_bracket") +\
            "$$ nije rješiva."
        )
    )
```

Rješenje sustava $$ \\\begin{bmatrix}
	4 & 1 & 3 \\
	4 & 2 & 7 \\
	3 & 5 & 2 \\
\end{bmatrix}\vec{x} = \begin{bmatrix}
	9 \\
	15 \\
	15 \\
\end{bmatrix}$$
 je $$ \\\begin{bmatrix}
	1 \\
	2 \\
	1 \\
\end{bmatrix}$$

Ovog puta smo uspjeli riješiti sustav.

### Zadatak 7

Korištenjem LUP dekompozicije izračunajte inverz

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

te ispišite dobiveni rezultat.

```python
p7_matrix = Matrix.from_array(
    [
        [1, 2, 3],
        [4, 5, 6],
        [7, 8, 9]
    ]
)
```

**Odgovor**:

```python
try:
    p7_inverse = p7_matrix.inverse

    display(
        Markdown(
            "Inverz matrice $$ \\\\" +\
            p7_matrix.latex("square_bracket") +\
            "$$<br> je $$ \\\\" +\
            p7_inverse.latex("square_bracket") +\
            "$$<br>"
        )
    )
except MatrixIsSingular:
    display(
        Markdown(
            "Matrica $$ \\\\" +\
            p7_matrix.latex("square_bracket") +\
            "$$<br> je singularna."
        )
    )
```

Matrica $$ \\\begin{bmatrix}
	1 & 2 & 3 \\
	4 & 5 & 6 \\
	7 & 8 & 9 \\
\end{bmatrix}$$
 je singularna.

### Zadatak 8

Korištenjem LUP dekompozicije izračunajte inverz

$$
\begin{equation}
    \begin{bmatrix}
         4 & -5 & -2 \\
         5 & -6 & -2 \\
        -8 &  9 &  3
    \end{bmatrix}
\end{equation}
$$

te ispišite dobiveni rezultat.

```python
p8_matrix = Matrix.from_array(
    [
        [4, -5, -2],
        [5, -6, -2],
        [-8, 9, 3]
    ]
).float()
```

**Odgovor**:

```python
try:
    p8_inverse = p8_matrix.inverse

    display(
        Markdown(
            "Inverz matrice $$ \\\\" +\
            p8_matrix.latex("square_bracket") +\
            "$$<br> je $$ \\\\" +\
            p8_inverse.latex("square_bracket") +\
            "$$<br>"
        )
    )
except MatrixIsSingular:
    display(
        Markdown(
            "Matrica $$ \\\\" +\
            p8_matrix.latex("square_bracket") +\
            "$$<br> je singularna."
        )
    )
```

Inverz matrice $$ \\\begin{bmatrix}
	4 & -5 & -2 \\
	5 & -6 & -2 \\
	-8 & 9 & 3 \\
\end{bmatrix}$$
 je $$ \\\begin{bmatrix}
	0 & -3 & -2 \\
	1 & -4 & -2 \\
	-3 & 4 & 1 \\
\end{bmatrix}$$

### Zadatak 9

Korištenjem LUP dekompozicije izračunajte determinantu matrice

$$
\begin{equation}
    \begin{bmatrix}
         4 & -5 & -2 \\
         5 & -6 & -2 \\
        -8 &  9 &  3
    \end{bmatrix}
\end{equation}
$$

```python
p9_matrix = Matrix.from_array(
    [
        [4, -5, -2],
        [5, -6, -2],
        [-8, 9, 3]
    ]
)
```

**Odgovor**:

```python
display(
    Markdown(
        "<br>$$ \\\\" +\
        p9_matrix.latex("vertical_bar") +\
        f" = {p9_matrix.determinant}$$<br>"
    )
)
```

$$ \\\begin{vmatrix}
	4 & -5 & -2 \\
	5 & -6 & -2 \\
	-8 & 9 & 3 \\
\end{vmatrix} = 1$$

### Zadatak 10

Korištenjem LUP dekompozicije izračunajte determinantu matrice

$$
\begin{equation}
    \begin{bmatrix}
        3 &  9 &  6 \\
        4 & 12 & 12 \\
        1 & -1 &  1
    \end{bmatrix}
\end{equation}
$$

```python
p10_matrix = Matrix.from_array(
    [
        [3, 9, 6],
        [4, 12, 12],
        [1, -1, 1]
    ]
)
```

**Odgovor**:

```python
display(
    Markdown(
        "<br>$$ \\\\" +\
        p10_matrix.latex("vertical_bar") +\
        f" = {p10_matrix.determinant}$$<br>"
    )
)
```

$$ \\\begin{vmatrix}
	3 & 9 & 6 \\
	4 & 12 & 12 \\
	1 & -1 & 1 \\
\end{vmatrix} = 48$$


## Capítulo 8

```python
from sympy.physics.mechanics import ReferenceFrame,Point,dynamicsymbols
from sympy.physics.mechanics import Point,RigidBody,inertia, KanesMethod
from sympy import latex,pprint,symbols,init_printing
from sympy.algebras.quaternion import Quaternion
from sympy.physics.mechanics import LagrangesMethod, Lagrangian
from scipy.integrate import odeint
from sympy import lambdify
import matplotlib.pyplot as plt
from matplotlib import animation
import numpy as np
import sys
sys.path.append("../tools")
from vis import Visualizer
%matplotlib notebook
init_printing() # Para visualizar símbolos

```

```python
# Definición del modelo para el ejemplo del robot scara

# Parámetros del modelo
l1,l2,l3,rb,rc,g=symbols('l_1,l_2,l_3,r_b,r_c,g')
# Parámetros inerciales (note que se usan las variables ib e ic  para la inercia en z).
mb,mc,ib,ic=symbols('m_b,m_c,i_b,i_c')
# Variables de movimiento
q1,q2=dynamicsymbols('q_1,q_2')

# Defina los marcos y puntos de interés en el robot
a=ReferenceFrame('A')
o=Point('O')
b=a.orientnew('B','Axis',(q1,a.z))
p=o.locatenew('P',l1*a.z)
bstar=p.locatenew('Bstar',rb*b.x)
c=b.orientnew('C','Axis',(q2,b.z))
q=p.locatenew('Q',l2*b.x)
cstar=q.locatenew('Cstar',rc*c.x)
r=q.locatenew('R',l3*c.x)

# Defina los cuerpos rígidos
#RigidBody(name, masscenter, frame, mass, inertia)
inertia_b=inertia(b,0,0,ib)
inertia_c=inertia(c,0,0,ic)
body_b=RigidBody('B', bstar, b, mb, (inertia_b,bstar))
body_c=RigidBody('C', cstar, c, mc, (inertia_c,cstar))

# Construya un objecto de visualización con el marco de referencia inercial y punto de origen
vis=Visualizer(a,o)
# Agrege marcos y puntos para ser visualizados (marco,punto,geometría)
vis.add(a,o,shape='../Visualizacion/assets/scara_base.stl')
vis.add(b,p,shape='../Visualizacion/assets/scara_brazo.stl')
vis.add(c,q,shape='../Visualizacion/assets/scara_brazo.stl')
vis.add(c,r,shape='../Visualizacion/assets/scara_cilindro.stl')

#vis.add(b,o,shape='assets/Atraccion_Parque_link1.stl')
#vis.add(c,p,shape='assets/Atraccion_Parque_disk.stl')
vis.add(a,o,frame_scale=100)
vis.add(b,p,frame_scale=100)
vis.add(c,q,frame_scale=100)
vis.add(c,r,frame_scale=100)

# Modifique las variables de movimiento y verifique el cambio en la posición y
# orientación de los marcos
vis.plot({l1:220,l2:210,l3:210,q1:0,q2:0})
```

    <IPython.core.display.Javascript object>

```python
## 8.1 Ecuaciones iterativas de Newton-Euler
```

```python
# Primero calcule la cinemática de los cuerpos
# Para esto defina las velocidades de los puntos en cada marco de referencia
p.set_vel(a,0)
q.set_vel(b,0)
cstar.set_vel(c,0)
bstar.set_vel(b,0)
q.v2pt_theory(p,a,b)
bstar.v2pt_theory(p,a,b)
cstar.v2pt_theory(q,b,c)
cstar.v2pt_theory(q,a,c)

# Una vez definidas las velocidades puede calcular las velocidades en el marco A
# Verifique los resultados con los siguientes comandos
#b.ang_vel_in(a)
#c.ang_vel_in(a)
#b.ang_acc_in(a)
#c.ang_acc_in(a)
#bstar.vel(a)
#cstar.vel(a)
#bstar.acc(a)
cstar.acc(a)

```

```python
#Usando ecuaciones de Newton-Euler (F=ma, M=Icm*alpha+ w x Icm w)
# desde el último objeto en la cadena cinemática puede analizar y resolver
# la cinética.

#Primero para el cuerpo C
# Encuentre la fuerza de reacción F_bc: sum(Fc)=,
F_bc=mc*g*a.z+mc*cstar.acc(a)
#
M_bc=q.pos_from(cstar).cross(F_bc)+body_c.angular_momentum(cstar,a).dt(a)
M_bc.express(b).simplify()
```

```python
#Calcule el momento asociado al motor
M2=M_bc.express(b).dot(b.z)
M2
```

```python
#Ahora de la sumatoria de fuerzas para el cuerpo B
F_ab=mb*g*a.z+mb*bstar.acc(a)+F_bc
F_ab
```

```python
# Y de la sumatoria de momentos para el cuerpo B
M_ab=-q.pos_from(bstar).cross(-F_bc)\
+p.pos_from(bstar).cross(F_ab)\
-body_b.angular_momentum(bstar,a).dt(a)
M_ab.express(b).simplify()
```

```python
#Calcule el momento asociado al motor
M_ab.dot(b.z).simplify()
```

## 8.2 Formulacion de Kane para sistemas multicuerpo

```python
# Defina nuevamente el modelo del robot Scara, esta vez
# incluya con rapideces generalizadas para definir la cinemática usando
# la formulacion de Kane.

#Coordenadas generalizadas
q1,q2,u1,u2=dynamicsymbols('q1,q2,u1,u2')
#Derivadas qdot y udot
q1d,q2d,u1d,u2d=dynamicsymbols('q1 q2 u1 u2',1)
# Parámetros del modelo
l1,l2,l3,rb,rc,g=symbols('l_1,l_2,l_3,r_b,r_c,g')
# Parámetros inerciales (note que se usan las variables ib e ic  para la inercia en z).
mb,mc,ib,ic=symbols('m_b,m_c,i_b,i_c')
# Defina variables para los momentos de los motores
M1,M2=symbols('M1,M2')

# Defina los marcos y puntos de interés en el robot
a=ReferenceFrame('A')
o=Point('O')
b=a.orientnew('B','Axis',(q1,a.z))
p=o.locatenew('P',l1*a.z)
bstar=p.locatenew('Bstar',rb*b.x)
c=b.orientnew('C','Axis',(q2,b.z))
q=p.locatenew('Q',l2*b.x)
cstar=q.locatenew('Cstar',rc*c.x)
r=q.locatenew('R',l3*c.x)
# Defina los cuerpos rígidos
#RigidBody(name, masscenter, frame, mass, inertia)
inertia_b=inertia(b,0,0,ib)
inertia_c=inertia(c,0,0,ic)
body_b=RigidBody('B', bstar, b, mb, (inertia_b,bstar))
body_c=RigidBody('C', cstar, c, mc, (inertia_c,cstar))

p.set_vel(a,0)
q.set_vel(b,0)
cstar.set_vel(c,0)
bstar.set_vel(b,0)
q.v2pt_theory(p,a,b)
bstar.v2pt_theory(p,a,b)
cstar.v2pt_theory(q,b,c)
cstar.v2pt_theory(q,a,c)
b.set_ang_vel(a,u1*a.z)
c.set_ang_vel(b,u2*a.z)

# Puede revisar la cinemática del sistema expresada en rapideces generalizadas
# e.g.
# b.ang_vel_in(a)
# c.ang_vel_in(a).express(b)
# b.ang_acc_in(a).express(b)
# c.ang_acc_in(a).express(a)
# bstar.vel(a)
# cstar.vel(a)
# bstar.acc(a)
cstar.acc(a)

```

```python
# Ecuaciones diferenciales cinemáticas
kd = [q1d-u1, q2d-u2]
km=KanesMethod(a,q_ind=[q1,q2],u_ind=[u1,u2],kd_eqs=kd)
# Tuplas con cuerpos y fuerzas (marcos y momentos)
ftuples=[(body_b.masscenter,body_b.mass*g*a.z),(body_c.masscenter,body_c.mass*g*a.z)]
mtuples=[(b,M1*b.z-M2*b.z),(c,M2*b.z)]
(fr,frstar)=km.kanes_equations([body_b,body_c],ftuples+mtuples)
```

```python
# Verifique fr y frstar
#fr
#frstar[1]
```

```python
# Genere las ecuaciones de movimiento en espacio de estados:
km.rhs()
```

$\displaystyle \left[\begin{matrix}\operatorname{u_{1}}{\left(t \right)}\\\operatorname{u_{2}}{\left(t \right)}\\\frac{M_{1} + l_{2} m_{c} r_{c} \left(\operatorname{u_{1}}{\left(t \right)} + \operatorname{u_{2}}{\left(t \right)}\right)^{2} \sin{\left(\operatorname{q_{2}}{\left(t \right)} \right)} - l_{2} m_{c} r_{c} \operatorname{u_{1}}^{2}{\left(t \right)} \sin{\left(\operatorname{q_{2}}{\left(t \right)} \right)} - \frac{\left(i_{c} + m_{c} \left(l_{2} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} + r_{c}^{2}\right)\right) \left(M_{2} - l_{2} m_{c} r_{c} \operatorname{u_{1}}^{2}{\left(t \right)} \sin{\left(\operatorname{q_{2}}{\left(t \right)} \right)} - \frac{\left(i_{c} + m_{c} \left(l_{2} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} + r_{c}^{2}\right)\right) \left(M_{1} + l_{2} m_{c} r_{c} \left(\operatorname{u_{1}}{\left(t \right)} + \operatorname{u_{2}}{\left(t \right)}\right)^{2} \sin{\left(\operatorname{q_{2}}{\left(t \right)} \right)} - l_{2} m_{c} r_{c} \operatorname{u_{1}}^{2}{\left(t \right)} \sin{\left(\operatorname{q_{2}}{\left(t \right)} \right)}\right)}{i_{b} + i_{c} + m_{b} r_{b}^{2} + m_{c} \left(l_{2}^{2} + 2 l_{2} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} + r_{c}^{2}\right)}\right)}{i_{c} + m_{c} r_{c}^{2} - \frac{\left(i_{c} + m_{c} \left(l_{2} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} + r_{c}^{2}\right)\right)^{2}}{i_{b} + i_{c} + m_{b} r_{b}^{2} + m_{c} \left(l_{2}^{2} + 2 l_{2} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} + r_{c}^{2}\right)}}}{i_{b} + i_{c} + m_{b} r_{b}^{2} + m_{c} \left(l_{2}^{2} + 2 l_{2} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} + r_{c}^{2}\right)}\\\frac{M_{2} - l_{2} m_{c} r_{c} \operatorname{u_{1}}^{2}{\left(t \right)} \sin{\left(\operatorname{q_{2}}{\left(t \right)} \right)} - \frac{\left(i_{c} + m_{c} \left(l_{2} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} + r_{c}^{2}\right)\right) \left(M_{1} + l_{2} m_{c} r_{c} \left(\operatorname{u_{1}}{\left(t \right)} + \operatorname{u_{2}}{\left(t \right)}\right)^{2} \sin{\left(\operatorname{q_{2}}{\left(t \right)} \right)} - l_{2} m_{c} r_{c} \operatorname{u_{1}}^{2}{\left(t \right)} \sin{\left(\operatorname{q_{2}}{\left(t \right)} \right)}\right)}{i_{b} + i_{c} + m_{b} r_{b}^{2} + m_{c} \left(l_{2}^{2} + 2 l_{2} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} + r_{c}^{2}\right)}}{i_{c} + m_{c} r_{c}^{2} - \frac{\left(i_{c} + m_{c} \left(l_{2} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} + r_{c}^{2}\right)\right)^{2}}{i_{b} + i_{c} + m_{b} r_{b}^{2} + m_{c} \left(l_{2}^{2} + 2 l_{2} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} + r_{c}^{2}\right)}}\end{matrix}\right]$

# 8.3 Uso de la formulacion de Lagrange en sistemas multicuerpo

```python
# Defina nuevamente el modelo del robot Scara

#Coordenadas generalizadas
q1,q2=dynamicsymbols('q1,q2')
#Derivadas qdot y udot
q1d,q2d=dynamicsymbols('q1 q2',1)
# Parámetros del modelo
l1,l2,l3,rb,rc,g=symbols('l_1,l_2,l_3,r_b,r_c,g')
# Parámetros inerciales (note que se usan las variables ib e ic  para la inercia en z).
mb,mc,ib,ic=symbols('m_b,m_c,i_b,i_c')
# Defina variables para los momentos de los motors
M1,M2=symbols('M1,M2')

# Defina los marcos y puntos de interes en el robot
a=ReferenceFrame('A')
o=Point('O')
b=a.orientnew('B','Axis',(q1,a.z))
p=o.locatenew('P',l1*a.z)
bstar=p.locatenew('Bstar',rb*b.x)
c=b.orientnew('C','Axis',(q2,b.z))
q=p.locatenew('Q',l2*b.x)
cstar=q.locatenew('Cstar',rc*c.x)
r=q.locatenew('R',l3*c.x)

p.set_vel(a,0)
q.set_vel(b,0)
cstar.set_vel(c,0)
bstar.set_vel(b,0)
q.v2pt_theory(p,a,b)
bstar.v2pt_theory(p,a,b)
cstar.v2pt_theory(q,b,c)
cstar.v2pt_theory(q,a,c)
b.set_ang_vel(a,q1d*a.z)
c.set_ang_vel(b,q2d*a.z)

# Defina los cuerpos rígidos
#RigidBody(name, masscenter, frame, mass, inertia)
inertia_b=inertia(b,0,0,ib)
inertia_c=inertia(c,0,0,ic)
body_b=RigidBody('B', bstar, b, mb, (inertia_b,bstar))
body_c=RigidBody('C', cstar, c, mc, (inertia_c,cstar))
```

```python
#En este caso todas las masas estan en el mismo plano
# se supone energía potencial 0.
Lb=Lagrangian(a,body_b)
Lc=Lagrangian(a,body_c)

# Tuplas con cuerpos y fuerzas (marcos y momentos)
ftuples=[(body_b.masscenter,body_b.mass*g*a.z),(body_c.masscenter,body_c.mass*g*a.z)]
mtuples=[(b,M1*b.z-M2*b.z),(c,M2*b.z)]

lm=LagrangesMethod(Lb+Lc,[q1,q2],ftuples+mtuples,frame=a)
lm.form_lagranges_equations()
rhs=lm.rhs()
# Ecuaciones de movimiento en espacio de estados
rhs

```

$\displaystyle \left[\begin{matrix}\frac{d}{d t} \operatorname{q_{1}}{\left(t \right)}\\\frac{d}{d t} \operatorname{q_{2}}{\left(t \right)}\\\frac{M_{1} - \frac{m_{c} \left(- 2 l_{2} r_{c} \left(\frac{d}{d t} \operatorname{q_{1}}{\left(t \right)} + \frac{d}{d t} \operatorname{q_{2}}{\left(t \right)}\right) \sin{\left(\operatorname{q_{2}}{\left(t \right)} \right)} \frac{d}{d t} \operatorname{q_{2}}{\left(t \right)} - 2 l_{2} r_{c} \sin{\left(\operatorname{q_{2}}{\left(t \right)} \right)} \frac{d}{d t} \operatorname{q_{1}}{\left(t \right)} \frac{d}{d t} \operatorname{q_{2}}{\left(t \right)}\right)}{2} - \frac{\left(i_{c} + \frac{m_{c} \left(2 l_{2} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} + 2 r_{c}^{2}\right)}{2}\right) \left(M_{2} - l_{2} m_{c} r_{c} \left(\frac{d}{d t} \operatorname{q_{1}}{\left(t \right)} + \frac{d}{d t} \operatorname{q_{2}}{\left(t \right)}\right) \sin{\left(\operatorname{q_{2}}{\left(t \right)} \right)} \frac{d}{d t} \operatorname{q_{1}}{\left(t \right)} + l_{2} m_{c} r_{c} \sin{\left(\operatorname{q_{2}}{\left(t \right)} \right)} \frac{d}{d t} \operatorname{q_{1}}{\left(t \right)} \frac{d}{d t} \operatorname{q_{2}}{\left(t \right)} - \frac{\left(M_{1} - \frac{m_{c} \left(- 2 l_{2} r_{c} \left(\frac{d}{d t} \operatorname{q_{1}}{\left(t \right)} + \frac{d}{d t} \operatorname{q_{2}}{\left(t \right)}\right) \sin{\left(\operatorname{q_{2}}{\left(t \right)} \right)} \frac{d}{d t} \operatorname{q_{2}}{\left(t \right)} - 2 l_{2} r_{c} \sin{\left(\operatorname{q_{2}}{\left(t \right)} \right)} \frac{d}{d t} \operatorname{q_{1}}{\left(t \right)} \frac{d}{d t} \operatorname{q_{2}}{\left(t \right)}\right)}{2}\right) \left(i_{c} + \frac{m_{c} \left(2 l_{2} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} + 2 r_{c}^{2}\right)}{2}\right)}{i_{b} + i_{c} + m_{b} r_{b}^{2} + \frac{m_{c} \left(2 l_{2}^{2} + 4 l_{2} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} + 2 r_{c}^{2}\right)}{2}}\right)}{i_{c} + m_{c} r_{c}^{2} - \frac{\left(i_{c} + \frac{m_{c} \left(2 l_{2} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} + 2 r_{c}^{2}\right)}{2}\right)^{2}}{i_{b} + i_{c} + m_{b} r_{b}^{2} + \frac{m_{c} \left(2 l_{2}^{2} + 4 l_{2} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} + 2 r_{c}^{2}\right)}{2}}}}{i_{b} + i_{c} + m_{b} r_{b}^{2} + \frac{m_{c} \left(2 l_{2}^{2} + 4 l_{2} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} + 2 r_{c}^{2}\right)}{2}}\\\frac{M_{2} - l_{2} m_{c} r_{c} \left(\frac{d}{d t} \operatorname{q_{1}}{\left(t \right)} + \frac{d}{d t} \operatorname{q_{2}}{\left(t \right)}\right) \sin{\left(\operatorname{q_{2}}{\left(t \right)} \right)} \frac{d}{d t} \operatorname{q_{1}}{\left(t \right)} + l_{2} m_{c} r_{c} \sin{\left(\operatorname{q_{2}}{\left(t \right)} \right)} \frac{d}{d t} \operatorname{q_{1}}{\left(t \right)} \frac{d}{d t} \operatorname{q_{2}}{\left(t \right)} - \frac{\left(M_{1} - \frac{m_{c} \left(- 2 l_{2} r_{c} \left(\frac{d}{d t} \operatorname{q_{1}}{\left(t \right)} + \frac{d}{d t} \operatorname{q_{2}}{\left(t \right)}\right) \sin{\left(\operatorname{q_{2}}{\left(t \right)} \right)} \frac{d}{d t} \operatorname{q_{2}}{\left(t \right)} - 2 l_{2} r_{c} \sin{\left(\operatorname{q_{2}}{\left(t \right)} \right)} \frac{d}{d t} \operatorname{q_{1}}{\left(t \right)} \frac{d}{d t} \operatorname{q_{2}}{\left(t \right)}\right)}{2}\right) \left(i_{c} + \frac{m_{c} \left(2 l_{2} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} + 2 r_{c}^{2}\right)}{2}\right)}{i_{b} + i_{c} + m_{b} r_{b}^{2} + \frac{m_{c} \left(2 l_{2}^{2} + 4 l_{2} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} + 2 r_{c}^{2}\right)}{2}}}{i_{c} + m_{c} r_{c}^{2} - \frac{\left(i_{c} + \frac{m_{c} \left(2 l_{2} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} + 2 r_{c}^{2}\right)}{2}\right)^{2}}{i_{b} + i_{c} + m_{b} r_{b}^{2} + \frac{m_{c} \left(2 l_{2}^{2} + 4 l_{2} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} + 2 r_{c}^{2}\right)}{2}}}\end{matrix}\right]$

```python
# Finalmente puede utilizar el modelo del sistema para realizar simulaciones
# de dinamica directa utilizando un integrador de ecuaciones diferenciales
# de primer orden.

# Defina las condiciones iniciales del sistema
x0=[0,0,0,0]
# Asigne valores numericos a los parámetros del modelo
# rhs.free_symbols (revise las variables libres del modelo)
parameters={M1:0,M2:0,mb:0.5,mc:0.5,ib:1e-3,ic:1e-3,l2:0.22,rb:0.1,rc:0.1}

#Modifique los valores de M1 y M2 para obtener diferentes resultados en
# la simulación
parameters[M1]=-0.1*q1d-0.1*(q1-0.5)
parameters[M2]=-0.1*q2d-0.1*(q2-0.5)

t=np.linspace(0,20,100)
# Cree una función numérica del modelo del sistema (i.e xdot=f(x))
rhs_fun=lambdify([q1,q2,q1d,q2d],rhs.subs(parameters))
ode_fun=lambda x,t: rhs_fun(x[0],x[1],x[2],x[3]).reshape(4)
x=odeint(ode_fun,x0,t)

plt.figure()
plt.plot(t,x)
plt.legend([q1,q2,q1d,q2d])

```

    <IPython.core.display.Javascript object>

    <matplotlib.legend.Legend at 0x1e47dd08940>

```python
# Y utilizar las herramientas de visualización para observar el movimiento

vis=Visualizer(a,o)
vis.add(a,o,shape='../Visualizacion/assets/scara_base.stl')
vis.add(b,p,shape='../Visualizacion/assets/scara_brazo.stl')
vis.add(c,q,shape='../Visualizacion/assets/scara_brazo.stl')
vis.add(c,r,shape='../Visualizacion/assets/scara_cilindro.stl')
vis.add(a,o,frame_scale=100)
vis.add(b,p,frame_scale=100)
vis.add(c,q,frame_scale=100)
vis.add(c,r,frame_scale=100)

vis.plot({l1:220,l2:parameters[l2]*1000,l3:210,q1:x0[0],q2:x0[0]})

def update(i,data):
    vis.plot({l1:220,l2:parameters[l2]*1000,l3:210,q1:data[i,0],q2:data[i,1]})

ani = animation.FuncAnimation(vis.fig, update, x.shape[0], fargs=(x,), interval=20/x.shape[0], blit=False)
```

    <IPython.core.display.Javascript object>

# 8.4 Derivacion de un conjunto eficiente de ecuaciones dinamicas

```python
# Defina nuevamente el modelo del robot Scara, esta vez
# con la rapidez generalizada u2=q1d+q2d para definir la cinematica usando
# la formulacion de Kane.

#Coordenadas generalizadas
q1,q2,u1,u2=dynamicsymbols('q1,q2,u1,u2')
#Derivadas qdot y udot
q1d,q2d,u1d,u2d=dynamicsymbols('q1 q2 u1 u2',1)
# Parámetros del modelo
l1,l2,l3,rb,rc,g=symbols('l_1,l_2,l_3,r_b,r_c,g')
# Parámetros inerciales (note que se usan las variables ib e ic  para la inercia en z).
mb,mc,ib,ic=symbols('m_b,m_c,i_b,i_c')
# Defina variables para los momentos de los motores
M1,M2=symbols('M1,M2')

# Defina los marcos y puntos de interés en el robot
a=ReferenceFrame('A')
o=Point('O')
b=a.orientnew('B','Axis',(q1,a.z))
p=o.locatenew('P',l1*a.z)
bstar=p.locatenew('Bstar',rb*b.x)
c=b.orientnew('C','Axis',(q2,b.z))
q=p.locatenew('Q',l2*b.x)
cstar=q.locatenew('Cstar',rc*c.x)
r=q.locatenew('R',l3*c.x)
# Defina los cuerpos rígidos
#RigidBody(name, masscenter, frame, mass, inertia)
inertia_b=inertia(b,0,0,ib)
inertia_c=inertia(c,0,0,ic)
body_b=RigidBody('B', bstar, b, mb, (inertia_b,bstar))
body_c=RigidBody('C', cstar, c, mc, (inertia_c,cstar))

p.set_vel(a,0)
q.set_vel(b,0)
cstar.set_vel(c,0)
bstar.set_vel(b,0)
q.v2pt_theory(p,a,b)
bstar.v2pt_theory(p,a,b)
cstar.v2pt_theory(q,b,c)
cstar.v2pt_theory(q,a,c)
b.set_ang_vel(a,u1*a.z)
c.set_ang_vel(b,q2d*a.z)

# Ecuaciones diferenciales cinemáticas
kd = [q1d-u1, q1d+q2d-u2]
km=KanesMethod(a,q_ind=[q1,q2],u_ind=[u1,u2],kd_eqs=kd)
# Tuplas con cuerpos y fuerzas (marcos y momentos)
ftuples=[(body_b.masscenter,body_b.mass*g*a.z),(body_c.masscenter,body_c.mass*g*a.z)]
mtuples=[(b,M1*b.z-M2*b.z),(c,M2*b.z)]
(fr,frstar)=km.kanes_equations([body_b,body_c],ftuples+mtuples)

frstar

```

$\displaystyle \left[\begin{matrix}l_{2} m_{c} r_{c} \operatorname{u_{2}}^{2}{\left(t \right)} \sin{\left(\operatorname{q_{2}}{\left(t \right)} \right)} - l_{2} m_{c} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} \frac{d}{d t} \operatorname{u_{2}}{\left(t \right)} - \left(i_{b} + l_{2}^{2} m_{c} + m_{b} r_{b}^{2}\right) \frac{d}{d t} \operatorname{u_{1}}{\left(t \right)}\\- l_{2} m_{c} r_{c} \operatorname{u_{1}}^{2}{\left(t \right)} \sin{\left(\operatorname{q_{2}}{\left(t \right)} \right)} - l_{2} m_{c} r_{c} \cos{\left(\operatorname{q_{2}}{\left(t \right)} \right)} \frac{d}{d t} \operatorname{u_{1}}{\left(t \right)} - \left(i_{c} + m_{c} r_{c}^{2}\right) \frac{d}{d t} \operatorname{u_{2}}{\left(t \right)}\end{matrix}\right]$


```python
import modern_robotics as mr
import numpy as np
import sympy as sp
from sympy import*
from sympy.physics.mechanics import dynamicsymbols, mechanics_printing
mechanics_printing()
```

### Utilities

```python
def exp3(omega, theta):
    omega = skew(omega)
    R = sp.eye(3) + sp.sin(theta) * omega + (1 - sp.cos(theta)) * omega * omega
    return R

def skew(v):
    return Matrix([[0, -v[2], v[1]],
                    [v[2], 0, -v[0]],
                    [-v[1], v[0], 0]])

def exp6(twist, theta):
    omega = skew(twist[:3])
    v = Matrix(twist[3:])
    T = eye(4)
    T[:3,:3] = exp3(twist[:3], theta)
    T[:3,3] = (eye(3) * theta + (1 - cos(theta)) * omega +
              (theta-sin(theta)) * omega * omega) * v
    return T

def Ad(T):
    AdT = sp.zeros(6)
    R = sp.Matrix(T[:3, :3])
    AdT[:3, :3] = R
    AdT[3:, 3:] = R
    AdT[3:, :3] = skew(T[:3, 3]) * R
    return AdT

def rotX(theta):
    Rx = sp.eye(4)
    Rx[1,1] =    sp.cos(theta)
    Rx[1,2] =   -sp.sin(theta)
    Rx[2,1] =    sp.sin(theta)
    Rx[2,2] =    sp.cos(theta)
    return Rx

def rotZ(theta):
    Rz = sp.eye(4)
    Rz[0,0] =    sp.cos(theta)
    Rz[0,1] =   -sp.sin(theta)
    Rz[1,0] =    sp.sin(theta)
    Rz[1,1] =    sp.cos(theta)
    return Rz

def PsFromTsd(Tsd):
    #Finner Ps fra T_sd
    #T_sd gir konfigurasjonen vi vil ha end-effector framen, B, i.
    #B, og derav også M, er lik som i DH
    #s er plassert nederst på roboten med positiv z oppover, altså ikke som i DH. Bør kanskje endres til å være lik DH
    Pd = np.array([0,0,80,1])
    Ps = Tsd@Pd

    return Ps

```

### Task 3-3

```python
#Definerer S og M
"""
S1 = np.array([0,0,-1,0,0,0])
S2 = np.array([0,1,0,-400,0,25])
S3 = np.array([0,1,0,-855,0,25])
S4 = np.array([-1,0,0,0,-890,0])
S5 = np.array([0,1,0,-890,0,445])
S6 = np.array([-1,0,0,0,-890,0])
Slist = np.array([S1,S2,S3,S4,S5,S6]).T
print(Slist)

M = np.array([[0,0,-1,525],
              [0,1,0,0],
              [1,0,0,890],
              [0,0,0,1]])

thetasUp = [0,0,0,0,0,0]
thetasDown = [0,0,0,0,0,0]

#Limits til roboten slik den er gitt i oppgaven. Antar at ledd 5 har limit på +-90
theta_limits = [[-180,180],[-190+90,45+90],[-120-90, 156-90],[-180,180],[-90,90],[-180,180]]
"""

S1 = np.array([0,0,1,0,0,0])
S2 = np.array([0,-1,0,0,0,-25])
S3 = np.array([0,-1,0,-455,0,-25])
S4 = np.array([-1,0,0,0,490,0])
S5 = np.array([0,-1,0,-490,0,-445])
S6 = np.array([-1,0,0,0,490,0])
Slist = np.array([S1,S2,S3,S4,S5,S6]).T
print(Slist)

M = np.array([[0,0,-1,525],
              [0,-1,0,0],
              [-1,0,0,-490],
              [0,0,0,1]])

thetasUp = [0,0,0,0,0,0]
thetasDown = [0,0,0,0,0,0]

```

    [[   0    0    0   -1    0   -1]
     [   0   -1   -1    0   -1    0]
     [   1    0    0    0    0    0]
     [   0    0 -455    0 -490    0]
     [   0    0    0  490    0  490]
     [   0  -25  -25    0 -445    0]]

```python
#Her endres thetasGen for å teste forskjellige konfigurasjoner:
thetasGen = np.array([3,3,1,2,1,0])
Tsd = mr.FKinSpace(M,Slist,thetasGen)
print("T_sd\n", Tsd)

```

    T_sd
     [[ 4.4539e-01 -7.4000e-01 -5.0401e-01  2.5001e+02]
     [ 4.3277e-01 -3.1487e-01  8.4473e-01 -9.7469e+01]
     [-7.8380e-01 -5.9436e-01  1.8001e-01  1.4107e+02]
     [ 0.0000e+00  0.0000e+00  0.0000e+00  1.0000e+00]]

```python

Ps = PsFromTsd(Tsd)
print("Ps", Ps)
Psmerket = [Ps[0], Ps[1], Ps[2]]

#theta1
thetasUp[0] = -atan2(-Psmerket[1],Psmerket[0]) #minus foran fordi vinkelen er definert andre vei ##
thetasDown[0] = thetasUp[0]

#theta2 and theta3
a = np.sqrt(420**2+35**2)
c = 455
b = np.sqrt((np.sqrt(Psmerket[0]**2+Psmerket[1]**2)-25)**2 + Psmerket[2]**2)
print("a",a,"c",c,"b", b)
print("d", np.sqrt((np.sqrt(Psmerket[0]**2+Psmerket[1]**2)-25)**2))

psi = np.arccos(420/a) #Vinkelen mellom den faktiske armen og den vi tegna for å få en trekant(Pga 35mm offset i elbow). Se notatbok
phi = atan2(-Psmerket[2], sqrt(Psmerket[0]**2 + Psmerket[1]**2)-25) ##
print("args", -Psmerket[2], sqrt(Psmerket[0]**2 + Psmerket[1]**2)-25)

alpha = np.arccos((b**2+c**2-a**2)/(2*b*c))
beta = np.arccos((a**2+c**2-b**2)/(2*a*c))

print("alpha:", np.rad2deg(alpha), "beta:", np.rad2deg(beta), "phi:", phi, "psi:", np.rad2deg(psi))
thetasUp[1] = np.pi/2 - (alpha + phi)
thetasDown[1] = np.pi/2 - (phi-alpha)

print(thetasUp[1])

thetasUp[2] = np.pi/2 - (beta-psi)
thetasDown[2] = -(2*np.pi - (beta+psi) - np.pi/2)

print(thetasUp, thetasDown)

#Vi har XYX euler angles. De er egentlig (-X)Y(-X) fordi det er slik S'ene er definert,.

#Elbow down:
T1 = exp6(S1, -thetasDown[0])
T2 = exp6(S2, -thetasDown[1])
T3 = exp6(S3, -thetasDown[2])

R = (T3@T2@T1@Tsd@np.linalg.inv(M)) #R er den resterende rotasjonen vi ønsker å få fra de tre siste leddene, definert i s

thetasDown[3] = -atan2(R[1,0], -R[2,0]) #minus foran theta4 og 6 fordi de er i minus x retning
thetasDown[4] = -atan2(sqrt(1-R[0,0]**2), R[0,0])
thetasDown[5] = -atan2(R[0,1], R[0,2])

#Elbow up:
T1 = exp6(S1, -thetasUp[0])
T2 = exp6(S2, -thetasUp[1])
T3 = exp6(S3, -thetasUp[2])

R = (T3@T2@T1@Tsd@np.linalg.inv(M))

thetasUp[3] = -atan2(R[1,0], -R[2,0])
thetasUp[4] = -atan2(sqrt(1-R[0,0]**2), R[0,0])
thetasUp[5] = -atan2(R[0,1], R[0,2])
```

    Ps [209.6891 -29.8905 155.4671   1.    ]
    a 421.4558102577303 c 455 b 243.03814022740696
    d 186.8088044312549
    args -155.46706465409926 186.808804431255
    alpha: 66.42135383932616 beta: 31.905434198839078 phi: -0.694084160821108 psi: 4.763641690726143
    1.10560916948759
    [-0.141592653589793, 1.10560916948759, 1.097082682635196, 1.09683919743480, 1.03515054185725 - pi, -0.0851244511188330] [-0.141592653589793, 3.42415180574442, -4.072392872448108, -1.2790915209693 + pi, -0.925458806156948, 0.239742346967086 - pi]

```python
#testing av analytisk løsning:

#UP
thetasUpN = np.zeros(6)
thetasUpDeg = np.zeros(6)
for i in range(0,6):
    thetasUpN[i] = N(thetasUp[i])
    thetasUpDeg[i] = np.rad2deg(thetasUpN[i])

#print(thetas_deg, np.rad2deg(thetas_gen))

TupThetas = mr.FKinSpace(M,Slist,thetasUpN)
PUpReached = PsFromTsd(TupThetas)
#print(P_reached, P_s)

#DOWN
thetasDownN = np.zeros(6)
thetasDownDeg = np.zeros(6)
for i in range(0,6):
    thetasDownN[i] = N(thetasDown[i])
    thetasDownDeg[i] = np.rad2deg(thetasDownN[i])

#print(thetas_deg, np.rad2deg(thetas_gen))

TDownThetas = mr.FKinSpace(M,Slist,thetasDownN)
PDownReached = PsFromTsd(TDownThetas)

#fk_test = exp6(S4,thetas_num[3])@exp6(S5,thetas_num[4])@exp6(S6,thetas_num[5])@M
#R_test = rotX(thetas_num[3])@rotY(thetas_num[4])@rotX(thetas_num[5])
thetasCalc, asd = mr.IKinSpace(Slist,M,Tsd,[0,0,0,0,0,0],0.01,0.01)

Tsd, thetasGen, TupThetas, thetasUpN, TDownThetas, thetasDownN, Ps, PUpReached, PDownReached
```

    (array([[ 4.4539e-01, -7.4000e-01, -5.0401e-01,  2.5001e+02],
            [ 4.3277e-01, -3.1487e-01,  8.4473e-01, -9.7469e+01],
            [-7.8380e-01, -5.9436e-01,  1.8001e-01,  1.4107e+02],
            [ 0.0000e+00,  0.0000e+00,  0.0000e+00,  1.0000e+00]]),
     array([3, 3, 1, 2, 1, 0]),
     array([[ 4.4539e-01, -7.4000e-01, -5.0401e-01,  2.5001e+02],
            [ 4.3277e-01, -3.1487e-01,  8.4473e-01, -9.7469e+01],
            [-7.8380e-01, -5.9436e-01,  1.8001e-01,  1.4107e+02],
            [ 0.0000e+00,  0.0000e+00,  0.0000e+00,  1.0000e+00]]),
     array([-0.1416,  1.1056,  1.0971,  1.0968, -2.1064, -0.0851]),
     array([[ 4.4539e-01, -7.4000e-01, -5.0401e-01,  2.5001e+02],
            [ 4.3277e-01, -3.1487e-01,  8.4473e-01, -9.7469e+01],
            [-7.8380e-01, -5.9436e-01,  1.8001e-01,  1.4107e+02],
            [ 0.0000e+00,  0.0000e+00,  0.0000e+00,  1.0000e+00]]),
     array([-0.1416,  3.4242, -4.0724,  1.8625, -0.9255, -2.9019]),
     array([209.6891, -29.8905, 155.4671,   1.    ]),
     array([209.6891, -29.8905, 155.4671,   1.    ]),
     array([209.6891, -29.8905, 155.4671,   1.    ]))

```python
Tsd, thetasUpN, thetasDownN

```

    (array([[ 4.4539e-01, -7.4000e-01, -5.0401e-01,  2.5001e+02],
            [ 4.3277e-01, -3.1487e-01,  8.4473e-01, -9.7469e+01],
            [-7.8380e-01, -5.9436e-01,  1.8001e-01,  1.4107e+02],
            [ 0.0000e+00,  0.0000e+00,  0.0000e+00,  1.0000e+00]]),
     array([-0.1416,  1.1056,  1.0971,  1.0968, -2.1064, -0.0851]),
     array([-0.1416,  3.4242, -4.0724,  1.8625, -0.9255, -2.9019]))

```python

```


# Liouville Theorem Visualization

HamiltonianPendulum class to generate basic pendulum plots, applied to problems from Taylor, but now solving using Hamilton's equations.

* Last revised 16-Mar-2019 by Dick Furnstahl (furnstahl.1@osu.edu).

For a pendulum, the Lagrangian is

$\begin{align}
  \mathcal{L} = \frac12 m L^2 \dot\phi^2 - mgL(1 - \cos\phi)
\end{align}$

which tells us that the generalized momentum is

$\begin{align}
   \frac{\partial\mathcal{L}}{\partial \dot\phi} = m L^2 \dot\phi \equiv p_\phi
   \;.
\end{align}$

We can invert this equation to find $\dot\phi = p_\phi / m L^2$.
Constructing the Hamiltonian by Legendre transformation we find

$\begin{align}
  \mathcal{H} &= \dot\phi p_\phi  - \mathcal{L} \\
              &= \frac{p_\phi^2}{m L^2} - \frac12 m L^2 \dot\phi^2 + mgL(1 - \cos\phi) \\
              &= \frac{p_\phi^2}{2 m L^2} + mgL(1 - \cos\phi)
              \;.
\end{align}$

Thus $\mathcal{H}$ is simply $T + V$.  Hamilton's equations are

$\begin{align}
  \dot\phi &= \frac{\partial\mathcal{H}}{\partial p_\phi} = \frac{p_\phi}{m L^2} \\
  \dot p_\phi &= -\frac{\partial\mathcal{H}}{\partial \phi} = -mgL \sin\phi
  \;.
\end{align}$

A consequence of Liouville's theorem is that if we consider a region in phase space as a set of initial conditions and then evolve each point in that space, the region can change shape but its volume will not change.

We will try this out for the simple pendulum by first picking an area (e.g., a rectangle) in $p_\phi$ by $\phi$ and then selecting random points inside of it.  Each of the points is then propagated a time step by Hamilton's equations and plotted again.

```python
%matplotlib inline
```

```python
import numpy as np
from numpy.random import uniform, rand

from scipy.integrate import solve_ivp
from scipy.spatial import ConvexHull

import matplotlib.pyplot as plt
```

```python
# The dpi (dots-per-inch) setting will affect the resolution and how large
#  the plots appear on screen and printed.  So you may want/need to adjust
#  the figsize when creating the figure.
plt.rcParams['figure.dpi'] = 100.    # this is the default for notebook

# Change the common font size (smaller when higher dpi)
font_size = 10
plt.rcParams.update({'font.size': font_size})
```

## Pendulum class and utility functions

```python
class HamiltonianPendulum():
    """
    Pendulum class implements the parameters and Hamilton's equations for
     a simple pendulum (no driving or damping).

    Parameters
    ----------
    omega_0 : float
        natural frequency of the pendulum (\sqrt{g/l} where l is the
        pendulum length)
    mass : float
        mass of pendulum

    Methods
    -------
    dy_dt(t, y)
        Returns the right side of the differential equation in vector y,
        given time t and the corresponding value of y.
    """
    def __init__(self, L=1., mass=1., g=1.
                ):
        self.L = L
        self.g = g
        self.omega_0 = np.sqrt(g/L)
        self.mass = mass

    def dy_dt(self, t, y):
        """
        This function returns the right-hand sides of Hamilton's eqs
        [dq/dt dp/dt] --> [dphi/dt dp_phi/dt]

        Parameters
        ----------
        t : float
            time
        y : float
            A 2-component vector with y[0] = phi(t) and y[1] = p_phi(t)

        Returns
        -------

        """
        return [y[1] / (self.mass * self.L**2),
                -self.mass * self.g * self.L * np.sin(y[0])
               ]

    def solve_ode(self, t_pts, q_0, p_0,
                  abserr=1.0e-10, relerr=1.0e-10):
        """
        Solve the ODE given initial conditions.
        Specify smaller abserr and relerr to get more precision.
        """
        y = [q_0, p_0]
        solution = solve_ivp(self.dy_dt, (t_pts[0], t_pts[-1]),
                             y, t_eval=t_pts,
                             atol=abserr, rtol=relerr)
        q, p = solution.y

        return q, p
```

```python
def plot_y_vs_x(x, y, axis_labels=None, label=None, title=None,
                color=None, linestyle=None, semilogy=False, loglog=False,
                ax=None):
    """
    Generic plotting function: return a figure axis with a plot of y vs. x,
    with line color and style, title, axis labels, and line label
    """
    if ax is None:        # if the axis object doesn't exist, make one
        ax = plt.gca()

    if (semilogy):
        line, = ax.semilogy(x, y, label=label,
                            color=color, linestyle=linestyle)
    elif (loglog):
        line, = ax.loglog(x, y, label=label,
                          color=color, linestyle=linestyle)
    else:
        line, = ax.plot(x, y, label=label,
                    color=color, linestyle=linestyle)

    if label is not None:    # if a label if passed, show the legend
        ax.legend()
    if title is not None:    # set a title if one if passed
        ax.set_title(title)
    if axis_labels is not None:  # set x-axis and y-axis labels if passed
        ax.set_xlabel(axis_labels[0])
        ax.set_ylabel(axis_labels[1])

    return ax, line
```

```python
def start_stop_indices(t_pts, plot_start, plot_stop):
    start_index = (np.fabs(t_pts-plot_start)).argmin()  # index in t_pts array
    stop_index = (np.fabs(t_pts-plot_stop)).argmin()  # index in t_pts array
    return start_index, stop_index
```

```python

```

## Make Liouville theorem visualization

```python
# Labels for individual plot axes
phi_vs_time_labels = (r'$t$', r'$\phi(t)$')
p_phi_vs_time_labels = (r'$t$', r'$p_\phi(t)$')
phase_space_labels = (r'$\phi$', r'$p_\phi$')

L = 1.
g = 1.
mass = 1.

# Instantiate a pendulum
p1 = HamiltonianPendulum(L=L, g=g, mass=mass)

```

```python
# Generate initial conditions
num_samples = 300
phi_and_p_low = [1.5, 0.0]
phi_and_p_high = [2.0, 0.5]
out = uniform(low=phi_and_p_low, high=phi_and_p_high, size=(num_samples,2))

fig = plt.figure()
ax = fig.add_subplot(1,1,1)
ax.plot(out[:, 0], out[:, 1], '.')
ax.set_aspect(1)
ax.set_xlabel(r'$\phi$')
ax.set_ylabel(r'$p_\phi$')
ax.set_xlim(-3., 3.)
ax.set_ylim(-2., 2.)

fig.tight_layout()
```

```python
fig = plt.figure()
ax = fig.add_subplot(1,1,1)

for phi, p in out:
    ax.plot(phi, p, '.')

ax.set_xlabel(r'$\phi$')
ax.set_ylabel(r'$p_\phi$')
ax.set_xlim(-3., 3.)
ax.set_ylim(-2., 2.)
ax.set_aspect(1)

fig.tight_layout()
```

```python
# Common plotting time (generate the full time then use slices)
delta_t = 0.5
t_start = 0.
num_t = 15
t_end = t_start + (num_t - 1) * delta_t

t_pts = np.arange(t_start, t_end+delta_t, delta_t)

# start the plot!
#fig = plt.figure(figsize=(15,5.*num_t/3.))
fig = plt.figure(figsize=(15,5.*num_t/3.))
overall_title = 'Liouville theorem visualization with pendulum'

plot_num = 0
all_orbits = []
for phi_0, p_phi_0 in out:
    phi, p_phi = p1.solve_ode(t_pts, phi_0, p_phi_0)
    all_orbits.append([phi, p_phi])

for i in np.arange(num_t):
    ax_c = fig.add_subplot(num_t/3, 3, i+1)
    ax_c.set_title(rf'$t = {t_pts[i]:.1f}$')
    for j in np.arange(num_samples):
        phi_now, p_phi_now = all_orbits[j]
        ax_c.plot(phi_now[i], p_phi_now[i], '.')
        ax_c.set_xlim(-3., 3.)
        ax_c.set_ylim(-2., 2.)

fig.tight_layout()
#fig.suptitle(overall_title, va='top')

fig.savefig('Liouville_test.png', bbox_inches='tight')
```

```python

```


```python
%matplotlib inline
%config InlineBackend.figure_format ='retina'

import torch
import socialforce
```

(pedped-2d)=
# 2D

## Parametric

```python
V = socialforce.potentials.PedPedPotential2D(asymmetry=-1.0)
with socialforce.show.canvas(figsize=(12, 6), ncols=2) as (ax1, ax2):
    socialforce.show.potential_2d(V, ax1)
    socialforce.show.potential_2d_grad(V, ax2)
```

## Asymmetry

We use a multiplicative function to make a symmetric potential asymmetric.
Qualitativly, we would like an exponential function, but one that does not grow
exponentially at large distances. We use a softplus function normalized such
that it is one at the origin:

\begin{align}
    f_\textrm{asymmetry}(x_{\perp}) &= \frac{1}{\ln 2} \ln(1 + \exp(\textrm{asymmetry} \cdot x_{\perp}))
\end{align}

```python
# HIDE CODE
asymmetry_factor = socialforce.potentials.PedPedPotential2D.asymmetry_factor

x = torch.linspace(-5, 5, 300)
with socialforce.show.canvas() as ax:
    ax.plot(x, asymmetry_factor(0.0, x), label='no asymmetry')
    ax.plot(x, asymmetry_factor(1.0, x), label='asymmetry = 1.0')
    ax.plot(x, asymmetry_factor(0.5, x), label='asymmetry = 0.5')
    ax.plot(x, asymmetry_factor(-1.0, x), label='asymmetry = -1.0')
    ax.legend()
    ax.set_xlabel('$x_{\\perp}$')
    ax.set_ylabel('asymmetry factor')
```

## Fitting to Circle and ParallelOvertake Scenarios

The preferred speed needs to be varied. Otherwise the symmetry of the problem creates unrealistic scenarios where the two pedestrians get stuck.

```python
circle = socialforce.scenarios.Circle(ped_ped=V)
parallel = socialforce.scenarios.ParallelOvertake(ped_ped=V)
scenarios = circle.generate(5) + parallel.generate(5)
```

```python
true_experience = socialforce.Trainer.scenes_to_experience(scenarios)
V = socialforce.potentials.PedPedPotentialMLP2D()

with socialforce.show.canvas(figsize=(12, 6), ncols=2) as (ax1, ax2):
    socialforce.show.potential_2d(V, ax1)
    socialforce.show.potential_2d_grad(V, ax2)
```

```python
simulator = socialforce.Simulator(ped_ped=V)
opt = torch.optim.SGD(V.parameters(), lr=1.0)
socialforce.Trainer(simulator, opt).loop(20, true_experience, log_interval=5)
```

```python
with socialforce.show.canvas(figsize=(12, 6), ncols=2) as (ax1, ax2):
    socialforce.show.potential_2d(V, ax1)
    socialforce.show.potential_2d_grad(V, ax2)
```

```python

```


```python
'''
Szorgalmi feladat: Példamegoldás SymPy-vel
Peter Bence X89O8X
'''
import numpy as np
import matplotlib.pyplot as plt
import sympy as sp
sp.init_printing(use_unicode=True)
%matplotlib inline

M = 5 # test tomege
```

```python
'''
F(t) fuggveny implementalasa
m = tomeg (kg)
t = ido (s)
F = ero (N) <-- F = a * m
a = ?
a = dv/dt
Feladat azt is definialta, ha az ero nagysaga eleri a 0-at akkor 0 is marad
'''
def F(t):
    F_t = (10-0.5*(t**2))
    if F_t >= 0:
        return F_t
    else:
        return 0

'''
a(t) fuggvenyt ki lehet szamolni az F = a*m bol
tehat ha tudjuk a test tomeget, akkor ki lehet szamolni
a gyorsulast az adott t idopontban
'''
def a(t):
    return ((F(t)/M))

'''
az a(t) bol megkaphatjuk a v(t) fuggvenyt
ha integraljuk a(t) fuggvenyt kozelitessel
az integralast lehet kozeliteni a simpson forumaval
a es b a t(s) idot jelenti
f pedig integralni kivant fuggveny
'''
def simpson(f, a, b):
    return (((b-a)/6)*(f(a)+4*f((a+b)/2)+f(b)))

'''
Integralas osszetett simpson formulaval
f kozeliteni kivant fuggveny
x = ido tablazat
f pedig integralni kivant fuggveny
'''
def osszSimpson(f, x):
    ret = 0.0
    for i in range(len(x)-1):
        ret += simpson(f, x[i], x[i+1])
    return ret
```

```python
t_max = 10.0 # max t(s)
dt = 0.01 # dt idokoz
t_vec = np.arange(0.0, t_max, dt, dtype=np.float16)
F_t = np.vectorize(F)(t_vec)
a_t = np.vectorize(a)(t_vec)
fig1 = plt.figure(figsize=(11,11))
ax1 = fig1.add_subplot(411)
ax1.set_ylabel('a(t)')
ax1.plot(t_vec, a_t)
ax2 = fig1.add_subplot(412)
ax2.set_ylabel('F(t)')
ax2.plot(t_vec, F_t)
# ax3 = fig1.add_subplot(413)
# ax3.set_ylabel('v(t)')
# ax3.plot(t_vec, v_t)
# for i in range(len(F_t)):
#    print("t %.2f F_t %.2f a_t %.2f v_t %.2f"%(t_vec[i], F_t[i], a_t[i], v_t[i]))
```

```python
# Hany masodpercig no a test sebessege?
t_veg = 0
i = 0
while a_t[i] != 0:
    i += 1
t_veg = t_vec[i]
print("A test sebessege %.2f s-ig no."%t_veg)
```

    A test sebessege 4.47 s-ig no.

```python
# Mekkora a test vegsebessege? (Osszetett simpson formulaval kozelitettem az integralast.
# a_t integraltja a delta v)
v_delta = osszSimpson(a, t_vec)
print("Test vegsebessege: %.2f m/s"%v_delta)
```

    Test vegsebessege: 5.96 m/s

```python
# Mennyire tavolodott el a kezdo helyzettol addig, amig elerte a vegsebesseget.
def v_nov(f, t_vec): # sebesseg tablazat amig el nem erte a vegsebesseget
    v_vec = np.empty((0))
    for i in range(len(t_vec)):
        if t_vec[i] <= t_veg:
            v_vec = np.append(v_vec, simpson(f, 0.0, t_vec[i]))
        else:
            break
    return v_vec

t_sebnov = np.arange(0.0, t_veg, dt)
v_t = v_nov(a, t_sebnov)
# fig2 = plt.figure() # ez csak edrekesseg kedveert, ki lehet rajzolni v_t-t
# ax1 = fig2.add_subplot()
# ax1.plot(t_sebnov, v_t)

def x(v_vec): #
    x_t = 0.0
    for i in range(len(v_vec)):
        x_t += (v_vec[i])*dt
    return x_t

x_t = x(v_t)
print("%.2f m-re tavolodott el."%x_t)
```

    16.68 m-re tavolodott el.

```python
# Feladat megoldasa formalis derivalassal
t, a, m, v, x = sp.symbols('t a m v x')
F_tf = sp.sympify("10 - 0.5 * t**2")
a_tf = F_tf / 5 # a_tf-et ugy kapjuk meg ha F=a*m bol leosztunk m-el es megkapjuk a-t
print("a(t)-t ugy kapjuk meg ha F(t)-t osztjuk a tomeggel: %s"%a_tf)
```

    a(t)-t ugy kapjuk meg ha F(t)-t osztjuk a tomeggel: 2 - 0.1*t**2

```python
# Hany masodpercig no a test sebessege?
# Amig a gyorsulas nem lesz nulla.
t_vegseb = sp.solveset(a_tf - 0, t)
for mo in t_vegseb: # itt kivalasztjuk azt a megoldast ami nagyobb mint 0
    if mo > 0:
        t_vegseb = mo
        print("A testnek %.2f s-ig no a sebessge."%t_vegseb)
```

    A testnek 4.47 s-ig no a sebessge.

```python
# Mekkora a test vegsebessege?
# Ezt ugy kapjuk meg hogy ha a(t)-t integraljuk
# a t0-tol addig az idopontig aimg no a sebessege. (Elozo feladatreszben megadtuk)
v_t = sp.integrate(a_tf, (t)) + sp.integrate(a_tf, (t, 0))
print("A test vegsebessege %.2f m/s."%v_t.subs(t, t_vegseb))
sp.simplify(v_t)
```

```python
# Mennyire távolodott el a kezdo helyzettol addig, amíg elérte a végsebességét?
# x(t)-t ugy kapjuk meg ha integraljuk v(t)-t, t0 tol vegsebesseg elereseig
a_megtett = sp.integrate(v_t, (t, 0, t_vegseb))
print("A test %.2f m-re tavolodott el."%a_megtett)
```

    A test 16.67 m-re tavolodott el.