# Multibody dynamics of simple biomechanical models

Marcos Duarte, Renato Watanabe

The human body is composed of multiple interconnected segments (which can be modeled as rigid or flexible) and each segment may have translational and rotational movement. The part of mechanics for the study of movement and forces of interconnected bodies is called [multibody system](http://en.wikipedia.org/wiki/Multibody_system) or multibody dynamics.

There are different approaches to deduce the kinematics and dynamics of such bodies, the most common are the [Newton-Euler](http://en.wikipedia.org/wiki/Newton%E2%80%93Euler_equations) and the [Langrangian](http://en.wikipedia.org/wiki/Lagrangian_mechanics) formalisms. The Newton-Euler formalism is based on the well known Newton-Euler equations. The Langrangian formalism uses the [principle of least action](http://en.wikipedia.org/wiki/Principle_of_least_action) and describes the movement based on [generalized coordinates](http://en.wikipedia.org/wiki/Generalized_coordinates), a set of parameters (typically, a convenient minimal set) to describe the configuration of the system taking into account its constraints. For a system with multiple bodies and several constraints, e.g., the human body, it is easier to describe the dynamics of such system using the Langrangian formalism.

Next, we will study two simple problems of multibody systems in the context of biomechanics which we can handle well using the Newton-Euler approach. First a planar one-link system (which is not a multibody!), which can represent the movement of one limb of the body or the entire body as a single inverted pendulum. Second, a planar two-link system, which can represent the movement of two segments of the body, e.g., upper arm and forearm. Zajac and Gordon (1989) and Zajac (1993) offer excellent discussions about applying multibody system concepts to understanding human body movement.

## Newton-Euler equations

For a two-dimensional movement in the $XY$ plane, the Newton-Euler equations are:

$$ \left\{ \begin{array}{l l}
\sum F_X = m \ddot{x}_{cm} \\
\\
\sum F_Y = m \ddot{y}_{cm} \\
\\
\sum M_Z = I_{cm} \ddot{\alpha}_Z
\end{array} \right.$$

Where the movement is described around the body center of mass ($cm$). $(F_X,\,F_Y)$ and $M_Z$ are, respectively, the forces and moment of forces (torques) acting on the body, $(\ddot{x}_{cm},\,\ddot{y}_{cm})$ and $\ddot{\alpha}_Z$ are, respectively, the linear and angular accelerations, and $I_{cm}$ is the body moment of inertia around the $Z$ axis passing through the body center of mass.

Let's use Sympy to derive some of the characteristics of the systems.

```python
from sympy import Symbol, symbols, cos, sin, Matrix, simplify
from sympy.physics.mechanics import dynamicsymbols, mlatex, init_vprinting
init_vprinting()
from IPython.display import display, Math
```

## One-link system

Let's study the dynamics of a planar inverted pendulum as a model for the movement of a human body segment with an external force acting on the segment (see Figure 1).

Figure. Planar inverted pendulum with joint actuators (muscles) and corresponding free body diagram. See text for notation convention.

The following notation convention will be used for this problem:

 - $L$ is the length of the segment.
 - $d$ is the distance from the joint of the segment to its center of mass position.
 - $m$ is the mass of the segment.
 - $g$ is the gravitational acceleration (+).
 - $\alpha$ is the angular position of the joint w.r.t. horizontal, $\ddot{\alpha_i}$ is the corresponding angular acceleration.
 - $I$ is the moment of inertia of the segment around its center of mass position.
 - $F_{r}$ is the joint reaction force.
 - $F_{e}$ is the external force acting on the segment.
 - $T$ is the joint moment of force (torque).

Muscles responsible for the movement of the segment are represented as a single pair of antagonistic joint actuators (e.g., flexors and extensors). We will consider that all joint torques are generated only by these muscles (we will disregard the torques generated by ligaments and other tissues) and the total or net joint torque will be the sum of the torques generated by the two muscles:

$$ T = T_{net} = T_{extension} - T_{flexion} $$

Where we considered the extensor torque as positive. In what follows, we will determine only the net torque, we will be unable to decompose the net torque in its components.

### Kinetics

From the free body diagram, the Newton-Euler equations for the planar inverted pendulum are:

$$ \begin{array}{l l}
F_{r,x} + F_{e,x} = m\ddot{x} \\
\\
F_{r,y} - mg + F_{e,y} = m\ddot{y} \\
\\
T + dF_{r,x}\sin\alpha - dF_{r,y}\cos\alpha - (L-d)F_{e,x}\sin\alpha + (L-d)F_{e,y}\cos\alpha = I\ddot{\alpha}
\end{array}$$

However, manually placing the terms in the Newton-Euler equations as we did above where we calculated the signs of the cross products is error prone. We can avoid this manual placing by treating the quantities as vectors and express them in matricial form:

$$ \begin{array}{l l}
\mathbf{F}_r + \mathbf{F}_g + \mathbf{F}_e = m\ddot{\mathbf{r}} \\
\\
\mathbf{T} + \mathbf{r}_{cm,j} \times \mathbf{F}_r + \mathbf{r}_{cm,e} \times \mathbf{F}_e = I\ddot{\mathbf{\alpha}}
\end{array}$$

Where:

$$ \begin{bmatrix} F_{rx} \\ F_{ry} \\ 0 \end{bmatrix}  + \begin{bmatrix} 0 \\ -g \\ 0 \end{bmatrix}  + \begin{bmatrix} F_{ex} \\ F_{ey} \\ 0 \end{bmatrix} = m\begin{bmatrix} \ddot{x} \\ \ddot{y} \\ 0 \end{bmatrix} , \quad \begin{bmatrix} \hat{i} \\ \hat{j} \\ \hat{k} \end{bmatrix} $$

$$ \begin{bmatrix} 0 \\ 0 \\ T_z \end{bmatrix}  + \begin{bmatrix} -d\cos\alpha \\ -d\sin\alpha \\ 0 \end{bmatrix}  \times \begin{bmatrix} F_{rx} \\ F_{ry} \\ 0 \end{bmatrix} + \begin{bmatrix} (L-d)\cos\alpha \\ (L-d)\sin\alpha \\ 0 \end{bmatrix}  \times \begin{bmatrix} F_{ex} \\ F_{ey} \\ 0 \end{bmatrix} = I_z\begin{bmatrix} 0 \\ 0 \\ \ddot{\alpha} \end{bmatrix} , \quad \begin{bmatrix} \hat{i} \\ \hat{j} \\ \hat{k} \end{bmatrix} $$

Note that $\times$ represents the cross product, not matrix multiplication. Then, both in symbolic or numeric manipulation we would use the cross product function to perform part of the calculations. There are different computational tools that can be used for the formulation of the equations of motion. For instance, Sympy has a module, [Classical Mechanics](http://docs.sympy.org/dev/modules/physics/mechanics/), and see [this list](http://real.uwaterloo.ca/~mbody/#Software) for other software. Let's continue with the manual formulation of the equations hence they are not complex.

We can rewrite the equation for the moments of force in a form that doesn't explicitly involve the joint reaction force expressing the moments of force around the joint center:

$$ T - mgd\cos\alpha - LF_{e,x}\sin\alpha + LF_{e,y}\cos\alpha = I_o\ddot{\alpha} $$

Where $I_o$ is the moment of inertia around the joint, $I_o=I_{cm}+md^2$, using the parallel axis theorem.

The torque due to the joint reaction force does not appear on this equation; this torque is null because by the definition the reaction force acts on the joint. If we want to determine the joint torque and we know the kinematics, we perform inverse dynamics:

$$ T = I_o\ddot{\alpha} + mgd \cos \alpha + LF_{e,x}\sin\alpha - LF_{e,y}\cos\alpha $$

If we want to determine the kinematics and we know the joint torque, we perform direct dynamics:

$$ \ddot{\alpha} = I_o^{-1}[T - mgd \cos \alpha - LF_{e,x}\sin\alpha + LF_{e,y}\cos\alpha ] $$

The expression above is a second-order differential equation which typically is solved numerically. So, unless we are explicitly interested in estimating the joint reaction forces, we don't need to use them for calculating the joint torque or simulate movement. Anyway, let's look at the kinematics of this problem to introduce some important concepts which will be needed later.

### Kinematics

A single planar inverted pendulum has one degree of freedom, the rotation movement of the segment around the pin joint. In this case, if the angular position $\alpha(t)$ is known, the coordinates $x(t)$ and $y(t)$ of the center of mass and their derivatives can be readily determined (a process referred as forward kinematics):

```python
t = Symbol('t')
d, L = symbols('d L', positive=True)
a = dynamicsymbols('alpha')
```

```python
x, y = d*cos(a), d*sin(a)
xd, yd = x.diff(t), y.diff(t)
xdd, ydd = xd.diff(t), yd.diff(t)

display(Math(r'x=' + mlatex(x)))
display(Math(r'\dot{x}=' + mlatex(xd)))
display(Math(r'\ddot{x}=' + mlatex(xdd)))
display(Math(r'y=' + mlatex(y)))
display(Math(r'\dot{y}=' + mlatex(yd)))
display(Math(r'\ddot{y}=' + mlatex(ydd)))
```

$$x=d \operatorname{cos}\left(\alpha\right)$$

$$\dot{x}=- d \operatorname{sin}\left(\alpha\right) \dot{\alpha}$$

$$\ddot{x}=- d \operatorname{sin}\left(\alpha\right) \ddot{\alpha} - d \operatorname{cos}\left(\alpha\right) \left(\dot{\alpha}\right)^{2}$$

$$y=d \operatorname{sin}\left(\alpha\right)$$

$$\dot{y}=d \operatorname{cos}\left(\alpha\right) \dot{\alpha}$$

$$\ddot{y}=- d \operatorname{sin}\left(\alpha\right) \left(\dot{\alpha}\right)^{2} + d \operatorname{cos}\left(\alpha\right) \ddot{\alpha}$$

The terms in $\ddot{x}$ and $\ddot{y}$ proportional to $\dot{\alpha}^2$ are components of the centripetal acceleration on the body. As the name suggests, the [centripetal](http://en.wikipedia.org/wiki/Centripetal_force) acceleration is always directed to the center (torwards the joint) when the segment is rotating. See the notebook [Kinematic chain](http://nbviewer.ipython.org/github/demotu/BMC/blob/master/notebooks/KinematicChain.ipynb) for more on that.

As an exercise, let's go back to the Newton-Euler equation for the sum of torques around the center of mass where the torques due to the joint reaction forces are explicit. From the equation for the the sum of forces, hence we have expressions for the linear accelerations, we can isolate the reaction forces and substitute them on the equation for the torques. With a little help from Sympy:

```python
m, I, g = symbols('m I g', positive=True)
Fex, Fey = symbols('F_ex F_ey')
add = a.diff(t, 2)
```

```python
Frx = m*xdd - Fex
Fry = m*ydd + m*g - Fey
display(Math(r'F_{rx}=' + mlatex(Frx)))
display(Math(r'F_{ry}=' + mlatex(Fry)))
```

$$F_{rx}=- F_{ex} + m \left(- d \operatorname{sin}\left(\alpha\right) \ddot{\alpha} - d \operatorname{cos}\left(\alpha\right) \left(\dot{\alpha}\right)^{2}\right)$$

$$F_{ry}=- F_{ey} + g m + m \left(- d \operatorname{sin}\left(\alpha\right) \left(\dot{\alpha}\right)^{2} + d \operatorname{cos}\left(\alpha\right) \ddot{\alpha}\right)$$

```python
T = I*add - d*sin(a)*Frx + d*cos(a)*Fry + (L-d)*sin(a)*Fex - (L-d)*cos(a)*Fey
display(Math(r'T\quad=\quad ' + mlatex(T)))
```

$$T\quad=\quad F_{ex} \left(L - d\right) \operatorname{sin}\left(\alpha\right) - F_{ey} \left(L - d\right) \operatorname{cos}\left(\alpha\right) + I \ddot{\alpha} - d \left(- F_{ex} + m \left(- d \operatorname{sin}\left(\alpha\right) \ddot{\alpha} - d \operatorname{cos}\left(\alpha\right) \left(\dot{\alpha}\right)^{2}\right)\right) \operatorname{sin}\left(\alpha\right) + d \left(- F_{ey} + g m + m \left(- d \operatorname{sin}\left(\alpha\right) \left(\dot{\alpha}\right)^{2} + d \operatorname{cos}\left(\alpha\right) \ddot{\alpha}\right)\right) \operatorname{cos}\left(\alpha\right)$$

This equation for the torques around the center of mass of only one rotating segment seems too complicated. The equation we derived before for the torques around the joint was much simpler. However, if we look at the terms on this last equation, we can simplify most of them. Let's use Sympy to simplify this equation:

```python
T = simplify(T)
display(Math(r'T=' + mlatex(T)))
```

$$T=F_{ex} L \operatorname{sin}\left(\alpha\right) - F_{ey} L \operatorname{cos}\left(\alpha\right) + I \ddot{\alpha} + d^{2} m \ddot{\alpha} + d g m \operatorname{cos}\left(\alpha\right)$$

And we are back to the more simple equation we've seen before. The first two terms on the right side are the torque due to the external force, the third and fourth are the moment of inertia around the joint (use the theorem of parallel axis) times the acceleration, and the last term is the gravitational torque.

But what happened with all the other terms in the equation?

First, the terms proportional to the angular acceleration were just components from each direction of the 'inertial' torque that when summed resulted in $md^2\ddot{\alpha}$.
Second, the terms proportional to $\dot{\alpha}^2$ are components of the torque due to the centripetal force (acceleration). But the centripetal force passes through the joint as well as through the center of mass, i.e., it has zero lever arm and this torque should be zero. Indeed, when summed these terms are canceled out.

Now let's study a two-link system which can rotate independently around each joint. We will see that now the torque due to the centripetal force most of the times will not cancel out and a new torque component will appear.

### The Jacobian matrix

Another way to deduce the velocity and acceleration of a point at the rotating link is to use the [Jacobian matrix](http://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant) (see [Kinematic chain](http://nbviewer.ipython.org/github/demotu/BMC/blob/master/notebooks/KinematicChain.ipynb)). Remember that in the context of kinematic chains, the Jacobian is a matrix of all first-order partial derivatives of the linear position vector of the endpoint with respect to the angular position vector. For the planar one-link case, this means that the Jacobian matrix is:

$$ \mathbf{J}=
\begin{bmatrix}
\frac{\partial x}{\partial \alpha} \\
\frac{\partial y}{\partial \alpha} \\
\end{bmatrix} $$

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

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

And Sympy has a function to calculate the Jacobian:

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

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

The linear velocity of a point in the link will be given by the product between the Jacobian of the kinematic link and its angular velocity:

$$ \mathbf{v} = \mathbf{J} \dot{\alpha} $$

Using Sympy:

```python
vel = J*a.diff(t)
display(Math(r'\begin{bmatrix} \dot{x} \\ \dot{y} \end{bmatrix}=' + mlatex(vel)))
```

$$\begin{bmatrix} \dot{x} \\ \dot{y} \end{bmatrix}=\left[\begin{matrix}- d \operatorname{sin}\left(\alpha\right) \dot{\alpha}\\d \operatorname{cos}\left(\alpha\right) \dot{\alpha}\end{matrix}\right]$$

And the linear acceleration will be given by the derivative of this last expression:

$$ \mathbf{a} = \dot{\mathbf{J}} \dot{\alpha} + \mathbf{J} \ddot{\alpha} $$

And using Sympy again:

```python
acc = (J*a.diff(t)).diff(t)
display(Math(r'\begin{bmatrix} \ddot{x} \\ \ddot{y} \end{bmatrix}=' + mlatex(acc)))
```

$$\begin{bmatrix} \ddot{x} \\ \ddot{y} \end{bmatrix}=\left[\begin{matrix}- d \operatorname{sin}\left(\alpha\right) \ddot{\alpha} - d \operatorname{cos}\left(\alpha\right) \left(\dot{\alpha}\right)^{2}\\- d \operatorname{sin}\left(\alpha\right) \left(\dot{\alpha}\right)^{2} + d \operatorname{cos}\left(\alpha\right) \ddot{\alpha}\end{matrix}\right]$$

Same expressions as before.

We can also use the Jacobian matrix to calculate the torque due to a force on the link:

$$ T = \mathbf{J}^T \begin{bmatrix} F_{ex} \\ F_{ey} \end{bmatrix}  $$

```python
Te = J.T*Matrix((Fex, Fey))
display(Math(r'T_e=' + mlatex(Te[0])))
```

$$T_e=- F_{ex} d \operatorname{sin}\left(\alpha\right) + F_{ey} d \operatorname{cos}\left(\alpha\right)$$

## Two-link system

Let's study the dynamics of a planar double inverted pendulum (see Figure 2) as a model of two interconnected segments in the human body with an external force acting on the distal segment. Once again, we will consider that there are muscles around each joint and they generate torques.

Figure. Planar double inverted pendulum  with joint actuators (muscles) and corresponding free body diagrams. See text for notation convention.

The following notation convention will be used for this problem:
 - Subscript $i$ runs 1 or 2 meaning first (most proximal) or second joint when referring to angles, joint moments, or joint reaction forces, or meaning first or second segment when referring to everything else.
 - $L_i$ is the length of segment $i$.
 - $d_i$ is the distance from the proximal joint of segment $i$ to its center of mass position.
 - $m_i$ is the mass of segment $i$.
 - $g$ is the gravitational acceleration (+).
 - $\alpha_i$ is the angular position of joint $i$ in the joint space, $\ddot{\alpha_i}$ is the corresponding angular acceleration.
 - $\theta_i$ is the angular position of joint $i$ in the segmental space w.r.t. horizontal, $\theta_1=\alpha_1$ and $\theta_2=\alpha_1+\alpha_2$.
 - $I_i$ is the moment of inertia of segment $i$ around its center of mass position.
 - $F_{ri}$ is the reaction force of joint $i$.
 - $F_{e}$ is the external force acting on the distal segment.
 - $T_i$ is the moment of force (torque) of joint $i$.

Hence we know we will need the linear accelerations for solving the Newton-Euler equations, let's deduce them first.

### Kinematics

Once again, if the angular positions $\alpha_1(t)$ and $\alpha_2(t)$ are known, the coordinates $(x_1(t), y_1(t))$ and $(x_2(t), y_2(t))$ and their derivatives can be readily determined (by forward kinematics):

#### Link 1

```python
t = Symbol('t')
d1, d2, L1, L2 = symbols('d1, d2, L_1 L_2', positive=True)
a1, a2 = dynamicsymbols('alpha1 alpha2')
a1d, a2d = a1.diff(t), a2.diff(t)
a1dd, a2dd = a1.diff(t, 2), a2.diff(t, 2)
```

```python
x1, y1 = d1*cos(a1), d1*sin(a1)
x1d, y1d = x1.diff(t), y1.diff(t)
x1dd, y1dd = x1d.diff(t), y1d.diff(t)

display(Math(r'x_1=' + mlatex(x1)))
display(Math(r'\dot{x}_1=' + mlatex(x1d)))
display(Math(r'\ddot{x}_1=' + mlatex(x1dd)))
display(Math(r'y_1=' + mlatex(y1)))
display(Math(r'\dot{y}_1=' + mlatex(y1d)))
display(Math(r'\ddot{y}_1=' + mlatex(y1dd)))
```

$$x_1=d_{1} \operatorname{cos}\left(\alpha_{1}\right)$$

$$\dot{x}_1=- d_{1} \operatorname{sin}\left(\alpha_{1}\right) \dot{\alpha}_{1}$$

$$\ddot{x}_1=- d_{1} \operatorname{sin}\left(\alpha_{1}\right) \ddot{\alpha}_{1} - d_{1} \operatorname{cos}\left(\alpha_{1}\right) \left(\dot{\alpha}_{1}\right)^{2}$$

$$y_1=d_{1} \operatorname{sin}\left(\alpha_{1}\right)$$

$$\dot{y}_1=d_{1} \operatorname{cos}\left(\alpha_{1}\right) \dot{\alpha}_{1}$$

$$\ddot{y}_1=- d_{1} \operatorname{sin}\left(\alpha_{1}\right) \left(\dot{\alpha}_{1}\right)^{2} + d_{1} \operatorname{cos}\left(\alpha_{1}\right) \ddot{\alpha}_{1}$$

#### Link 2

```python
x2, y2 = L1*cos(a1) + d2*cos(a1+a2), L1*sin(a1) + d2*sin(a1+a2)
x2d, y2d = x2.diff(t), y2.diff(t)
x2dd, y2dd = x2d.diff(t), y2d.diff(t)

display(Math(r'x_2=' + mlatex(x2)))
display(Math(r'\dot{x}_2=' + mlatex(x2d)))
display(Math(r'\ddot{x}_2=' + mlatex(x2dd)))
display(Math(r'y_2=' + mlatex(y2)))
display(Math(r'\dot{y}_2=' + mlatex(y2d)))
display(Math(r'\ddot{y}_2=' + mlatex(y2dd)))
```

$$x_2=L_{1} \operatorname{cos}\left(\alpha_{1}\right) + d_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)$$

$$\dot{x}_2=- L_{1} \operatorname{sin}\left(\alpha_{1}\right) \dot{\alpha}_{1} - d_{2} \left(\dot{\alpha}_{1} + \dot{\alpha}_{2}\right) \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right)$$

$$\ddot{x}_2=- L_{1} \operatorname{sin}\left(\alpha_{1}\right) \ddot{\alpha}_{1} - L_{1} \operatorname{cos}\left(\alpha_{1}\right) \left(\dot{\alpha}_{1}\right)^{2} - d_{2} \left(\dot{\alpha}_{1} + \dot{\alpha}_{2}\right)^{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right) - d_{2} \left(\ddot{\alpha}_{1} + \ddot{\alpha}_{2}\right) \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right)$$

$$y_2=L_{1} \operatorname{sin}\left(\alpha_{1}\right) + d_{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right)$$

$$\dot{y}_2=L_{1} \operatorname{cos}\left(\alpha_{1}\right) \dot{\alpha}_{1} + d_{2} \left(\dot{\alpha}_{1} + \dot{\alpha}_{2}\right) \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)$$

$$\ddot{y}_2=- L_{1} \operatorname{sin}\left(\alpha_{1}\right) \left(\dot{\alpha}_{1}\right)^{2} + L_{1} \operatorname{cos}\left(\alpha_{1}\right) \ddot{\alpha}_{1} - d_{2} \left(\dot{\alpha}_{1} + \dot{\alpha}_{2}\right)^{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right) + d_{2} \left(\ddot{\alpha}_{1} + \ddot{\alpha}_{2}\right) \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)$$

Inspecting the equations above, we see a new kind of acceleration, proportional to $\dot{\alpha_1}\dot{\alpha_2}$. This acceleration is due to the [Coriolis effect](http://en.wikipedia.org/wiki/Coriolis_effect) and is  present only when there are movement in the two joints.

### Kinetics

From the free body diagrams, the Newton-Euler equations for the planar double inverted pendulum are:

#### Link 2

$$ \begin{array}{l l}
F_{r2x} + F_{e,x} = m_2\ddot{x}_{2} \\
\\
F_{r2y} - m_2g + F_{e,y} = m_2\ddot{y}_{2} \\
\\
T_2 + d_2F_{r2x}\sin(\alpha_1+\alpha_2) - d_2F_{r2y}\cos(\alpha_1+\alpha_2) - (L_2-d_2)F_{e,x}\sin(\alpha_1+\alpha_2) - (L_2-d_2)F_{e,y}\cos(\alpha_1+\alpha_2) = I_{2}(\ddot{\alpha}_1+\ddot{\alpha}_2)
\end{array} $$

#### Link 1

$$ \begin{array}{l l}
F_{r1x} - F_{r2x} = m_1\ddot{x}_{1} \\
\\
F_{r1y} - F_{r2y} - m_1g = m_1\ddot{y}_{1} \\
\\
T_1 - T_2 + d_1F_{r1x}\sin\alpha_1 - d_1F_{r1y}\cos\alpha_1 + (L_1-d_1)F_{r2x}\sin\alpha_1 - (L_1-d_1)F_{r2y}\cos\alpha_1 = I_{1}\ddot{\alpha}_1
\end{array} $$

If we want to determine the joint torques and we know the kinematics of the links, the inverse dynamics approach, we isolate the joint torques in the equations above, start solving for link 2 and then link 1. To determine the kinematics knowing the joint torques, the direct dynamics approach, we isolate the joint angular accelerations in the equations above and solve the ordinary differential equations.

Let's express the equations for the torques substituting the terms we know:

```python
m1, m2, I1, I2, g = symbols('m_1, m_2, I_1 I_2 g', positive=True)
```

```python
# link 2
Fr2x = m2*x2dd - Fex
Fr2y = m2*y2dd + m2*g - Fey
T2 = I2*(a1dd+a2dd) - d2*Fr2x*sin(a1+a2) + d2*Fr2y*cos(a1+a2) + (L2-d2)*Fex*sin(a1+a2) - (L2-d2)*Fey*cos(a1+a2)
T2 = simplify(T2)
# link 1
Fr1x = m1*x1dd + Fr2x
Fr1y = m1*y1dd + Fr2y + m1*g
T1 = I1*a1dd + T2 - d1*Fr1x*sin(a1) + d1*Fr1y*cos(a1) - (L1-d1)*Fr2x*sin(a1) + (L1-d1)*Fr2y*cos(a1)
T1 = simplify(T1)
```

The expressions for the joint moments of force are:

```python
display(Math(r'T_1\quad = \quad ' + mlatex(T1)))
display(Math(r'T_2\quad = \quad ' + mlatex(T2)))
```

$$T_1\quad = \quad F_{ex} L_{1} \operatorname{sin}\left(\alpha_{1}\right) + F_{ex} L_{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right) - F_{ey} L_{1} \operatorname{cos}\left(\alpha_{1}\right) - F_{ey} L_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right) + I_{1} \ddot{\alpha}_{1} + I_{2} \ddot{\alpha}_{1} + I_{2} \ddot{\alpha}_{2} + L_{1}^{2} m_{2} \ddot{\alpha}_{1} - 2 L_{1} d_{2} m_{2} \operatorname{sin}\left(\alpha_{2}\right) \dot{\alpha}_{1} \dot{\alpha}_{2} - L_{1} d_{2} m_{2} \operatorname{sin}\left(\alpha_{2}\right) \left(\dot{\alpha}_{2}\right)^{2} + 2 L_{1} d_{2} m_{2} \operatorname{cos}\left(\alpha_{2}\right) \ddot{\alpha}_{1} + L_{1} d_{2} m_{2} \operatorname{cos}\left(\alpha_{2}\right) \ddot{\alpha}_{2} + L_{1} g m_{2} \operatorname{cos}\left(\alpha_{1}\right) + d_{1}^{2} m_{1} \ddot{\alpha}_{1} + d_{1} g m_{1} \operatorname{cos}\left(\alpha_{1}\right) + d_{2}^{2} m_{2} \ddot{\alpha}_{1} + d_{2}^{2} m_{2} \ddot{\alpha}_{2} + d_{2} g m_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)$$

$$T_2\quad = \quad F_{ex} L_{2} \operatorname{sin}\left(\alpha_{1} + \alpha_{2}\right) - F_{ey} L_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right) + I_{2} \ddot{\alpha}_{1} + I_{2} \ddot{\alpha}_{2} + L_{1} d_{2} m_{2} \operatorname{sin}\left(\alpha_{2}\right) \left(\dot{\alpha}_{1}\right)^{2} + L_{1} d_{2} m_{2} \operatorname{cos}\left(\alpha_{2}\right) \ddot{\alpha}_{1} + d_{2}^{2} m_{2} \ddot{\alpha}_{1} + d_{2}^{2} m_{2} \ddot{\alpha}_{2} + d_{2} g m_{2} \operatorname{cos}\left(\alpha_{1} + \alpha_{2}\right)$$

There is an elegant form to display the equations for the torques using generalized coordinates, $q=[\alpha_1, \alpha_2]^T$ and grouping the terms proportional to common quantities in matrices, see for example, Craig (2005, page 180), Pandy (2001), and Zatsiorsky (2002, page 383):

$$ \tau = M(q)\ddot{q} + C(q,\dot{q}) + G(q) + E(q,\dot{q}) $$

Where $\tau$ is a matrix (2x1) of joint torques; $M$ is the mass or inertia matrix (2x2); $\ddot{q}$ is a matrix (2x1) of angular accelerations; $C$ is a matrix (2x1) of [centipetal](http://en.wikipedia.org/wiki/Centripetal_force) and [Coriolis](http://en.wikipedia.org/wiki/Coriolis_effect) torques; $G$ is a matrix (2x1) of  gravitational torques; and $E$ is a matrix (2x1) of external torques.
Let's use Sympy to display the equations in this new form:

```python
T1, T2 = T1.expand(), T2.expand()
q1, q2 = dynamicsymbols('q_1 q_2')
q1d, q2d = q1.diff(t), q2.diff(t)
q1dd, q2dd = q1.diff(t, 2), q2.diff(t, 2)
T1 = T1.subs({a1:q1, a2:q2, a1d:q1d, a2d:q2d, a1dd:q1dd, a2dd:q2dd})
T2 = T2.subs({a1:q1, a2:q2, a1d:q1d, a2d:q2d, a1dd:q1dd, a2dd:q2dd})
```

```python
M = Matrix(((simplify(T1.coeff(q1dd)), simplify(T1.coeff(q2dd))),
            (simplify(T2.coeff(q1dd)), simplify(T2.coeff(q2dd)))))
C = Matrix((simplify(T1.coeff(q1d**2)*q1d**2 + T1.coeff(q2d**2)*q2d**2 + T1.coeff(q1d*q2d)*q1d*q2d),
            simplify(T2.coeff(q1d**2)*q1d**2 + T2.coeff(q2d**2)*q2d**2 + T2.coeff(q1d*q2d)*q1d*q2d)))
G = Matrix((simplify(T1.coeff(g)*g),
            simplify(T2.coeff(g)*g)))
E = Matrix((simplify(T1.coeff(Fex)*Fex + T1.coeff(Fey)*Fey),
            simplify(T2.coeff(Fex)*Fex + T2.coeff(Fey)*Fey)))

display(Math(r'\begin{eqnarray}\tau&=&\begin{bmatrix}\tau_1\\ \tau_2\\ \end{bmatrix} \\' +
             r'M(q)&=&' + mlatex(M) + r'\\' +
             r'\ddot{q}&=&' + mlatex(Matrix((q1dd, q2dd))) + r'\\' +
             r'C(q,\dot{q})&=&' + mlatex(C) + r'\\' +
             r'G(q)&=&' + mlatex(G) + r'\\' +
             r'E(q,\dot{q})&=&' + mlatex(E) + r'\end{eqnarray}'))
```

$$\begin{eqnarray}\tau&=&\begin{bmatrix}\tau_1\\ \tau_2\\ \end{bmatrix} \\M(q)&=&\left[\begin{matrix}I_{1} + I_{2} + L_{1}^{2} m_{2} + 2 L_{1} d_{2} m_{2} \operatorname{cos}\left(q_{2}\right) + d_{1}^{2} m_{1} + d_{2}^{2} m_{2} & I_{2} + L_{1} d_{2} m_{2} \operatorname{cos}\left(q_{2}\right) + d_{2}^{2} m_{2}\\I_{2} + L_{1} d_{2} m_{2} \operatorname{cos}\left(q_{2}\right) + d_{2}^{2} m_{2} & I_{2} + d_{2}^{2} m_{2}\end{matrix}\right]\\\ddot{q}&=&\left[\begin{matrix}\ddot{q}_{1}\\\ddot{q}_{2}\end{matrix}\right]\\C(q,\dot{q})&=&\left[\begin{matrix}- L_{1} d_{2} m_{2} \left(2 \dot{q}_{1} + \dot{q}_{2}\right) \operatorname{sin}\left(q_{2}\right) \dot{q}_{2}\\L_{1} d_{2} m_{2} \operatorname{sin}\left(q_{2}\right) \left(\dot{q}_{1}\right)^{2}\end{matrix}\right]\\G(q)&=&\left[\begin{matrix}g \left(L_{1} m_{2} \operatorname{cos}\left(q_{1}\right) + d_{1} m_{1} \operatorname{cos}\left(q_{1}\right) + d_{2} m_{2} \operatorname{cos}\left(q_{1} + q_{2}\right)\right)\\d_{2} g m_{2} \operatorname{cos}\left(q_{1} + q_{2}\right)\end{matrix}\right]\\E(q,\dot{q})&=&\left[\begin{matrix}F_{ex} \left(L_{1} \operatorname{sin}\left(q_{1}\right) + L_{2} \operatorname{sin}\left(q_{1} + q_{2}\right)\right) - F_{ey} \left(L_{1} \operatorname{cos}\left(q_{1}\right) + L_{2} \operatorname{cos}\left(q_{1} + q_{2}\right)\right)\\L_{2} \left(F_{ex} \operatorname{sin}\left(q_{1} + q_{2}\right) - F_{ey} \operatorname{cos}\left(q_{1} + q_{2}\right)\right)\end{matrix}\right]\end{eqnarray}$$

With this convention, to perform inverse dynamics we would calculate:

$$ \tau = M(q)\ddot{q} + C(q,\dot{q}) + G(q) + E(q,\dot{q}) $$

And for direct dynamics we would solve the differential equation:

$$ \ddot{q} = M(q)^{-1} \left[\tau - C(q,\dot{q}) - G(q) - E(q,\dot{q}) \right] $$

The advantage of calculating analytically the derivatives of the position vector as function of the joint angles and using the notation above is that each term that contributes to each joint torque or acceleration can be easily identified.

#### Coupling (or interaction) effects

The terms off the main diagonal in the inertia matrix (which are the same) and the centripetal and Coriolis terms represent the effects of the movement (nonzero velocity) of one joint over the other. These torques are referred as coupling or interaction effects (see for example Hollerbach and Flash (1982) for an application of this concept in the study of the motor control of the upper limb movement).

#### Planar double pendulum

Using the same equations above, one can represent a planar double pendulum (hanging from the top, not inverted) considering the angles $\alpha_1$ and $\alpha_2$ negative, e.g., at $\alpha_1=-90^o$ and $\alpha_2=0$ the pendulum is hanging vertical.

WARNING: $F_r$ is not the actual joint reaction force!

For these two examples, in the Newton-Euler equations based on the free body diagrams we represented the consequences of all possible muscle forces on a joint as a net muscle torque and all forces acting on a joint as a resultant joint reaction force. That is, all forces between segments were represented as a resultant force that doesn't generate torque and a force couple (or free moment) that only generates torque. This is an important principle in mechanics of rigid bodies, see for example [this text](http://nbviewer.ipython.org/github/demotu/BMC/blob/master/notebooks/FreeBodyDiagram.ipynb). However, this principle creates the unrealistic notion that the sum of forces is applied directly on the joint (which has no further implication for a rigid body), but it is inaccurate for the understanding of the local effects on the joint. So, if we are trying to understand the stress on the joint or mechanisms of joint injury, the forces acting on the joint and on the rest of the segment must be considered individually.

#### Determination of muscle force

The torque $T$ exerted by a muscle is given by the product between the muscletendon moment arm $r$ and its force $F$. For the human body, there is more than one muscle crossing a joint and several joints. In such case, the torques due to the muscles are expressed in the following matrix form considering $n$ joints and $m$ muscles:

$$ \begin{eqnarray}
\begin{bmatrix} T_1 \\ \vdots \\ T_n \end{bmatrix} = \begin{bmatrix} r_{11} & \cdots & r_{1m} \\ \vdots & \ddots & \vdots \\ r_{n1} & \cdots & r_{nm} \end{bmatrix} \begin{bmatrix} F_1 \\ \vdots \\ F_m \end{bmatrix} \end{eqnarray} $$

Where $r_{nm}$ is the moment arm about joint $n$ of the muscle $m$.
In the example of the two-link system, we sketched two uniarticular muscles for each of the two joints, consequently:

$$ \begin{eqnarray}
\begin{bmatrix} T_1 \\ T_2 \end{bmatrix} = \begin{bmatrix} r_{1,ext} & r_{1,flex} & 0 & 0 \\ 0 & 0 & r_{1,ext} & r_{1,flex} \end{bmatrix} \begin{bmatrix} F_{1,ext} \\ -F_{1,flex} \\ F_{2,ext} \\ -F_{2,flex} \end{bmatrix} \end{eqnarray} $$

The moment arm of a muscle varies with the motion of the joints it crosses. In this case, using the [virtual work principle](http://en.wikipedia.org/wiki/Virtual_work) the moment arm can be given by (Sherman et al., 2013; Nigg and Herzog, 2006, page 634):

$$ r(q) = \frac{\partial L_{MT}(q)}{\partial q} $$

Where $L_{MT}(q)$ is the length of the muscletendon unit expressed as a function of angle $q$.

For the simulation of human movement, muscles can be modeled as [Hill-type muscles](http://nbviewer.ipython.org/github/demotu/BMC/blob/master/notebooks/MuscleSimulation.ipynb), the torques they generate are given by the matrix above, and this matrix is entered in the ODE for a multibody system dynamics we deduced before:

$$ \ddot{q} = M(q)^{-1} \left[R_{MT}(q)F_{MT}(a,L_{MT},\dot{L}_{MT}) - C(q,\dot{q}) - G(q) - E(q,\dot{q}) \right] $$

Where $R_{MT}$ and $F_{MT}$ are matrices for the moment arms and muscletendon forces, respectively.
This ODE is then solved numerically given initial values; but this problem is far from trivial for a simulation with several segments and muscles.

## Numerical simulation

Let's simulate a voluntary movement of the upper limb using the planar two-link system as a model in order to visualize the contribution of each torque term. We will ignore the muscle dynamics and we will calculate the joint torques necessary to move the upper limb from one point to another under the assumption that the movement is performed with the smoothest trajectory possible. I.e., the movement is performed with a [minimum-jerk trajectory](http://nbviewer.ipython.org/github/demotu/BMC/blob/master/notebooks/MinimumJerkHypothesis.ipynb), a hypothesis about control of voluntary movements proposed by Flash and Hogan (1985).

Once we determine the desired trajectory, we can calculate the velocity and acceleration of the segments and combine with anthropometric measures to calculate the joint torques necessary to move the segments. This means we will perform inverse dynamics.

Let's simulate a slow (4 s) and a fast (0.5 s) movement of the upper limb starting at the anatomical neutral position (upper limb at the side of the trunk) and ending with the upper arm forward at horizontal and elbow flexed at 90 degrees.

First, let's import the necessary Python libraries and customize the environment:

```python
import numpy as np
%matplotlib inline
import matplotlib
import matplotlib.pyplot as plt
matplotlib.rcParams['lines.linewidth'] = 3
matplotlib.rcParams['font.size'] = 13
matplotlib.rcParams['lines.markersize'] = 5
matplotlib.rc('axes', grid=False, labelsize=14, titlesize=16, ymargin=0.05)
matplotlib.rc('legend', numpoints=1, fontsize=11)
import sys
sys.path.insert(1, r'./../functions')  # add to pythonpath
```

Let's take the anthropometric data from Dempster's model (see [Body segment parameters](http://nbviewer.ipython.org/github/demotu/BMC/blob/master/notebooks/BodySegmentParameters.ipynb)):

```python
height, mass = 1.70,               70  # m, kg
L1n, L2n     = 0.188*height,       0.253*height
d1n, d2n     = 0.436*L1n,          0.682*L2n
m1n, m2n     = 0.0280*mass,        0.0220*mass
rg1n, rg2n   = 0.322,              0.468
I1n, I2n     = m1n*(rg1n*L1n)**2,  m2n*(rg2n*L2n)**2
```

Considering these lengths, the initial and final positions of the endpoint (finger tip) for the simulated movement will be:

```python
xi, yi = 0, -L1n-L2n
xf, yf = L1n, L2n
gn = 9.81  # gravity acceleration m/s2
```

### Slow movement

```python
duration = 4  # seconds
```

The endpoint minimum jerk trajectory will be (see [Kinematic chain in a plane (2D)](http://nbviewer.ipython.org/github/demotu/BMC/blob/master/notebooks/KinematicChain.ipynb)):

```python
from minjerk import minjerk
```

```python
time, rlin, vlin, alin, jlin = minjerk([xi, yi], [xf, yf], duration=duration)
```

Let's find the joint angles to produce this minimum-jerk trajectory (inverse kinematics):

```python
from invkin2_2d import invkin
```

```python
rang = invkin(time, rlin, L1=L1n, L2=L2n)
```

For the joint torques, we need to calculate the angular velocity and acceleration. Let's do that using numerical differentiation:

```python
def diff_c(ang, duration):
    """Numerical differentiations using the central difference for the angular data.
    """
    # central difference (f(x+h)-f(x-h))/(2*h)
    dt = duration/(ang.shape[0]-1)
    vang = np.empty_like(rang)
    aang = np.empty_like(rang)
    vang[:, 0] = np.gradient(rang[:, 0], dt)
    vang[:, 1] = np.gradient(rang[:, 1], dt)
    aang[:, 0] = np.gradient(vang[:, 0], dt)
    aang[:, 1] = np.gradient(vang[:, 1], dt)

    _, ax = plt.subplots(1, 3, sharex=True, figsize=(10, 3))
    ax[0].plot(time, rang*180/np.pi)
    ax[0].legend(['Ang 1', 'Ang 2'], framealpha=.5, loc='best')
    ax[1].plot(time, vang*180/np.pi)
    ax[2].plot(time, aang*180/np.pi)
    ylabel = [r'Displacement [$\mathrm{^o}$]', r'Velocity [$\mathrm{^o/s}$]', r'Acceleration [$\mathrm{^o/s^2}$]']
    for i, axi in enumerate(ax):
        axi.set_xlabel('Time [$s$]')
        axi.set_ylabel(ylabel[i])
        axi.xaxis.set_major_locator(plt.MaxNLocator(4))
        axi.yaxis.set_major_locator(plt.MaxNLocator(4))
    plt.tight_layout()
    plt.show()

    return vang, aang

vang, aang = diff_c(rang, duration)
```

```python
def dyna(time, L1n, L2n, d1n, d2n, m1n, m2n, gn, I1n, I2n, q1, q2, rang, vang, aang, Fexn, Feyn, M, C, G, E):
    """Numerical calculation and plot for the torques of a planar two-link system.
    """
    from sympy import lambdify, symbols
    Mfun  = lambdify((I1, I2, L1, L2, d1, d2, m1, m2, q1, q2), M, 'numpy')
    Mn    = Mfun(I1n, I2n, L1n, L2n, d1n, d2n, m1n, m2n, rang[:, 0], rang[:, 1])
    M00   = Mn[0, 0][:, 0]*aang[:, 0]
    M01   = Mn[0, 1][:, 0]*aang[:, 1]
    M10   = Mn[1, 0][:, 0]*aang[:, 0]
    M11   = Mn[1, 1]*aang[:, 1]
    Q1d, Q2d = symbols('Q1d Q2d')
    dicti = {q1.diff(t, 1):Q1d, q2.diff(t, 1):Q2d}
    C0fun = lambdify((L1, d2, m2, q2, Q1d, Q2d), C[0].subs(dicti), 'numpy')
    C0    = C0fun(L1n, d2n, m2n, rang[:, 1], vang[:, 0], vang[:, 1])
    C1fun = lambdify((L1, d2, m2, q2, Q1d, Q2d), C[1].subs(dicti), 'numpy')
    C1    = C1fun(L1n, d2n, m2n, rang[:, 1], vang[:, 0], vang[:, 1])
    G0fun = lambdify((L1, d1, d2, m1, m2, g, q1, q2), G[0], 'numpy')
    G0    = G0fun(L1n, d1n, d2n, m1n, m2n, gn, rang[:, 0], rang[:, 1])
    G1fun = lambdify((L1, d1, d2, m1, m2, g, q1, q2), G[1], 'numpy')
    G1    = G1fun(L1n, d1n, d2n, m1n, m2n, gn, rang[:, 0], rang[:, 1])
    E0fun = lambdify((L1, L2, q1, q2, Fex, Fey), E[0], 'numpy')
    E0    = E0fun(L1n, L2n, rang[:, 0], rang[:, 1], 0, 0)
    E1fun = lambdify((L1, L2, q1, q2, Fex, Fey), E[1], 'numpy')
    E1    = E1fun(L1n, L2n, rang[:, 0], rang[:, 1], Fexn, Feyn)

    _, ax = plt.subplots(1, 2, sharex=True, squeeze=True, figsize=(10, 4))
    ax[0].plot(time, M00+M01)
    ax[0].plot(time, C0)
    ax[0].plot(time, G0)
    ax[0].plot(time, E0)
    ax[0].plot(time, M00+M01+C0+G0, 'k--', linewidth=4)
    ax[0].set_ylabel(r'Torque [Nm]')
    ax[0].set_title('Joint 1')
    ax[1].plot(time, M10+M11, label='Mass/Inertia')
    ax[1].plot(time, C1, label='Centripetal/Coriolis')
    ax[1].plot(time, G1, label='Gravitational')
    ax[1].plot(time, E1, label='External')
    ax[1].plot(time, M10+M11+C1+G1, 'k--', linewidth=4, label='Muscular (sum)')
    ax[1].set_title('Joint 2')
    ax[1].legend(framealpha=.5, loc='upper right', bbox_to_anchor=(1.6, 1))
    for i, axi in enumerate(ax):
        axi.set_xlabel('Time [$s$]')
        axi.xaxis.set_major_locator(plt.MaxNLocator(4))
        axi.yaxis.set_major_locator(plt.MaxNLocator(4))
    plt.tight_layout()
    plt.show()

    return M00, M01, M10, M11, C0, C1, G0, G1, E0, E1
```

```python
Fexn, Feyn = 0, 0
M00, M01, M10, M11, C0, C1, G0, G1, E0, E1 = dyna(time, L1n, L2n, d1n, d2n, m1n, m2n, gn, I1n, I2n,
                                                  q1, q2, rang, vang, aang, Fexn, Feyn, M, C, G, E)
```

The joint torques essentially compensate the gravitational torque.

### Fast movement

Let's see what is changed for a fast movement:

```python
duration = 0.5  # seconds
time, rlin, vlin, alin, jlin = minjerk([xi, yi], [xf, yf], duration=duration)
rang = invkin(time, rlin, L1=L1n, L2=L2n)
vang, aang = diff_c(rang, duration)
M00, M01, M10, M11, C0, C1, G0, G1, E0, E1 = dyna(time, L1n, L2n, d1n, d2n, m1n, m2n, gn, I1n, I2n,
                                                  q1, q2, rang, vang, aang, Fexn, Feyn, M, C, G, E)
```

The interaction torques are larger than the gravitational torques for most part of the movement.

### Fast movement in the horizontal plane

Let's simulate a fast movement in the horizontal plane:

```python
gn = 0  # gravity acceleration m/s2
M00, M01, M10, M11, C0, C1, G0, G1, E0, E1 = dyna(time, L1n, L2n, d1n, d2n, m1n, m2n, gn, I1n, I2n,
                                                  q1, q2, rang, vang, aang, Fexn, Feyn, M, C, G, E)
```

## Exercises

1. Derive the equations of motion for a single pendulum (not inverted).
2. Derive the equations of motion for a double pendulum (not inverted).
3. Consider the double pendulum moving in the horizontal plane and with no external force. Find out the type of movement and which torque terms are changed when:
  a) $\dot{\alpha}_1=0^o$
  b) $\alpha_2=0^o$
  c) $\dot{\alpha}_2=0^o$
  d) $2\alpha_1+\alpha_2=180^o$ (hint: a two-link system with this configuration is called polar manipulator)
4. Derive the equations of motion and the torque terms using angles in the segmental space $(\theta_1,\,\theta_2)$.
5. Run the numerical simulations for the torques with different parameters.

## References

- Craig JJ (2005) [Introduction to Robotics: Mechanics and Control](http://books.google.com.br/books?id=MqMeAQAAIAAJ). 3rd Edition. Prentice Hall.
- Hollerbach JM, Flash T (1982) [Dynamic interactions between limb segments during planar arm movement](http://link.springer.com/article/10.1007%2FBF00353957). Biological Cybernetics, 44, 67-77.
- Nigg BM and Herzog W (2006) [Biomechanics of the Musculo-skeletal System](https://books.google.com.br/books?id=hOIeAQAAIAAJ&dq=editions:ISBN0470017678). 3rd Edition. Wiley.
- Pandy MG (2001) [Computer modeling and simulation](https://drive.google.com/open?id=0BxbW72zV7WmUbXZBR2VRMnF5UTA&authuser=0). Annu. Rev. Biomed. Eng., 3, 245–73.
- Sherman MA, Seth A, Delp SL (2013) [What is a moment arm? Calculating muscle effectiveness in biomechanical models using generalized coordinates](http://simtk-confluence.stanford.edu:8080/download/attachments/3376330/ShermanSethDelp-2013-WhatIsMuscleMomentArm-Final2-DETC2013-13633.pdf?version=1&modificationDate=1369103515834) in Proc. ASME Int. Design Engineering Technical Conferences (IDETC), Portland, OR, USA.
- Zajac FE (1993) [Muscle coordination of movement: a perspective](http://e.guigon.free.fr/rsc/article/Zajac93.pdf). J Biomech., 26, Suppl 1:109-24.
- Zajac FE, Gordon ME (1989) [Determining muscle's force and action in multi-articular movement](https://drive.google.com/open?id=0BxbW72zV7WmUcC1zSGpEOUxhWXM&authuser=0). Exercise and Sport Sciences Reviews, 17, 187-230.
- Zatsiorsky VM (2002) [Kinetics of human motion](http://books.google.com.br/books?id=wp3zt7oF8a0C). Human Kinetics.


# Lab 1:  Introduction to Jupyter and Symbolic Math

## 1. Background

This course includes a set of computer laboratories that explore topics related to signals and systems.  These will be made available to you as the course progresses, and you are expected to engage with the content and produce and submit a set of results related to some defined tasks. Assistance will be provided if required, but it is really expected that you'll work in your own time and at your own pace: it's important right from the start for you to take responsibility for your own learning. You shouldn't be working in groups, but helping one another out on specific issues is fine.

The computer labs are based on the Python programming language, and they require a kernel that includes all the standard numerical libraries. The labs have been created as worksheets using the *Jupyter Notebook* system, which works in a browser and allows you to interleave text and Python code snippets that can be run interactively. Jupyter can be installed via a package called *Anaconda* - see the Orientation "Labs" lesson on Vula for details.

This first lab covers the introduction to Jupyter as well as specific scientific libraries such as NumPy, SymPy and Matplotlib. These packages are instrumental for data visualisation using Python, and will be used throughout the three labs for this course.

### Basic Jupyter usage

A notebook is basically composed of cells.  A cell can contain text ("markdown") or code - in this case, Python.  For any cell you can select the content type using the pulldown on the menubar.  A cell can be executed by pressing shift-enter while it is in focus.

Below is a cell that creates two "numpy" arrays. The values in `x` (i.e. function `x(t)`) are initialised to be a particular function of `t` (i.e. your time variable).  Select it and press shift-enter to run the code in the Python kernel attached to the notebook.  Change the number of points in `t` and press shift-enter again to run, and note the change in output values.

```python
import numpy as np # Import numpy for vector computations

t = np.linspace(0, 10, 10); # Create a time variable from t = 0 to t = 10 with 10 points in this range
x = 0.1*t**2 - np.cos(t); # Create x variable as a function of t
print(x); # Print values of x at the 10 defined points of t
```

    [-1.         -0.32020923  1.10014808  2.09278512  2.24005852  2.3396668
      3.51707674  5.97325259  8.76104957 10.83907153]

The Python kernel is fully-featured for numeric computation, and includes most of the packages useful for scientific computing.  The following code plots the vectors defined above using "matplotlib".  The `%matplotlib notebook` line makes any plots appear in the notebook itself rather than in a separate window.

```python
import matplotlib.pyplot as plt # Import matplotlib for plotting
%matplotlib notebook

plt.rcParams['figure.dpi'] = 60; # Decrease plot size
plt.plot(t, x, 'ro-') # Plot x vs t using red, circular dots connected by lines
plt.xlabel('t');  plt.ylabel('x(t)'); # Labels for axes
```

    <IPython.core.display.Javascript object>

When you execute the code above it runs it in the Python kernel for the notebook, which already has the variables `t` and `x` defined.  This can cause problems:  if you jump around the notebook running cells in some arbitrary order then the kernel will probably end up in a weird state.  If this happens then you can use the menu item "Cell" -> "Run All" to re-execute all cells in the notebook in order.  If things really go wrong then restart the kernel.

Executing a text or markdown cell has a different effect:  the content is converted and rendered as rich text.  The markdown language is documented all over the internet, and supports both HTML and LaTeX equations.  Do a web search if you want details or introductions to these topics.

### Gaining familiarity

One of the nice things about Python is that there's an active community of technical people that provide useful information and resources.  If you don't know how to do something, then a simple web search can quickly help. There are also some "Cheat Sheets" available on Vula if you're feeling rusty with your Python.

There's a basic numerical Python tutorial at `http://cs231n.github.io/python-numpy-tutorial`, and a Python notebook version at `https://github.com/kuleshov/cs228-material/blob/master/tutorials/python/cs228-python-tutorial.ipynb`. A copy of this notebook that has been updated for Python 3 is included in the "examples" directory of your "notebooks" folder. Load this notebook and work through the cells.  Make changes to the contents, and press "shift-enter" to run and see the corresponding outputs. Pay particular attention to how to use numpy arrays, and how to plot using matplotlib.

You can also access function help in Jupyter notebook. You can access help windows for functions by placing a `?` before a function, e.g. `#?plt.plot()`.

## 2. Symbolic math introduction

Now that we've covered the basics, we can move onto something more useful: symbolic mathematics. This is a maturing technology that lets a computer do maths using symbolic manipulation rather than numerical computation. Python has support for symbolic computation via the "sympy" package. Some good examples of sympy in use are at `http://www.cfm.brown.edu/people/dobrush/am33/SymPy/index.html` and `https://github.com/sympy/sympy/wiki/Quick-examples`.

### Basic differentiation

The cell below imports the symbolic math package, and defines two symbolic variables `x` and `y`.  A symbolic function $f(x,y) = (x^2-2x+3)/y$ is then defined and printed.

```python
import sympy as sp # Import the sympy library under the name 'sp'

x, y = sp.symbols('x y');
f = (x**2 - 2*x + 3)/y;
print(f);
```

    (x**2 - 2*x + 3)/y

```python
# Pro-tip: you can make your outputs look good with LaTeX:
from IPython.display import display
sp.init_printing() # Initialises pretty printing. Only needs to be run once!

# Behold:
print(f) # Ugly mode
display(f) # Fancy-pants mode
```

Note that `f` here is a symbol representing a function.  It would be nice if the notation made it explicit that it's actually a function of $x$ and $y$, namely `f(x,y)`, but that's not how it works.  However, we can query the free variables:

```python
f.free_symbols
```

We can get sympy to find a symbolic expression for the partial derivative of $f(x,y)$ with respect to $y$ by using:

```python
fpy = sp.diff(f, y)
fpy
```

To evaluate this derivative at some particular values $x=\pi$ and $y=2$ we can substitute into the symbolic expression:

```python
fpyv = fpy.subs([(x, sp.pi), (y, 2)])
fpyv
```

Notice though that this is still a symbolic expression.  It can be evaluated using the "evalf" method, which finally returns a number:

```python
fpyv.evalf()
```

### More advanced differentiation

Symbolic expressions can be manipulated. For example, we can define $g(t) = f(x(t), y(t))$, which in this case (given above) means:
$$g(t) = (x(t)^2-2x(t)+3)/y(t),$$
and we can find its derivative with respect to time. I.e. $g$ is a function of time determined by $x$ and $y$, which are also functions of time.

```python
t = sp.symbols('t');
xt = sp.Function("x")(t);  # x(t)
yt = sp.Function("y")(t)  # y(t)

g = f.subs([(x,xt),(y,yt)]); # Define g(t)
gp = sp.diff(g,t); # Differentiate g(t) with respect to time
g # Print g(t)
```

```python
gp # Print g'(t)
```

### Plotting symbolic functions

The sympy module also has a `plot` method that knows how to plot symbolic functions of a single variable.  The function `g` above with $x(t) = \sin(t)$ and $y(t) = \cos(2t)$ is a function of a single time variable `t`, and can be visualised as follows:

```python
gs = g.subs([(xt,sp.sin(t)), (yt,sp.cos(2*t))]); # Create function gs(t)
gs
```

```python
sp.plot(gs, (t,1,2), xlabel="t", ylabel="gs(t)"); # Plot gs vs t using sympy (t defined from t = 1 to t = 2)
```

    <IPython.core.display.Javascript object>

A roughly equivalent plot could be obtained numerically by creating a lambda function for the expression, evaluating it for a closely-spaced set of values of `t` over the required range, and using standard numerical plotting functions that draw straight lines between the calulated points.  If you increase the number of calculated points over the interval then the  approximation in the above graph becomes more accurate. You can think of a lambda function as just another way of defining a function, e.g. `f = x^2` is the same as `def my_func(x): x^2`.

```python
tv = np.linspace(1, 2, 10); # Axis between tv = 1 and tv = 2 for 10 steps
gs_h = sp.lambdify(t, gs, modules=['numpy']);
gstv = gs_h(tv);
plt.figure(); # New figure
plt.plot(tv, gstv);
```

    <IPython.core.display.Javascript object>

### Symbolic integration

Integration is also a standard function in sympy, so we can find for example the integral
$$y(t) = \int_{-10}^t x(\lambda) d\lambda$$
for $x(t) = e^{-t/10} \cos(t)$:

```python
xt = sp.exp(-t/10)*sp.cos(t);  # x(t)
lamb = sp.symbols('lamb');   # Dummy variable Lambda
xl = xt.subs(t,lamb);  # x(lamb)
yt = sp.integrate(xl, (lamb, -10, t));  # Indefinite integral that produces a function of t
yt # Display y(t)
```

NOTE: To get a definite integral over the range, say -10 to 0, you'd go `yt = sp.integrate(xl, (lamb, -10, 0))`. Also, don't forget about your initial conditions; the definite integral only gives the change in the variable over the
interval, so you need to add its initial state to this value get the true final state. We'll usually assume initial rest conditions in this course, but NOT always.

Overall, the sympy plot function is quite fragile, and might not always work. Symbolic math packages are amazing, but they're difficult to implement and are sometimes not robust: you'll find various postings on the internet that give instances of very good symbolic math engines giving a wrong result. In short, they are useful but you should be careful when using them.

## Tasks

These tasks involve writing code, or modifying existing code, to meet the objectives described.

1.  We often need to plot complex-valued signals, where for each value of $t$ there is a real and an imaginary value, or a magnitude and a phase.  We therefore need two sets of axes.  Use the `subplot` functionality of `matplotlib.pyplot` to plot the real and imaginary parts of the signal $x(t) = e^{j \omega_0 t}$ in a single figure, but on two separate axes.  Use a value of $\omega_0 = 4$ and make the plot range over $t=0$ to $t=10$. (5 marks)

2.  Signal processing often involves recursion. This course concentrates on continuous-time signals, but equivalent processes can be applied in the digital (discrete) case. Consider the recursive equation $x[n] = -0.90 x[n-1]$.  Create a numpy array with 100 elements for each of the values $x[0]$ to $x[99]$, and write code to populate it (assuming the initial condition $x[0] = 10$).  Use `stem` to make a plot of $x[n]$ versus $n$ (discrete time) over the range calculated. (5 marks)

3.  Define the expression $y(t) = v_0 t - \frac{1}{2} g t^2$ for some symbolic values of $v_0$ and $g$ using sympy.  You should recognise this as the "altitude" of a particle moving under the influence of gravity, given that the initial velocity at time $t=0$ is $v_0$.  Make a plot of the particle height in meters for $v_0 = 20 m/s$ given $g = 9.8 m/s^2$, over the range $t=0$ to $t=10$ s. (5 marks)

4.  Suppose the acceleration of a particle is given by $a(t) = 0.3 + \cos(t)$ for positive time.  Use symbolic methods to find and plot the velocity $v(t)$ of the particle over the range $t=0$ to $t=5$ given the initial condition $v(0) = -0.2$.  Then find and plot the position $s(t)$ of the particle over the same time period, given the additional auxiliary condition $s(0) = 0.1$. (5 marks)


# Predictive coding (Rao & Ballard, 1999) モデルの実装
>

- toc: true
- badges: true
- comments: true
- author: 山拓
- categories: [neuroscience]

この記事ではPredictive codingの初めの数理的モデルとなる ([Rao & Ballard, *Nat. Neurosci*. 1999](https://www.nature.com/articles/nn0199_79))を解説し、Pythonによる実装を行います。コードを実行したい場合は<https://github.com/takyamamoto/PredictiveCoding-RaoBallard-Model>をCloneして`predictive-coding.ipynb`か、`train.py`を実行してください。

## 観測世界の階層的予測
構築するネットワークは入力層を含め、3層のネットワークとします。網膜への入力として画像 $\boldsymbol{I} \in \mathbb{R}^{n_0}$を考えます。画像 $\boldsymbol{I}$ の観測世界における隠れ変数、すなわち**潜在変数** (latent variable)を$\boldsymbol{r} \in \mathbb{R}^{n_1}$とし、ニューロン群によって発火率で表現されているとします (真の変数と $\boldsymbol{r}$は異なるので文字を分けるべきでしょうが簡単のためにこう表します)。このとき、

$$
\boldsymbol{I} = f(U\boldsymbol{r}) + \boldsymbol{n} \tag{1}
$$

が成立しているとします。ただし、$f(\cdot)$は活性化関数 (activation function)、$U \in \mathbb{R}^{n_0 \times n_1}$は重み行列です。$\boldsymbol{n} \in \mathbb{R}^{n_0} $は平均0, 分散 $\sigma^2$ のGaussian ノイズ項とします。

潜在変数 $\boldsymbol{r}$はさらに高次 (higher-level)の潜在変数 $\boldsymbol{r}^h$により、次式で表現されます。

$$
\boldsymbol{r} = \boldsymbol{r}^{td}+\boldsymbol{n}^{td}=f(U^h \boldsymbol{r}^h)+\boldsymbol{n}^{td} \tag{2}
$$

ただし、Top-downの予測信号を $\boldsymbol{r}^{td}:=f(U^h \boldsymbol{r}^h)$としました。また、$\boldsymbol{r}^{td} \in \mathbb{R}^{n_1}, \boldsymbol{r}^{h} \in \mathbb{R}^{n_2}, U^h \in \mathbb{R}^{n_1 \times n_2} $です。$\boldsymbol{n}^{td} \in \mathbb{R}^{n_1} $は平均0, 分散 $\sigma_{td}^2$ のGaussian ノイズ項とします。

話は飛びますが、Predictive codingのネットワークは下図のように表されます。特徴は
- 階層的な構造
- 高次による低次の予測 (Feedback or Top-down信号)
- 低次から高次への誤差信号の伝搬 (Feedforward or Bottom-up 信号)

です。

 (Rao and Ballard, 1999; Fig. 1a)

ここまで、高次表現による低次表現の予測、というFeedback信号について説明しました。それではPredictive codingのもう一つの要となる、低次から高次への予測誤差の伝搬というFeedforward信号はどのように導かれるのでしょうか。結論から言えば、これは**復元誤差 (reconstruction error)の最小化を行う再帰的ネットワーク (recurrent network)を考慮することで自然に導かれます**。

## 生成モデル的表現とMAP推定
### 尤度の設定
ネットワークの損失関数を導くために**生成モデル** (generative model)的な表現をしてみましょう。潜在変数 (あるいはCauses) $\boldsymbol{r}$の事前分布 (prior)を$p(\boldsymbol{r})$,  入力画像 $\boldsymbol{I}$の尤度 (likelihood)を$p(\boldsymbol{I}|\boldsymbol{r}; U)$とします。(1)式およびノイズ項を$\boldsymbol{n}\sim\mathcal{N}(0, \sigma^2)$としたことから、

$$
\begin{align}
p(\boldsymbol{I}|\ \boldsymbol{r}; U)&=\mathcal{N}\left(\boldsymbol{I}|\ f(U\boldsymbol{r}), \sigma^2\right)\\
&\varpropto \exp\left(-\frac{\|\boldsymbol{I} -  f(U\boldsymbol{r})\|^2}{\sigma^2}\right)
\end{align}
$$

と表せます。

### 事前分布の設定
事前分布$p(\boldsymbol{r})$に対してもGaussian分布を用いてもよいですが、 **Sparse coding** を考えると異なる分布を用いた方がよいでしょう。Sparse codingでは、$\boldsymbol{r}$の各要素$r_i$はほとんど0に等しく、ある入力に対しては大きな値を取る、というのが理想的です。この性質を考慮すると$p(\boldsymbol{r})$としては、0においてピークがあり、裾の重い(heavy tail)を持つsparse distributionあるいは **super-Gaussian distribution** (Laplace 分布やCauchy分布などGaussian分布よりもkurtoticな分布)を用いるのが良いでしょう。ここではGaussian分布またはCauchy分布の場合について考えてみます。なお、後で用いるために$p(\boldsymbol{r})$の負の対数事前分布を$g(\boldsymbol{r}):=-\log p(\boldsymbol{r})$とします。

#### Gaussian 分布を用いる場合
$$
\begin{align}
p(\boldsymbol{r})&=\exp(-\alpha\|\boldsymbol{r}\|^2)\\
g(\boldsymbol{r})&=-\ln p(\boldsymbol{r})=\alpha\|\boldsymbol{r}\|^2\\
g'(\boldsymbol{r})&=\frac{\partial g(\boldsymbol{r})}{\partial \boldsymbol{r}}=2\alpha\boldsymbol{r}
\end{align}\tag{3}
$$

#### Cauchy分布を用いる場合

$$
\begin{align}
p(\boldsymbol{r})&=\prod_i p(r_i)=\prod_i \exp\left[-\alpha \ln(1+r_i^2)\right]\\
g(\boldsymbol{r})&=-\ln p(\boldsymbol{r})=\alpha \sum_i \ln(1+r_i^2)\\
g'(\boldsymbol{r})&=\frac{\partial g(\boldsymbol{r})}{\partial \boldsymbol{r}}=\left[\frac{2\alpha r_i}{1+r_i^2}\right]_i
\end{align}\tag{4}
$$

いずれの分布を用いても良いですが、今回はCauchy分布を用います。

一方で、重み行列$U$の事前分布 $p(U)$はGaussian分布としておきます。$p(U)$の負の対数事前分布を$h(U):=-\ln p(U)$とすると、次のように表されます。

$$
\begin{align}
p(U)&=\exp(-\lambda\|U\|^2_F)\\
h(U)&=-\ln p(U)=\lambda\|U\|^2_F\\
h'(U)&=\frac{\partial h(U)}{\partial U}=2\lambda U
\end{align}\tag{5}
$$

ただし、$\|\cdot \|_F^2 $はフロベニウスノルムを意味します。

### MAP推定によるパラメータの最適化
MAP推定(最大事後確率推定)によりパラメータ $\boldsymbol{r}, U$を最適化することを考えます。まず、事後分布 (posterior)は

$$
p(\boldsymbol{r}, U|\boldsymbol{I})=k \cdot p(\boldsymbol{I}|\boldsymbol{r}, U) p(\boldsymbol{r})p(U)\tag{6}
$$

となります。ただし$k$は定数です。次に対数事後確率$F(\boldsymbol{r}, U):=\ln p(\boldsymbol{r}|\boldsymbol{I};U)$を考えます。

$$
\begin{align}
F(\boldsymbol{r}, U)
&=\ln p(\boldsymbol{I}|\boldsymbol{r}, U)+\ln p(\boldsymbol{r})+\ln p(U)+\ln k\\
&=-\frac{1}{\sigma^2}\|\boldsymbol{I} -  f(U\boldsymbol{r})\|^2-g(\boldsymbol{r})-h(U)+\ln k
\end{align}\tag{7}
$$

この$F$を最大化すれば$p(\boldsymbol{r}, U|\boldsymbol{I})$も最大となります。このためには勾配上昇法 (gradient ascent)を用いればよいです。ただ、一般のニューラルネットワークモデルと合わせるために$F$の符号を反転させて定数項を除いたものを損失関数 $E\ (\approx -F)$ とし、この$E$を最小化するために勾配降下法 (gradient descent)を適応することを次節で考えます。

## 損失関数と学習則
### 損失関数の設定
前節では2層までのパラメータを最適化することを考えましたが、高次の活動も考慮して損失関数 $E$を次のように再定義します。

$$
\begin{align}
E=\underbrace{\frac{1}{\sigma^{2}}\|\boldsymbol{I}-f(U \boldsymbol{r})\|^2+\frac{1}{\sigma_{t d}^{2}}\left\|\boldsymbol{r}-f(U^h \boldsymbol{r}^h)\right\|^2}_{\text{reconstruction error}}+\underbrace{g(\boldsymbol{r})+g(\boldsymbol{r}^{h})+h(U)+h(U^h)}_{\text{sparsity penalty}}\tag{8}
\end{align}
$$

### 再帰ネットワークの更新則
簡単のために$\boldsymbol{x}:=U\boldsymbol{r}, \boldsymbol{x}^h:=U^h\boldsymbol{r}^h$とします。

$$
\begin{align}
\frac{d \boldsymbol{r}}{d t}&=-\frac{k_{1}}{2} \frac{\partial E}{\partial \boldsymbol{r}}=k_{1}\cdot\Bigg(\frac{1}{\sigma^{2}} U^{T}\bigg[\frac{\partial f(\boldsymbol{x})}{\partial \boldsymbol{x}}\odot\underbrace{(\boldsymbol{I}-f(\boldsymbol{x}))}_{\text{bottom-up error}}\bigg]-\frac{1}{\sigma_{t d}^{2}}\underbrace{\left(\boldsymbol{r}-f(\boldsymbol{x}^h)\right)}_{\text{top-down error}}-\frac{1}{2}g'(\boldsymbol{r})\Bigg)\tag{9}\\
\frac{d \boldsymbol{r}^h}{d t}&=-\frac{k_{1}}{2} \frac{\partial E}{\partial \boldsymbol{r}^h}=k_{1}\cdot\Bigg(\frac{1}{\sigma_{t d}^{2}}(U^h)^T\bigg[\frac{\partial f(\boldsymbol{x}^h)}{\partial \boldsymbol{x}^h}\odot\underbrace{\left(\boldsymbol{r}-f(\boldsymbol{x}^h)\right)}_{\text{bottom-up error}}\bigg]-\frac{1}{2}g'(\boldsymbol{r}^h)\Bigg)\tag{10}
\end{align}
$$

ただし、$k_1$は更新率 (updating rate)です。または、発火率の時定数を$\tau:=1/k_1$として、$k_1$は発火率の時定数$\tau$の逆数であると捉えることもできます。ここで(9)式において、中間表現 $\boldsymbol{r}$ のダイナミクスはbottom-up errorとtop-down errorで記述されています。このようにbottom-up errorが $\boldsymbol{r}$ への入力となることは自然に導出されます。なお、top-down errorに関しては高次からの予測 (prediction)の項 $f(\boldsymbol{x}^h)$とleaky-integratorとしての項 $-\boldsymbol{r}$に分割することができます。また$U^T, (U^h)^T$は重み行列の転置となっており、bottom-upとtop-downの投射において対称な重み行列を用いることを意味しています。$-g'(\boldsymbol{r})$は発火率を抑制してスパースにすることを目的とする項ですが、無理やり解釈をすると自己再帰的な抑制と言えます。

(9), (10)式の処理をまとめると下図のようになります。各ステップが１つずつ実行されるのではなく、連続時間RNNであることに注意しましょう。

 (Rao and Ballard, 1999; Fig. 1b)

なお、$\boldsymbol{r}, \boldsymbol{r}^h$の初期値はそれぞれ $\boldsymbol{r}(0) = U^T \boldsymbol{I}, \boldsymbol{r}^h(0)=(U^h)^T \boldsymbol{r}$としておきます。誤差が計算されるまでは通常のfeedforwordのネットワークと同様の処理をすると見なして良いでしょう。

### 重み行列の学習則
重み行列 $U, U^h$についても

$$
\begin{align}
\frac{d U}{d t}&=-\frac{k_{2}}{2} \frac{\partial E}{\partial U}=k_2\cdot \Bigg(\frac{1}{\sigma^{2}} \bigg[\frac{\partial f(\boldsymbol{x})}{\partial \boldsymbol{x}}\odot (\boldsymbol{I}-f(\boldsymbol{x}))\bigg] \boldsymbol{r}^{T}- \frac{1}{2}h'(U)\Bigg)\tag{11}\\
\frac{d U^h}{d t}&=-\frac{k_{2}}{2} \frac{\partial E}{\partial U^h}=k_2\cdot \Bigg(\frac{1}{\sigma_{td}^{2}}\bigg[\frac{\partial f(\boldsymbol{x}^h)}{\partial \boldsymbol{x}^h}\odot(\boldsymbol{r}-f(\boldsymbol{x}^h))\bigg] (\boldsymbol{r}^h)^{T}- \frac{1}{2}h'(U^h)\Bigg)\tag{12}
\end{align}
$$

更新則の中にはHebb則 (特に[Oja's rule](https://en.wikipedia.org/wiki/Oja%27s_rule))を表す項$(\boldsymbol{I}-f(\boldsymbol{x})) \boldsymbol{r}^{T}$が含まれます。ここで、$\boldsymbol{r}$がシナプス前細胞の発火率、$(\boldsymbol{I}-f(\boldsymbol{x}))$がシナプス後細胞の発火率を表します。

## Predictive coding networkの実装
### ネットワークの構造
(Rao & Ballard, 1999)におけるネットワークを実装しましょう。ネットワークは入力層(level-0 module), 3つのlevel-1 module, 1つのlevel-2 moduleから構成されます。

 (Rao and Ballard, 1999; Fig. 1c)

まず、白色化された自然画像を**DoG** (difference of Gaussians)フィルタで処理します。DoGフィルタはLGNのモデルですが、今回は使用しません (ただし、関数としての実装例は示しておきます)。次に前処理した自然画像から16 × 26 pxの画像パッチをランダムに切り出し、そこからさらに3つの16 × 16 pxの画像パッチ($I_0, I_1, I_2$)を切り出して3つのlevel-1 moduleへの入力とします (ただし水平に5pxずつオーバーラップさせて切り出します)。さらに入力前に16 × 16のGaussianフィルタを乗じます。

level-0 moduleの次元は3 × 16 × 16となりますが、平坦化 (flatten)して3 × 256とします。

1つのlevel-1 moduleは32ニューロンから構成され、これが3つあるので計96ニューロンとなります。ただし、**重み共有** (weight sharing)を行うことで、学習効率を向上させます。さらにlevel-2 moduleは64ニューロンで構成され、96ニューロンにフィードバックを返します。

### ライブラリのimport

```python
import cv2
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.patches as patches
from tqdm.notebook import tqdm
import scipy.io as sio

np.random.seed(0)
```

### 入力画像の前処理
入力画像の前処理用関数を定義します。前述したようにDoGフィルタは使用しません。

```python
# DoG filter as a model of LGN
def DoG(img, ksize=(5,5), sigma=1.3, k=1.6):
    g1 = cv2.GaussianBlur(img, ksize, sigma)
    g2 = cv2.GaussianBlur(img, ksize, k*sigma)
    dog = g1 - g2
    return (dog - dog.min())/(dog.max()-dog.min())

# Gaussian mask for inputs
def GaussianMask(sizex=16, sizey=16, sigma=5):
    x = np.arange(0, sizex, 1, float)
    y = np.arange(0, sizey, 1, float)
    x, y = np.meshgrid(x,y)

    x0 = sizex // 2
    y0 = sizey // 2
    mask = np.exp(-((x-x0)**2 + (y-y0)**2) / (2*(sigma**2)))
    return mask / np.sum(mask)
```

画像ファイル(`IMAGES.mat`および`IMAGES_RAW.mat`)を読み込みます。この画像ファイルは(Olshausen & Field. *Nature*. 1996)で用いられた10枚の自然画像(natural images)で<http://www.rctn.org/bruno/sparsenet/>からダウンロードできます。`IMAGES.mat`は白色化(whitening)された画像で、`IMAGES_RAW.mat`には元画像が保存されています。

```python
# Preprocess of inputs
num_images = 10
num_iter = 5000

# datasets from http://www.rctn.org/bruno/sparsenet/
mat_images = sio.loadmat('datasets/IMAGES.mat')
imgs = mat_images['IMAGES']
mat_images_raw = sio.loadmat('datasets/IMAGES_RAW.mat')
imgs_raw = mat_images_raw['IMAGESr']
```

どんな画像が保存されているか表示してみましょう。ここでは白色化前のデータを使用します。

```python
# Plot datasets
fig = plt.figure(figsize=(8, 4))
for i in range(10):
    plt.subplot(2, 5, i+1)
    plt.imshow(imgs_raw[:,:,i], cmap="gray")
    plt.axis("off")
plt.tight_layout()
fig.suptitle("Natural Images", fontsize=20)
plt.subplots_adjust(top=0.9)
```

ここで、画像を読み込み、1つのパッチを作成するコードを確認のために記述します。赤枠の部分が切り取った画像パッチです。

```python
# Get image from imglist
img = imgs[:, :, 0]
H, W = img.shape

# Get the coordinates of the upper left corner of clopping image randomly.
beginx = np.random.randint(0, W-27)
beginy = np.random.randint(0, H-17)
img_clopped = img[beginy:beginy+16, beginx:beginx+26]

# Clop three inputs
inputs = [img_clopped[:, 0:16],
          img_clopped[:, 5:21],
          img_clopped[:, 10:26]]

# Show clopped images
plt.figure(figsize=(5,10))
ax1 = plt.subplot(1,2,1)
plt.title("Orignal image")
plt.imshow(img, cmap="gray")
ax1.add_patch(patches.Rectangle(xy=(beginx, beginy),
                                width=26, height=16, ec='red', fill=False))

ax2 = plt.subplot(1,2,2)
plt.title("Clopped image")
plt.imshow(img_clopped, cmap="gray")
plt.tight_layout()
plt.show()
```

### ネットワークの実装
ネットワークを実装します。`__init__`関数でネットワークの初期化をし、`__call__`関数でネットワークの1ステップの計算を行います。また、以下の実装では$f(x)=x$としました (元のディレクトリにある`network.py`には$f(x)=\tanh(x)$の実装も付けていますが、結果があまり変わりませんでした)。よってこのネットワークは線形となります。

```python
class RaoBallard1999Model:
    def __init__(self, dt=1, sigma2=1, sigma2_td=10):
        self.dt = dt
        self.inv_sigma2 = 1/sigma2 # 1 / sigma^2
        self.inv_sigma2_td = 1/sigma2_td # 1 / sigma_td^2

        self.k1 = 0.3 # k_1: update rate
        self.k2 = 0.2 # k_2: learning rate

        self.lam = 0.02 # sparsity rate
        self.alpha = 1
        self.alphah = 0.05

        self.num_units_level0 = 256
        self.num_units_level1 = 32
        self.num_units_level2 = 128
        self.num_level1 = 3

        U = np.random.randn(self.num_units_level0,
                            self.num_units_level1)
        Uh = np.random.randn(int(self.num_level1*self.num_units_level1),
                             self.num_units_level2)
        self.U = U.astype(np.float32) * np.sqrt(2/(self.num_units_level0+self.num_units_level1))
        self.Uh = Uh.astype(np.float32) * np.sqrt(2/(int(self.num_level1*self.num_units_level1)+self.num_units_level2))

        self.r = np.zeros((self.num_level1, self.num_units_level1))
        self.rh = np.zeros((self.num_units_level2))

    def initialize_states(self, inputs):
        self.r = inputs @ self.U
        self.rh = self.Uh.T @ np.reshape(self.r, (int(self.num_level1*self.num_units_level1)))

    def calculate_total_error(self, error, errorh):
        recon_error = self.inv_sigma2*np.sum(error**2) + self.inv_sigma2_td*np.sum(errorh**2)
        sparsity_r = self.alpha*np.sum(self.r**2) + self.alphah*np.sum(self.rh**2)
        sparsity_U = self.lam*(np.sum(self.U**2) + np.sum(self.Uh**2))
        return recon_error + sparsity_r + sparsity_U

    def __call__(self, inputs, training=False):
        # inputs : (3, 256)
        r_reshaped = np.reshape(self.r, (int(self.num_level1*self.num_units_level1))) # (96)

        fx = self.r @ self.U.T
        fxh = self.Uh @ self.rh # (96, )

        # Calculate errors
        error = inputs - fx # (3, 256)
        errorh = r_reshaped - fxh # (96, )
        errorh_reshaped = np.reshape(errorh, (self.num_level1, self.num_units_level1)) # (3, 32)

        g_r = self.alpha * self.r / (1 + self.r**2) # (3, 32)
        g_rh = self.alphah * self.rh / (1 + self.rh**2) # (64, )

        # Update r and rh
        dr = self.inv_sigma2 * error @ self.U - self.inv_sigma2_td * errorh_reshaped - g_r
        drh = self.inv_sigma2_td * self.Uh.T @ errorh - g_rh

        dr = self.k1 * dr
        drh = self.k1 * drh

        # Updates
        self.r += dr
        self.rh += drh

        if training:
            dU = self.inv_sigma2 * error.T @ self.r - 3*self.lam * self.U
            dUh = self.inv_sigma2_td * np.outer(errorh, self.rh) - self.lam * self.Uh

            self.U += self.k2 * dU
            self.Uh += self.k2 * dUh

        return error, errorh, dr, drh
```

元画像における赤枠の範囲が切り取った画像パッチです。右の画像パッチを更に3つに分割して、Gaussianフィルタを乗じた後にネットワークに入力します。

### モデルの定義
上で実装したネットワークを`model`として定義します。また、シミュレーションで用いる定数などを初期化しておきます。

```python
# Define model
model = RaoBallard1999Model()

# Simulation constants
H, W, num_images = imgs.shape
nt_max = 1000 # Maximum number of simulation time
eps = 1e-3 # small value which determines convergence
input_scale = 40 # scale factor of inputs
gmask = GaussianMask() # Gaussian mask
error_list = [] # List to save errors
```

### シミュレーションの実行
シミュレーションを実行します。外側の`for loop`では画像パッチの作成と`r`と`rh`の初期化 (すなわちfeedforward処理の計算)を行います。内側の`for loop`では`r`および`rh`が収束するまで更新を行い、収束したときに重み行列`U`および`Uh`を更新します。また、`model.k2`は40画像パッチごとに1.015で除します。

```python
for iter_ in tqdm(range(num_iter)):
    # Get images randomly
    idx = np.random.randint(0, num_images)
    img = imgs[:, :, idx]

    # Get the coordinates of the upper left corner of clopping image randomly.
    beginx = np.random.randint(0, W-27)
    beginy = np.random.randint(0, H-17)
    img_clopped = img[beginy:beginy+16, beginx:beginx+26]

    # Clop three inputs
    inputs = np.array([(gmask*img_clopped[:, i*5:i*5+16]).flatten() for i in range(3)])
    inputs = (inputs - np.mean(inputs)) * input_scale

    # Reset states
    model.initialize_states(inputs)

    # Input an image patch until latent variables are converged
    for i in range(nt_max):
        # Update r and rh without update weights
        error, errorh, dr, drh = model(inputs, training=False)

        # Compute norm of r and rh
        dr_norm = np.linalg.norm(dr, ord=2)
        drh_norm = np.linalg.norm(drh, ord=2)

        # Check convergence of r and rh, then update weights
        if dr_norm < eps and drh_norm < eps:
            error, errorh, dr, drh = model(inputs, training=True)
            break

        # If failure to convergence, break and print error
        if i >= nt_max-2:
            print("Error at patch:", iter_)
            print(dr_norm, drh_norm)
            break

    error_list.append(model.calculate_total_error(error, errorh)) # Append errors

    # Decay learning rate
    if iter_ % 40 == 39:
        model.k2 /= 1.015

    # Print moving average error
    if iter_ % 1000 == 999:
        print("iter: "+str(iter_+1)+"/"+str(num_iter)+", Moving error:", np.mean(error_list[iter_-999:iter_]))
```

    HBox(children=(FloatProgress(value=0.0, max=5000.0), HTML(value='')))

    iter: 1000/5000, Moving error: 2.1590278564615817
    iter: 2000/5000, Moving error: 1.692464205507875
    iter: 3000/5000, Moving error: 1.6900546798398537
    iter: 4000/5000, Moving error: 1.7189523951808592
    iter: 5000/5000, Moving error: 1.7695244313404115

### 訓練中の損失のplot

訓練中の損失(の移動平均)をplotする。

```python
def moving_average(x, n=100) :
    ret = np.cumsum(x, dtype=float)
    ret[n:] = ret[n:] - ret[:-n]
    return ret[n - 1:] / n

moving_average_error = moving_average(np.array(error_list))
plt.figure(figsize=(5, 3))
plt.ylabel("Error")
plt.xlabel("Iterations")
plt.plot(np.arange(len(moving_average_error)), moving_average_error)
plt.show()
```

### Level-1 moduleの重み行列のplot
学習後の重み行列を可視化してみましょう。中央部にしか受容野が無いように見えますが、これは画像パッチの入力時にGaussianフィルタを乗じているためです。このため、端に受容野が見えるということはこのネットワークではまずありません。

```python
# Plot Receptive fields of level 1
fig = plt.figure(figsize=(8, 4))
plt.subplots_adjust(hspace=0.1, wspace=0.1)
for i in range(32):
    plt.subplot(4, 8, i+1)
    plt.imshow(np.reshape(model.U[:, i], (16, 16)), cmap="gray")
    plt.axis("off")

fig.suptitle("Receptive fields of level 1", fontsize=20)
plt.subplots_adjust(top=0.9)
plt.show()
```

白色が**ON領域**(興奮)、黒色が**OFF領域**(抑制)を表します。これは一次視覚野(V1)における**単純型細胞**([simple cell](https://en.wikipedia.org/wiki/Simple_cell))の受容野に類似しています。

なお、受容野について詳しくない場合は[受容野 - 脳科学辞典](https://bsd.neuroinf.jp/wiki/%E5%8F%97%E5%AE%B9%E9%87%8E)を参照してください。

### Level-2 moduleの重み行列のplot
Level-2 moduleはそのままではplotできないので、Level-1 moduleの重み行列をかけて統合します。こうすることで入力パッチの全体 (16 × 26)のサイズの受容野が得られます。128ニューロン全てを表示すると大きくなるので、24ニューロンの受容野のみを描画します。

```python
# Plot Receptive fields of level 2
zero_padding = np.zeros((80, 32))
U0 = np.concatenate((model.U, zero_padding, zero_padding))
U1 = np.concatenate((zero_padding, model.U, zero_padding))
U2 = np.concatenate((zero_padding, zero_padding, model.U))
U_ = np.concatenate((U0, U1, U2), axis = 1)
Uh_ = U_ @ model.Uh

fig = plt.figure(figsize=(8, 4))
plt.subplots_adjust(hspace=0.1, wspace=0.1)
for i in range(24):
    plt.subplot(4, 6, i+1)
    plt.imshow(np.reshape(Uh_[:, i], (16, 26), order='F'), cmap="gray")
    plt.axis("off")

fig.suptitle("Receptive fields of level 2", fontsize=20)
plt.subplots_adjust(top=0.9)
plt.savefig("RF_level2.png")
plt.show()
```

Level 1 moduleが単純型細胞のような受容野を持っていたので、その線形和であるLevel 2 moduleの受容野は**複雑型細胞**(complex cell)に類似していると言えます。ただ、複雑型細胞の受容野は元々可視化が困難(ON領域とOFF領域が重なり合うため)なので、この可視化から単純な比較はできません。

それでは論文内の結果と比較をしてみましょう。下図はFig. 2b-cで、上段がlevel-1 moduleの, 下段がlevel-2 moduleの受容野です。概ね再現できまていることが分かります。

 (Rao and Ballard, 1999; Fig. 2b-c)

tanhを活性化関数に用いた場合は(Olshausen & Field. *Nature*. 1996)のようなGaborフィルタが得られるようです (今回は再現できませんでしたが)。
- (参照)：[Sparse coding (Olshausen & Field, 1996) モデルの実装](https://salad-bowl-of-knowledge.github.io/hp/neuroscience/2020/05/01/sparse-coding.html)

 (Rao and Ballard, 1999; Fig. 2d)

なお、論文はこれだけでは終わらず、長方形(棒)を入力した時の端点(endstopping)に対する応答を実際のニューロンの出力と比較する、などを行っています。

## まとめ
- Predictive codingの初めの数理モデルを実装した
- 誤差の順方向伝搬と予測の逆伝搬という構造は予測誤差 (≒自由エネルギー)を最小化するRNNを考慮することによって生まれた

## 参考文献
### 論文
- Rao RP, Ballard DH. [Predictive coding in the visual cortex: a functional interpretation of some extra-classical receptive-field effects](https://www.nature.com/articles/nn0199_79). *Nat Neurosci*. 1999;2(1):79–87.
- Olshausen BA, Field DJ. [Emergence of simple-cell receptive field properties by learning a sparse code for natural images](https://www.nature.com/articles/381607a0). Nature. 1996;381(6583):607–609. [Data and Code](http://www.rctn.org/bruno/sparsenet/)

### GitHubで確認した他の実装例
- https://github.com/lorenzohonegger/PredictiveCodingModel
- https://github.com/miyosuda/predictive_coding

### Rao先生の講義
- [Why Consider Probabilistic Models? Computational Reasons - PowerPoint PPT](https://www.slideserve.com/tracey/why-consider-probabilistic-models-computational-reasons-powerpoint-ppt-presentation)
- [7.3 Sparse Coding and Predictive Coding - Networks that Learn: Plasticity in the Brain & Learning (Rajesh Rao) | Coursera](https://www.coursera.org/lecture/computational-neuroscience/7-3-sparse-coding-and-predictive-coding-w2qIu)


# 2-level system
Illustrating how to use the `toy_systems` package by simulating a closed two level system.

## Imports
Start by importing the necessary packages:

```python
%load_ext autoreload
%autoreload 2

import matplotlib.pyplot as plt
plt.style.use("ggplot")
import numpy as np
import qutip
from sympy import Symbol

from toy_systems.couplings import ToyCoupling, ToyEnergy
from toy_systems.decays import ToyDecay
from toy_systems.hamiltonian import Hamiltonian
from toy_systems.quantum_system import QuantumSystem
from toy_systems.states import Basis, BasisState, ToyQuantumNumbers
```

## Rabi oscillations
In Rabi oscillations the probability of being in each state flip-flops between the two states of the system sinusoidally in time. Rabi oscillations occur when the two levels are coupled by a driving field at a constant detuning from the natural frequency, and here we are also assuming a constant amplitude for the driving field.

I'm defining the system in the rotating frame so that the Hamiltonian is time-independent

### Define states
We start by defining the two states of the system: the ground state $|g\rangle$ and the excited $|e\rangle$, which we will later set to have a decay path to $|g\rangle$:

```python
g = BasisState(qn=ToyQuantumNumbers(label="g"))
e = BasisState(qn=ToyQuantumNumbers(label="e"))

# Print the states as a check
print(f"|g> = {g}")
print(f"|e> = {e}")

# Check that the states are orthogonal by taking inner product
print(f"<e|g> = {e @ g}")
```

    |g> = |g>
    |e> = |e>
    <e|g> = 0

### Define basis
We use the two orthogonal states to define a basis as shown below. The indexing of the matrix and vector representations of operators is determined by the order of the states when generating the state

```python
basis = Basis((g,e))
basis.print()
```

    |0> = |g>
    |1> = |e>

### Define energies
I'm going to define the system in the rotating frame so that the Hamiltonian is time independent. The detuning between the driving field and the natural frequency of the system is taken to be $\Delta$, so I can set the energies of the two states to be $E_g = -\Delta/2$ and $E_e = \Delta/2$:

```python
Δ = Symbol('Delta') # Using sympy to generate a symbolic Hamiltonian first
E_g = ToyEnergy([g], -Δ/2)
E_e = ToyEnergy([e], +Δ/2)
```

### Define coupling
The coupling is defined to have a constant magnitude and since we're in the rotating frame, there is no time dependence.

```python
Ω = Symbol('Omega')
coupling = ToyCoupling(g, e, Ω/2)
```

### Define decay matrix from |e> to |g>
Can add a spontaneous decay from the excited state to the ground state as shown below:

```python
decay = ToyDecay(e, g, Symbol("Gamma"))
decay.generate_matrix(basis)
decay.generate_qobj()
```

### Define a QuantumSystem
The QuantumSystem object combines the basis, Hamiltonian and decays to make setting parameters for time evolution using QuTiP more convenient.

```python
# Define the system
system = QuantumSystem(
    basis=basis,
    couplings=[E_g, E_e, coupling],
    decays=[decay],
)

# Get representations of the Hamiltonian and the decays that will be accepted by qutip
Hqobj, c_qobj = system.get_qobjs()
```

### Time-evolution using `QuTiP`
The `toy_systems` package is designed to take care of generating the Hamiltonian and collapse operators for the quantum system, and spit out a `qutip.QobjEVo`-object. The actual time-evolution is then taken care of by `qutip` (or something else if the user so desires).

```python
# Get a pointer to the time-evolution arguments
args = Hqobj.args

# Print the args dictionaries
print(f"Hqobj.args = {Hqobj.args}")
print(f"args = {args}")

# Change a parameter in args
args["Delta"] = 0

# Print the args dictionaries
print(f"Hqobj.args = {Hqobj.args}")
print(f"args = {args}")
# Both changed because args and Hqobj.args refer to the same objec
```

    Hqobj.args = {'Delta': 1, 'Omega': 1, 'Gamma': 1}
    args = {'Delta': 1, 'Omega': 1, 'Gamma': 1}
    Hqobj.args = {'Delta': 0, 'Omega': 1, 'Gamma': 1}
    args = {'Delta': 0, 'Omega': 1, 'Gamma': 1}

```python
psi0 = (1*g).qobj(basis) # Initial state
P_g_op = qutip.Qobj((1*g).density_matrix(basis), type = "oper")
P_e_op = qutip.Qobj((1*e).density_matrix(basis), type = "oper")

# Set the parameters for the system
args["Delta"] = 0
args["Omega"] = 1
args["Gamma"] = 0

times = np.linspace(0,10,1001) # Times at which result is requested
result = qutip.mesolve(Hqobj, psi0, times, c_ops = c_qobj, e_ops = [P_g_op, P_e_op])
```

Plot the result:

```python
fig, ax = plt.subplots(figsize = (16,9))
ax.plot(times, result.expect[0], label = "P_g")
ax.plot(times, result.expect[1], label = "P_e")
ax.legend()
ax.set_xlabel("Time / (2pi/Ω)")
ax.set_ylabel("Population in each state")
```

We can repeat the simulation with differrent parameters by changing the `args` of `Hqobj`:

```python
# Set the parameters for the system by using the pointer to Hqobj.args
args["Delta"] = 0
args["Omega"] = 1
args["Gamma"] = .5
```

```python
result = qutip.mesolve(Hqobj, psi0, times, c_ops = c_qobj, e_ops = [P_g_op, P_e_op])

fig, ax = plt.subplots(figsize = (16,9))
ax.plot(times, result.expect[0], label = "P_g")
ax.plot(times, result.expect[1], label = "P_e")
ax.legend()
ax.set_xlabel("Time / (1/Ω)")
ax.set_ylabel("Population in each state")
```

examples/Rabi oscillations in 2-level system.ipynb


```python
#!pip install seaborn
import sympy as sym
from sympy import apart

import matplotlib.pyplot as plt
import numpy as np
import seaborn as sns
sns.set()
sns.set_style("whitegrid", {'grid.linestyle': '--'})
sym.init_printing()
%matplotlib inline
```

# Respuesta dinámica

Ya se ha establecido que el primer paso para analizar un sistema es obtener un modelo.

En el área de la **ingeniería de control**, se configura una base de comparación del comportamiento de sistemas controlados contra señales de prueba particulares.

## Señales de prueba

Las señales de prueba más comunes corresponden a funciones impulso, escalón, rampa, parábola, etc.

Como ya se ha estudiado, la respuesta ante la señal impulso permite caracterizar completamente al sistema, sin embargo, no es posible obtener esta señal en laboratorio.

La señal de prueba más apropiada para un sistema debe determinarse por la forma más frecuente de las excitaciones en su operación normal.

### Impulso
Entendido como la derivada del escalón.
\begin{equation}
    \delta(t) = \frac{d\epsilon}{dt}
\end{equation}

Aplicando la transformada de Laplace se obtiene:
\begin{equation}
    \mathcal{L} \{ \delta(t) \} = 1
\end{equation}

Determina completamente el comportamiento del sistema.

### Escalón
\begin{equation}
\epsilon(t) = \begin{cases} 0 & t<0 \\  1 & t > 0 \end{cases}
\end{equation}

Aplicando la transformada de Laplace se obtiene:
\begin{equation}
    \mathcal{L} \{ \epsilon(t) \} = \frac{1}{s}
\end{equation}

Apropiada para visualizar el comportamiento de sistemas que buscan mantener variables fijas.

### Rampa
Entendida como la integral del escalón.

\begin{equation}
    rampa(t) = \int_{-\infty}^{t} \epsilon(\tau) d\tau = t \cdot \epsilon(t)
\end{equation}

Aplicando la transformada de Laplace se obtiene:
\begin{equation}
    \mathcal{L} \{ rampa(t) \} = \frac{1}{s^2}
\end{equation}

Apropiada para visualizar el comportamiento de sistemas que buscan variables que cambian con velocidad constante.

### Parábola
Entendida como la integral de la rampa.

\begin{equation}
    parabola(t) = \int_{-\infty}^{t} rampa(\tau) d\tau   = \frac{t^2}{2} \cdot \epsilon(t)
\end{equation}

Aplicando la transformada de Laplace se obtiene:
\begin{equation}
    \mathcal{L} \{ parabola(t) \} = \frac{1}{s^3}
\end{equation}

Apropiada para visualizar el comportamiento de sistemas que buscan variables que cambian con aceleración constante.

## Respuesta transitoria y respuesta estacionaria

Cuando un [sistema estable](https://en.wikipedia.org/wiki/BIBO_stability) es excitado en el instante $t=0$, este responde de manera continua (respuesta transitoria) desde un estado inicial relacionado con las condiciones iniciales hasta que logra equilibrio (respuesta estacionaria).

## Sistemas de primer orden

Considere la siguiente función de transferencia:

\begin{equation}
G(s) = \frac{C(s)}{R(s)} = \frac{a}{b s + c}
\end{equation}

Este sistema tiene $3$ parámetros ($a$, $b$ y $c$), sin embargo, para facilitar el análisis de estos sistemas puede transformarse la función a su forma canónica al dividir numerador y denominador por $c$.

\begin{equation}
G(s) = \frac{C(s)}{R(s)}  = \frac{\frac{a}{c}}{\frac{b}{c} s + \frac{c}{c}}= \frac{k}{\tau s + 1}
\end{equation}

En la forma canónica, se definen los parámetro como:
- $k$: ganancia en estado estacionario.
- $\tau$: constante de tiempo.

### Respuesta al escalón

Si el sistema es excitado con un escalón unitario, la señal de salida en el dominio de la frecuencia es:

\begin{equation}
C(s) = G(s) \cdot R(s) = \frac{k}{\tau s + 1} \cdot \frac{1}{s}
\end{equation}

Observe que esta expresión puede reescrbirse como:

\begin{equation}
C(s)= k \cdot \left ( \frac{1}{s} - \frac{\tau}{\tau s + 1} \right )
\end{equation}

La transformada inversa de Laplace permite obtener una expresión para la señal de salida en el dominio del tiempo $c(t)$.

\begin{equation}
c(t)= k \left ( 1 - e^{-t/\tau} \right ) \epsilon(t)
\end{equation}

Observe que:

- $c(t<0)=0$ (condción inicial).
- $\lim_{t\rightarrow \infty} c(t)=k$ (respuesta estacionaria).
- La respuesta transitoria es dominada por $e^{-t/\tau}$.
- $c(\tau) = 0.632k$, el sistema alcanza el $63.2\%$ del valor final después de un tiempo $\tau$.
- $c(2\tau) = 0.865k$, el sistema alcanza el $86.5\%$ del valor final después de un tiempo $2\tau$.
- $c(3\tau) = 0.95k$, el sistema alcanza el $95\%$ del valor final después de un tiempo $3\tau$.
- $\frac{dc}{dt}\big\vert_{t = 0^+} = \frac{d}{dt} \left ( k \left ( 1 - e^{-t/\tau} \right ) \right ) \big\vert_{t = 0^+} = \frac{k}{\tau}  e^{-t/\tau} \big\vert_{t = 0^+} =\frac{k}{\tau}$
- El sistema tiene un polo ubicado en $s=\frac{-1}{\tau}$

```python
k, tau = sym.symbols('k, tau',real=True, positive=True)
t = sym.symbols('t',real=True, positive=True)

s = sym.Symbol('s')

def L(f):
    return sym.laplace_transform(f, t, s, noconds=True)
def invL(F):
    return sym.inverse_laplace_transform(F, s, t)
```

```python
# la entrada r(t) es escalon unitario
r = sym.Heaviside(t)
r
```

```python
# la entrada R(s) es la transformada de Laplace de r(t)
R = L(r)
R
```

```python
# Se define la función de transferencia del sistema G(s)
G = k/(tau*s + 1)
g =invL(G)
G
```

```python
# la salida se calcula como C(s) = G(s)R(s)
C = G*R
C
```

```python
# la salida c(t) es la transformada inversa de Laplace de C(s)
c = invL(C)
c
```

Hasta este punto se ha encontrado la respuesta general de un sistema de primer orden ante una entrada escalón. Se recomienda variar los parámetros del sistema para identificar cómo varía su comportamiento.

```python
# Para valores particulares de k y tau
K = 1
T = 1
kt = {k: K, tau: T}
ckt = c.subs(kt)
ckt
```

```python
plt.rcParams['figure.figsize'] = 9, 3
g0 = sym.plot(r,(t,0,10),show = False, line_color='green');
g1 = sym.plot(ckt, (t,0,10),show = False, line_color='blue')
g2 = sym.plot(t*K/T,(t,0,T),show = False, line_color='red');
g0.extend(g1)
g0.extend(g2)
g0.show()
kt
```

### Respesta ante la rampa

Si el sistema es excitado con una rampa unitaria, la señal de salida en el dominio de la frecuencia es:

\begin{equation}
C(s) = G(s) \cdot R(s) = \frac{k}{\tau s + 1} \cdot \frac{1}{s^2}
\end{equation}

Observe que esta expresión puede reescrbirse como:

\begin{equation}
C(s)= k \cdot \left ( \frac{1}{s^2} - \frac{\tau}{s} + \frac{\tau^2}{\tau s + 1} \right )
\end{equation}

La transformada inversa de Laplace permite obtener una expresión para la señal de salida en el dominio del tiempo $c(t)$.

\begin{equation}
c(t)= k \left ( t - \tau + \tau e^{-t/\tau} \right ) \epsilon(t)
\end{equation}

Observe que:

- $c(t)$ se compone de una rampa ($t$), una constante ($\tau$) y una exponencial ($\tau e^{-t/\tau}$).
- $c(t<0)=0$ (condción inicial).
- A medida que $t\rightarrow \infty$, $\tau e^{-t/\tau} \rightarrow 0$, por lo que $\lim_{t\rightarrow \infty} c(t)=k \left ( t - \tau \right )$, es decir hace un seguimiento de la rampa con una desviación que depende de $k$ y $\tau$.

- $\lim_{t\rightarrow \infty} \frac{dc}{dt} = \lim_{t\rightarrow \infty} k \left (1 - e^{-t/\tau} \right ) = k$

```python
# la salida se calcula como C(s) = G(s)R(s)
C_ramp = G*(1/s**2)
C_ramp
```

```python
# la salida c_ramp(t) es la transformada inversa de Laplace de C(s)
c_ramp = invL(C_ramp)
c_ramp
```

```python
# Para valores particulares de k y tau
K = 1.3
T = 2
kt = {k: K, tau: T}
c_rampkt = c_ramp.subs(kt)
c_rampkt
```

```python
plt.rcParams['figure.figsize'] = 9, 3
g0 = sym.plot(t,(t,0,10),show = False, line_color='green');
g1 = sym.plot(c_rampkt, (t,0,10),show = False, line_color='blue')
g2 = sym.plot(t*K,(t,0,10),show = False, line_color='red');
g0.extend(g1)
g0.extend(g2)
g0.show()
kt
```

```python
from ipywidgets import interact

evalfimpulse = sym.lambdify((k, tau, t), g , 'numpy')
evalfstep = sym.lambdify((k, tau, t), c, 'numpy')
evalframp = sym.lambdify((k, tau, t), c_ramp, 'numpy')

ts = np.linspace(0, 10)
def orden1(tau_in, K_in):
    plt.figure(figsize=(7, 5))
    ax_impulse = plt.subplot2grid((3, 2), (0, 0))
    ax_step = plt.subplot2grid((3, 2), (1, 0))
    ax_ramp = plt.subplot2grid((3, 2), (2, 0))
    ax_complex = plt.subplot2grid((2, 2), (0, 1), rowspan=2)

    ax_impulse.plot(ts, evalfimpulse(K_in, tau_in, ts))
    ax_impulse.set_title('Resp. al impulso')
    ax_impulse.set_ylim(0, 10)

    tau_height = 1 - np.exp(-1)
    ax_step.set_title('Resp. al escalón')
    ax_step.plot(ts, evalfstep(K_in, tau_in, ts),ts,K_in*ts**0)
    ax_step.set_ylim(0, 10)

    ax_ramp.set_title('Resp. a la rampa')
    ax_ramp.plot(ts, evalframp(K_in, tau_in, ts),ts,K_in*ts)
    ax_ramp.set_ylim(0, 100)

    ax_complex.set_title('Polos y ceros')
    ax_complex.scatter(-1/tau_in, 0, marker='x', s=30)
    ax_complex.axhline(0, color='black')
    ax_complex.axvline(0, color='black')
    ax_complex.axis([-10, 10, -10, 10])

interact(orden1, tau_in=(0.1, 5), K_in=(0.1, 10.));
```

    interactive(children=(FloatSlider(value=2.5500000000000003, description='tau_in', max=5.0, min=0.1), FloatSlid…

Una vez analizadas las simulaciones anteriores, comparta sus ideas para obtener modelos de primer orden de forma experimental.

## Sistemas de segundo orden

Considere la siguiente función de transferencia:

\begin{equation}
G(s) = \frac{C(s)}{R(s)} = \frac{a}{b s^2 + c s + d}
\end{equation}

Este sistema tiene $4$ parámetros ($a$, $b$, $c$ y $d$), sin embargo, para facilitar el análisis de estos sistemas, puede transformarse la función a su forma canónica al dividir numerador y denominador por $b$.

\begin{equation}
G(s) = \frac{C(s)}{R(s)} = \frac{\frac{a}{b}}{\frac{b}{b} s^2 + \frac{c}{b} s + \frac{d}{b}} = \frac{k\omega_0^2}{s^2 + 2 \zeta\omega_0s + \omega_0^2}
\end{equation}

En la forma canónica, se definen los parámetro como:
- $k$: ganancia en estado estacionario.
- $\omega_0$: frecuencia natural.
- $\zeta$: coeficiente de amortiguación.

Considerando que el sistema es estable, puede observarse que:
- Este sistema no tiene **ceros**, pues no existe un valor de $s$ que haga que el numerador $k\omega_0^2$ sea igual a cero.
- El sistema tiene dos **polos** ubicados en

$$s_1,s_2 =-\zeta \omega_0 \pm \omega_0\sqrt{\zeta^2 - 1}$$

Así, deben considerarse 3 casos:
1. $\zeta < 1$, por lo cual $\sqrt{\zeta^2 - 1}$ arroja un parte imaginaria y el sistema tiene dos polos complejos conjugados. En este caso, el sistema es **subamortiguado**.

2. $\zeta = 1$, por lo cual $\sqrt{\zeta^2 - 1} = 0$ y el sistema tendría dos polos reales repetidos. En este caso, el sistema es **críticamente amortiguado**.

3. $\zeta > 1$, por lo cual $\sqrt{\zeta^2 - 1}$ arroja un parte real y el sistema tiene dos polos reales diferentes. En este caso, el sistema es **sobreamortiguado**.

### Caso subamortiguado

La función de transferencia del sistema es:

\begin{equation}
G(s) = \frac{C(s)}{R(s)} = \frac{k\omega_0^2}{s^2 + 2 \zeta\omega_0s + \omega_0^2}
\end{equation}

En este caso se considera que $\zeta < 1$, por lo cual $\sqrt{\zeta^2 - 1}$ arroja un parte imaginaria y el sistema tiene dos polos complejos conjugados.

#### Respuesta al escalón

Si el sistema es excitado con un escalón unitario, la señal de salida en el dominio de la frecuencia es:

\begin{equation}
C(s) = G(s) \cdot R(s) = \frac{k\omega_0^2}{s^2 + 2 \zeta\omega_0s + \omega_0^2} \cdot \frac{1}{s}
\end{equation}

Observe que esta expresión puede reescrbirse como:

\begin{equation}
C(s)= k \cdot \left ( \frac{1}{s} - \frac{s + 2\zeta\omega_0}{s^2 + 2\zeta\omega_0s + \omega_0^2} \right )
\end{equation}

Suponiendo que la parte imaginaria de los polos es $\omega_d = \omega_0\sqrt{1-\zeta^2}$, la respuesta se puede reescribir como:

\begin{equation}
C(s)= k \cdot \left ( \frac{1}{s} - \frac{s + \zeta\omega_0}{(s+\zeta\omega_0)^2 + \omega_d^2} - \frac{\zeta\omega_0}{(s+\zeta\omega_0)^2 + \omega_d^2} \right )
\end{equation}

La transformada inversa de Laplace permite obtener una expresión para la señal de salida en el dominio del tiempo $c(t)$.

\begin{align}
c(t) &= k \left ( 1 - e^{-\zeta\omega_0 t} \cos{\omega_d t} - \frac{\zeta\omega_0}{\omega_d}e^{-\zeta\omega_0 t} \sin{\omega_d t}\right ) \epsilon(t) \\
&= k \left ( 1 - e^{-\zeta\omega_0 t} \cos{\omega_d t} - \frac{\zeta}{\sqrt{1-\zeta^2}}e^{-\zeta\omega_0 t} \sin{\omega_d t}\right ) \epsilon(t) \\
&= k \left ( 1 - \frac{e^{-\zeta\omega_0 t}}{\sqrt{1-\zeta^2}} \sin \left (\omega_d t + \tan^{-1} \frac{\sqrt{1-\zeta^2}}{\zeta} \right )\right ) \epsilon(t) \\
&= k \left ( 1 - \frac{e^{-\zeta\omega_0 t}}{\sqrt{1-\zeta^2}} \sin \left (\omega_0 \sqrt{1-\zeta^2} t + \tan^{-1} \frac{\sqrt{1-\zeta^2}}{\zeta} \right )\right ) \epsilon(t)
\end{align}

Observe que:

- $c(t<0)=0$ (condción inicial).
- $\lim_{t\rightarrow \infty} c(t)=k$ (respuesta estacionaria).
- La respuesta transitoria es dominada por $e^{-\zeta\omega_0 t}$ siendo modulada por una oscilación cuya frecuencia es $\omega_d = \omega_0 \sqrt{1-\zeta^2}$.
- Si $\zeta = 0 $, el sistema oscilaría eternamente.
- El sistema tiene dos polos ubicado en $s_1,s_2 =-\zeta \omega_0 \pm j \omega_d$

```python
k, omega0, zeta, omegad = sym.symbols('k, omega_0, zeta, omega_d',real=True, positive=True)
t = sym.symbols('t',real=True, positive=True)
wd = {omegad : omega0*sym.sqrt(1-zeta**2)}

s = sym.Symbol('s')

G2_sub = k*omega0**2/((s + zeta*omega0 + omegad*1j)*(s + zeta*omega0 - omegad*1j))
G2_sub # La función de transferencia
```

```python
G2_sub_step = G2_sub*(1/s) # La respuesta ante el escalón
G2_sub_step = G2_sub_step.apart(s)   # Descompone larespuesta al escalón en fracciones parciales
G2_sub_step
```

```python
g2_sub_step = invL(G2_sub_step).subs(wd).factor().simplify()
g2_sub_step
```

```python
parametros = {k: -5,omega0: 8, zeta: 0.25}
sym.plot(g2_sub_step.subs(parametros),(t,0,10))

```

### Caso críticamente amortiguado

En este caso se considera que $\zeta = 1$, por lo cual $\sqrt{\zeta^2 - 1}$ arroja un parte imaginaria igual a cero polos reales repetidos. La función de transferencia del sistema queda:

\begin{equation}
G(s) = \frac{C(s)}{R(s)} = \frac{k\omega_0^2}{s^2 + 2 \omega_0s + \omega_0^2} =  \frac{k\omega_0^2}{\left (s + \omega_0 \right )^2}
\end{equation}

#### Respuesta al escalón

Si el sistema es excitado con un escalón unitario, la señal de salida en el dominio de la frecuencia es:

\begin{equation}
C(s) = G(s) \cdot R(s) = \frac{k\omega_0^2}{\left (s + \omega_0 \right )^2} \cdot \frac{1}{s}
\end{equation}

Observe que esta expresión puede reescrbirse como:

\begin{equation}
C(s)= k \cdot \left ( \frac{1}{s} - \frac{\omega_0}{\left ( s + \omega_0 \right )^2} - \frac{k}{s + \omega_0} \right )
\end{equation}

Aplicando la transformada inversa se obtiene:

\begin{align}
c(t) &= k \left ( 1 - \omega_0 t e^{-\omega_0 t} - e^{-\omega_0 t} \right ) \epsilon(t) \\
&= k \left ( 1 - (1 + \omega_0 t  )e^{-\omega_0 t} \right ) \epsilon(t)
\end{align}

- $c(t<0)=0$ (condción inicial).
- $\lim_{t\rightarrow \infty} c(t)=k$ (respuesta estacionaria).
- La respuesta transitoria es dominada por $(1 + \omega_0 t  )e^{-\omega_0 t}$.
- El sistema tiene dos polos ubicados en $s_1,s_2 =-\omega_0$

```python
k, omega0 = sym.symbols('k, omega_0',real=True, positive=True)
t = sym.symbols('t',real=True, positive=True)

s = sym.Symbol('s')

G2_cri = k*omega0**2/(s + omega0)**2
G2_cri # La función de transferencia
```

```python
G2_cri_step = G2_cri*(1/s) # La respuesta ante el escalón
G2_cri_step = G2_cri_step.apart(s)   # Descompone larespuesta al escalón en fracciones parciales
G2_cri_step
```

```python
g2_cri_step = invL(G2_cri_step).factor().simplify()
g2_cri_step
```

```python
parametros = {k: 5,omega0: 6}
sym.plot(g2_cri_step.subs(parametros),(t,0,2))
```

### Caso sobreamortiguado

En este caso se considera que $\zeta > 1$, por lo cual $\sqrt{\zeta^2 - 1}$ arroja un parte imaginaria igual a cero y polos reales diferentes en $s_1,s_2 = \omega_0 (-\zeta  \pm \sqrt{\zeta^2 - 1})$. La función de transferencia del sistema queda:

\begin{equation}
G(s) = \frac{C(s)}{R(s)} = \frac{k\omega_0^2}{s^2 + 2 \zeta \omega_0s + \omega_0^2} =  \frac{k s_1 s_2}{\left (-s + s_1 \right ) \left (-s + s_2 \right )}
\end{equation}

#### Respuesta al escalón

Si el sistema es excitado con un escalón unitario, la señal de salida en el dominio de la frecuencia es:

\begin{equation}
C(s) = G(s) \cdot R(s) = \frac{k s_1 s_2}{\left (-s + s_1 \right ) \left (-s + s_2 \right )} \cdot \frac{1}{s}
\end{equation}

Observe que esta expresión puede reescrbirse como:

\begin{equation}
C(s)= k \cdot \left ( \frac{1}{s} - \frac{s_1}{s_1 - s_2}\frac{1}{s+s_2} + \frac{s_2}{s_1 - s_2}\frac{1}{s+s_1} \right )
\end{equation}

\begin{equation}
C(s)= k \cdot \left ( \frac{1}{s} +  \frac{1}{s_1 - s_2} \left (  \frac{s_1}{s+s_2} + \frac{s_2}{s+s_1} \right )  \right )
\end{equation}

Aplicando la transformada inversa se obtiene:

\begin{equation}
c(t)= k \cdot \left ( 1 +  \frac{1}{s_1 - s_2} \left (s_1 e^{-s_2 t} +  s_2 e^{-s_1 t} \right )  \right )
\end{equation}

- $c(t<0)=0$ (condción inicial).
- $\lim_{t\rightarrow \infty} c(t)=k$ (respuesta estacionaria).
- La respuesta transitoria es dominada por dos exponenciales reales.
- El sistema tiene dos polos reales ubicados en $s_1,s_2 = \omega_0 (-\zeta  \pm \sqrt{\zeta^2 - 1})$

```python
k, s1,s2,zeta,omega0 = sym.symbols('k, s_1, s_2,\zeta,\omega_0',real=True, positive=True)
t = sym.symbols('t',real=True, positive=True)

s = sym.Symbol('s')

G2_sob = k*s1*s2/((-s + s1)*(-s+s2))
G2_sob # La función de transferencia
```

```python
G2_sob_step = G2_sob*(1/s) # La respuesta ante el escalón
G2_sob_step = G2_sob_step.apart(s)   # Descompone larespuesta al escalón en fracciones parciales
G2_sob_step
```

```python
g2_sob_step = invL(G2_sob_step).factor().simplify()
g2_sob_step
```

```python
s1s2 = {s1: omega0*(-zeta + sym.sqrt(zeta**2-1)) ,s2:  omega0*(-zeta - sym.sqrt(zeta**2-1))}
# \omega_0 (-\zeta  \pm \sqrt{\zeta^2 - 1})
g2_sob_step = g2_sob_step.subs(s1s2).simplify()
g2_sob_step
```

```python
parametros1 = {k: 1.0,omega0: 2.0, zeta: 1.1}
parametros2 = {k: 1.0,omega0: 2.0, zeta: 2.5}
parametros3 = {k: 1.0,omega0: 2.0, zeta: 5}

g1 = sym.plot(g2_sob_step.subs(parametros1),
              (t,0,10),show=False,line_color='green')
g2 = sym.plot(g2_sob_step.subs(parametros2),
              (t,0,10),show=False, line_color='blue')
g3 = sym.plot(g2_sob_step.subs(parametros3),
              (t,0,10),show=False, line_color='red')
g1.extend(g2)
g1.extend(g3)
g1.show()
```

**Pregunta**
¿Qué esperaría que ocurra si el coeficiente de amoriguamiento de un sistema de segundo orden es "muy grande"?