# Kinematic chain in a plane (2D)

> Marcos Duarte, Renato Naville Watanabe
> [Laboratory of Biomechanics and Motor Control](http://pesquisa.ufabc.edu.br/bmclab)
> Federal University of ABC, Brazil

Contents

1  Properties of kinematic chains

2  The kinematics of one-link system

2.1  Forward and inverse kinematics

2.2  Matrix representation of the kinematics

3  Differential kinematics

3.1  Linear velocity of the endpoint

3.2  Linear acceleration of the endpoint

3.2.1  Tangential acceleration

3.2.2  Centripetal acceleration

3.3  Jacobian matrix

3.4  Derivative of a vector-valued function using the Jacobian matrix

3.5  Jacobian matrix in the context of kinematic chains

3.5.1  Jacobian matrix of one-link chain

4  The kinematics of a two-link chain

4.1  Joint and segment angles

4.2  Inverse kinematics

5  Differential  kinematics

6  Further reading

7  Video lectures on the Internet

8  Problems

9  References

Kinematic chain refers to an assembly of rigid bodies (links) connected by joints that is the mathematical model for a mechanical system which in turn can represent a biological system such as the human arm ([Wikipedia](http://en.wikipedia.org/wiki/Kinematic_chain)). The term chain refers to the fact that the links are constrained by their connections (typically, by a hinge joint which is also called pin joint or revolute joint) to other links. As consequence of this constraint, a kinematic chain in a plane is an example of circular motion of a rigid object.

Chapter 16 of Ruina and Rudra's book is a good formal introduction on the topic of circular motion of a rigid object. However, in this notebook we will not employ the mathematical formalism introduced in that chapter - the concept of a rotating reference frame and the related rotation matrix - we cover these subjects in the notebooks [Time-varying frame of reference](http://nbviewer.jupyter.org/github/BMClab/BMC/blob/master/notebooks/Time-varying%20frames.ipynb) and [Rigid-body transformations (2D)](https://nbviewer.jupyter.org/github/BMClab/BMC/blob/master/notebooks/Transformation2D.ipynb). Now, we will describe the kinematics of a chain in a Cartesian coordinate system using trigonometry and calculus. This approach is simpler and more intuitive but it gets too complicated for a kinematic chain with many links or in the 3D space. For such more complicated problems, it would be recommended using rigid transformations (see for example, Siciliano et al. (2009)).

We will deduce the kinematic properties of kinematic chains algebraically using [Sympy](http://sympy.org/), a Python library for symbolic mathematics. And in Sympy we could have used the [mechanics module](http://docs.sympy.org/latest/modules/physics/mechanics/index.html), a specific module for creation of symbolic equations of motion for multibody systems, but let's deduce most of the stuff by ourselves to understand the details.

## Properties of kinematic chains

For a kinematic chain, the base is the extremity (origin) of a kinematic chain which is typically considered attached to the ground, body or fixed. The endpoint is the other extremity (end) of a kinematic chain and typically can move. In robotics, the term end-effector is used and usually refers to a last link (rigid body) in this chain.

In topological terms, a kinematic chain is termed open when there is only one sequence of links connecting the two ends of the chain. Otherwise it's termed closed and in this case a sequence of links forms a loop. A kinematic chain can be classified as serial or parallel or a mixed of both. In a serial chain the links are connected in a serial order. A serial chain is an open chain, otherwise it is a parallel chain or a branched chain (e.g., hand and fingers).

Although the definition above is clear and classic in mechanics, it is unfortunately not the definition used by health professionals (clinicians and athletic trainers) when describing human movement. They refer to human joints and segments as a closed or open kinematic (or kinetic) chain simply if the distal segment (typically the foot or hand) is fixed (closed chain) or not (open chain).
In this text we will be consistent with mechanics, but keep in mind this difference when interacting with clinicians and athletic trainers.

Another important term to characterize a kinematic chain is degree of freedom (DOF). In mechanics, the degree of freedom of a mechanical system is the number of independent parameters that define its configuration or that determine the state of a physical system. A particle in the 3D space has three DOFs because we need three coordinates to specify its position. A rigid body in the 3D space has six DOFs because we need three coordinates of one point at the body to specify its position and three angles to to specify its orientation in order to completely define the configuration of the rigid body. For a link attached to a fixed body by a hinge joint in a plane, all we need to define the configuration of the link is one angle and then this link has only one DOF. A kinematic chain with two links in a plane has two DOFs, and so on.

The mobility of a kinematic chain is its total number of degrees of freedom. The redundancy of a kinematic chain is its mobility minus the number of degrees of freedom of the endpoint.

## The kinematics of one-link system

First, let's study the case of a system composed by one planar hinge joint and one link, which technically it's not a chain but it will be useful to review (or introduce) key concepts.

Figure. One link attached to a fixed body by a hinge joint in a plane.

First, let's import the necessary libraries from Python and its ecosystem:

```python
import numpy as np
import matplotlib.pyplot as plt
%matplotlib inline
import seaborn as sns
sns.set_context("notebook", font_scale=1.2, rc={"lines.linewidth": 2,
                                                "lines.markersize": 10})
from IPython.display import display, Math

from sympy import Symbol, symbols, Function
from sympy import Matrix, simplify, lambdify, expand, latex
from sympy import diff, cos, sin, sqrt, acos, atan2, atan, Abs
from sympy.vector import CoordSys3D
from sympy.physics.mechanics import dynamicsymbols, mlatex, init_vprinting
init_vprinting()

import sys
sys.path.insert(1, r'./../functions')  # add to pythonpath
```

We need to define a Cartesian coordinate system and the symbolic variables, $t$, $\ell$, $\theta$ (and make $\theta$ a function of time):

```python
G = CoordSys3D('')
t = Symbol('t')
l = Symbol('ell', real=True, positive=True)
# type \theta and press tab for the Greek letter θ
θ = dynamicsymbols('theta', real=True)  # or Function('theta')(t)
```

Using trigonometry, the endpoint position in terms of the joint angle and link length is:

```python
r_p = l*cos(θ)*G.i + l*sin(θ)*G.j + 0*G.k
r_p
```

With the components:

```python
r_p.components
```

### Forward and inverse kinematics

Computing the configuration of a link or a chain (including the endpoint location) from the joint parameters (joint angles and link lengths) as we have done is called [forward or direct kinematics](https://en.wikipedia.org/wiki/Forward_kinematics).

If the linear coordinates of the endpoint position are known (for example, if they are measured with a motion capture system) and one wants to obtain the joint angle(s), this process is known as [inverse kinematics](https://en.wikipedia.org/wiki/Inverse_kinematics). For the one-link system above:

$$ \theta = arctan\left(\frac{y_P}{x_P}\right) $$

### Matrix representation of the kinematics

The mathematical manipulation will be easier if we use the matrix formalism (and let's drop the explicit dependence on $t$):

```python
r = Matrix((r_p.dot(G.i), r_p.dot(G.j)))
r
```

Using the matrix formalism will simplify things, but we will loose some of the Sympy methods for vectors (for instance, the variable `r_p` has a method `magnitude` and the variable `r` does not.
If you prefer, you can keep the pure vector representation and just switch to matrix representation when displaying a variable:

```python
r_p.to_matrix(G)
```

The third element of the matrix above refers to the   $\hat{\mathbf{k}}$ component which is zero for the present case (planar movement).

## Differential kinematics

Differential kinematics gives the relationship between the joint velocities and the corresponding endpoint linear velocity. This mapping is described by a matrix, termed [Jacobian matrix](http://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant), which depends on the kinematic chain configuration and it is of great use in the study of kinematic chains.
First, let's deduce the endpoint velocity without using the Jacobian and then we will see how to calculate the endpoint velocity using the Jacobian matrix.

The velocity of the endpoint can be obtained by the first-order derivative of the position vector. The derivative of a vector is obtained by differentiating each vector component:

$$
\frac{\mathrm{d}\overrightarrow{\mathbf{r}}}{\mathrm{d}t} =
\large
\begin{bmatrix}
\frac{\mathrm{d}x_P}{\mathrm{d}t} \\
\frac{\mathrm{d}y_P}{\mathrm{d}t} \\
\end{bmatrix}
$$

Note that the derivative is with respect to time but $x_P$ and $y_P$ depend explicitly on $\theta$ and it's $\theta$ that depends on $t$ ($x_P$ and $y_P$ depend implicitly on $t$). To calculate this type of derivative we will use the [chain rule](http://en.wikipedia.org/wiki/Chain_rule).

Chain rule

For variable $f$ which is function of variable $g$ which in turn is function of variable $t$, $f(g(t))$ or $(f\circ g)(t)$, the derivative of $f$  with respect to $t$ is (using Lagrange's notation):

$$(f\circ g)^{'}(t) = f'(g(t)) \cdot g'(t)$$

Or using what is known as Leibniz's notation:

$$\frac{\mathrm{d}f}{\mathrm{d}t} = \frac{\mathrm{d}f}{\mathrm{d}g} \cdot \frac{\mathrm{d}g}{\mathrm{d}t}$$

If $f$ is function of two other variables which both are function of  $t$, $ f(x(t),y(t))$, the chain rule for this case is:

$$\frac{\mathrm{d}f}{\mathrm{d}t} = \frac{\partial f}{\partial x} \cdot \frac{\mathrm{d}x}{\mathrm{d}t} + \frac{\partial f}{\partial y} \cdot \frac{\mathrm{d}y}{\mathrm{d}t}$$

Where $df/dt$ represents the total derivative and $\partial f / \partial x$ represents the partial derivative of a function.

Product rule

The derivative of the product of two functions is:

$$ (f \cdot g)' = f' \cdot g + f \cdot g'  $$

### Linear velocity of the endpoint

For the planar one-link case, the linear velocity of the endpoint is:

```python
v = r.diff(t)
v
```

Where we used the [Newton's notation](http://en.wikipedia.org/wiki/Notation_for_differentiation) for differentiation.
Note that $\dot{\theta}$ represents the unknown angular velocity of the joint; this is why the derivative of $\theta$ is not explicitly solved.
The magnitude or [Euclidian norm](http://en.wikipedia.org/wiki/Vector_norm) of the vector $\overrightarrow{\mathbf{v}}$ is:

$$ ||\overrightarrow{\mathbf{v}}||=\sqrt{v_x^2+v_y^2} $$

```python
simplify(sqrt(v[0]**2 + v[1]**2))
```

Which is $\ell\dot{\theta}$.

We could have used the function `norm` of Sympy, but the output does not simplify nicely:

```python
simplify(v.norm())
```

The direction of $\overrightarrow{\mathbf{v}}$ is tangent to the circular trajectory of the endpoint as can be seen in the figure below where its components are also shown.

Figure. Endpoint velocity of one link attached to a fixed body by a hinge joint in a plane.

### Linear acceleration of the endpoint

The acceleration of the endpoint position can be given by the second-order derivative of the position or by the first-order derivative of the velocity.
Using the chain and product rules for differentiation, the linear acceleration of the endpoint is:

```python
acc = v.diff(t, 1)
acc
```

Examining the terms of the expression for the linear acceleration, we see there are two types of them: the term (in each direction) proportional to the angular acceleration $\ddot{\theta}$ and other term proportional to the square of the angular velocity $\dot{\theta}^{2}$.

#### Tangential acceleration

The term proportional to angular acceleration, $a_t$, is always tangent to the trajectory of the endpoint (see figure below) and it's magnitude or Euclidean norm is:

```python
A = θ.diff(t, 2)
simplify(sqrt(expand(acc[0]).coeff(A)**2 + expand(acc[1]).coeff(A)**2))*A
```

#### Centripetal acceleration

The term proportional to angular velocity, $a_c$, always points to the joint, the center of the circular motion (see figure below), because of that this term is termed [centripetal acceleration](http://en.wikipedia.org/wiki/Centripetal_acceleration#Tangential_and_centripetal_acceleration). Its magnitude is:

```python
A = θ.diff(t)**2
simplify(sqrt(expand(acc[0]).coeff(A)**2+expand(acc[1]).coeff(A)**2))*A
```

This means that there will be a linear acceleration even if the angular acceleration is zero because although the magnitude of the linear velocity is constant in this case, its direction varies (due to the centripetal acceleration).

Figure. Endpoint tangential and centripetal acceleration terms of one link attached to a fixed body by a hinge joint in a plane.

Let's plot some simulated data to have an idea of the one-link kinematics.
Consider  $\ell=1\:m,\theta_i=0^o,\theta_f=90^o $, and  $1\:s$ of movement duration, and that it is a [minimum-jerk movement](https://nbviewer.jupyter.org/github/BMClab/bmc/blob/master/notebooks/MinimumJerkHypothesis.ipynb).

```python
θ_i, θ_f, d = 0, np.pi/2, 1
ts = np.arange(0.01, 1.01, .01)
mjt  = θ_i + (θ_f - θ_i)*(10*(t/d)**3 - 15*(t/d)**4 + 6*(t/d)**5)

ang  = lambdify(t, mjt, 'numpy'); ang = ang(ts)
vang = lambdify(t, mjt.diff(t,1), 'numpy'); vang = vang(ts)
aang = lambdify(t, mjt.diff(t,2), 'numpy'); aang = aang(ts)
jang = lambdify(t, mjt.diff(t,3), 'numpy'); jang = jang(ts)

b, c, d, e = symbols('b c d e')
dicti = {l:1, θ:b, θ.diff(t, 1):c, θ.diff(t, 2):d, θ.diff(t, 3):e}

r2 = r.subs(dicti);
rxfu = lambdify(b, r2[0], modules = 'numpy')
ryfu = lambdify(b, r2[1], modules = 'numpy')

v2 = v.subs(dicti);
vxfu = lambdify((b, c), v2[0], modules = 'numpy')
vyfu = lambdify((b, c), v2[1], modules = 'numpy')

acc2 = acc.subs(dicti);
axfu = lambdify((b, c, d), acc2[0], modules = 'numpy')
ayfu = lambdify((b, c, d), acc2[1], modules = 'numpy')

jerk = r.diff(t,3)
jerk2 = jerk.subs(dicti);
jxfu = lambdify((b, c, d, e), jerk2[0], modules = 'numpy')
jyfu = lambdify((b, c, d, e), jerk2[1], modules = 'numpy')
```

```python
fig, hax = plt.subplots(2, 4, sharex = True, figsize=(14, 7))

hax[0, 0].plot(ts, ang*180/np.pi, linewidth=3)
hax[0, 0].set_title('Angular displacement [ $^o$]');
hax[0, 0].set_ylabel('Joint')

hax[0, 1].plot(ts, vang*180/np.pi, linewidth=3)
hax[0, 1].set_title('Angular velocity [ $^o/s$]');

hax[0, 2].plot(ts, aang*180/np.pi, linewidth=3)
hax[0, 2].set_title('Angular acceleration [ $^o/s^2$]');

hax[0, 3].plot(ts, jang*180/np.pi, linewidth=3)
hax[0, 3].set_title('Angular jerk [ $^o/s^3$]');

hax[1, 0].plot(ts, rxfu(ang), 'r', linewidth=3, label = 'x')
hax[1, 0].plot(ts, ryfu(ang), 'k', linewidth=3, label = 'y')
hax[1, 0].set_title('Linear displacement [$m$]');
hax[1, 0].legend(loc='best').get_frame().set_alpha(0.8)
hax[1, 0].set_ylabel('Endpoint')

hax[1, 1].plot(ts,vxfu(ang, vang), 'r', linewidth=3)
hax[1, 1].plot(ts,vyfu(ang, vang), 'k', linewidth=3)
hax[1, 1].set_title('Linear velocity [$m/s$]');

hax[1, 2].plot(ts,axfu(ang, vang, aang), 'r', linewidth=3)
hax[1, 2].plot(ts,ayfu(ang, vang, aang), 'k', linewidth=3)
hax[1, 2].set_title('Linear acceleration [$m/s^2$]');

hax[1, 3].plot(ts, jxfu(ang, vang, aang, jang), 'r', linewidth=3)
hax[1, 3].plot(ts, jyfu(ang, vang, aang, jang), 'k', linewidth=3)
hax[1, 3].set_title('Linear jerk [$m/s^3$]');

fig.suptitle('Minimum jerk trajectory kinematics of one-link system', fontsize=20);
for i, hax2 in enumerate(hax.flat):
        hax2.locator_params(nbins=5)
        hax2.grid(True)
        if i > 3:
            hax2.set_xlabel('Time [s]');
plt.subplots_adjust(hspace=0.2, wspace=.3) #plt.tight_layout()
```

### Jacobian matrix

The Jacobian matrix is the matrix of all first-order partial derivatives of a vector-valued function $F$:

$$
F(q_1,...q_n) = \begin{bmatrix}F_{1}(q_1,...q_n)\\
\vdots\\
F_{m}(q_1,...q_n)\\
\end{bmatrix}
$$

In a general form, the Jacobian matrix of the function $F$ is:

$$
\mathbf{J}=
\large
\begin{bmatrix}
\frac{\partial F_{1}}{\partial q_{1}} & ... & \frac{\partial F_{1}}{\partial q_{n}} \\
\vdots  & \ddots  & \vdots \\
\frac{\partial F_{m}}{\partial q_{1}} & ... & \frac{\partial F_{m}}{\partial q_{n}} \\
\end{bmatrix}
$$

### Derivative of a vector-valued function using the Jacobian matrix

The time-derivative of a vector-valued function $F$ can be computed using the Jacobian matrix:

$$
\frac{dF}{dt} = \mathbf{J} \cdot \begin{bmatrix}\frac{d q_1}{dt}\\
\vdots\\
\frac{d q_n}{dt}\\
\end{bmatrix}
$$

### Jacobian matrix in the context of kinematic chains

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. The Jacobian matrix for a kinematic chain relates differential changes in the joint angle vector with the resulting differential changes in the linear position vector of the endpoint.

For a kinematic chain, the function $F_{i}$ is the expression of the endpoint position in $m$ coordinates and the variable $q_{i}$ is the angle of each $n$ joints.

#### Jacobian matrix of one-link chain

For the planar one-link case, the Jacobian matrix of the position vector of the endpoint $r_P$ with respect to the angular position vector $q_1=\theta$ is:

\begin{equation}
\mathbf{J}=
\large
\begin{bmatrix}
\frac{\partial x_P}{\partial \theta} \\
\frac{\partial y_P}{\partial \theta} \\
\end{bmatrix}
\end{equation}

Which evaluates to:

```python
J = r.diff(θ)
J
```

And Sympy has a function to calculate the Jacobian:

```python
J = r.jacobian([θ])
J
```

We can recalculate the kinematic expressions using the Jacobian matrix, which can be useful for simplifying the deduction.

The linear velocity of the end-effector is given by the product between the Jacobian of the kinematic link and the angular velocity:

$$ \overrightarrow{\mathbf{v}} = \mathbf{J} \cdot \dot{\theta}$$

Where:

```python
ω = θ.diff(t)
ω
```

The angular velocity is also a vector; it's direction is perpendicular to the plane of rotation and using the [right-hand rule](http://en.wikipedia.org/wiki/Right-hand_rule) this direction is the same as of the versor $\hat{\mathbf{k}}$ coming out of the screen (paper).

Then:

```python
velJ = J*ω
velJ
```

And the linear acceleration of the endpoint is given by the derivative of this product:

$$ \overrightarrow{\mathbf{a}} = \dot{\mathbf{J}} \cdot \overrightarrow{\mathbf{\omega}} + \mathbf{J} \cdot \dot{\overrightarrow{\mathbf{\omega}}} $$

Let's calculate this derivative:

```python
accJ = J.diff(t)*ω + J*ω.diff(t)
accJ
```

These two expressions derived with the Jacobian are the same as the direct derivatives of the equation for the endpoint position.

## The kinematics of a two-link chain

We now will look at the case of a planar kinematic chain with two links, as shown below. The deduction will be similar to the case with one link we just saw.

Figure. Kinematics of a two-link chain with hinge joints in a plane.

We need to define a Cartesian coordinate system and the symbolic variables $t,\:\ell_1,\:\ell_2,\:\theta_1,\:\theta_2$ (and make $\theta_1$ and $\theta_2$ function of time):

```python
G = CoordSys3D('')
t = Symbol('t')
l1, l2 = symbols('ell_1 ell_2', positive=True)
θ1, θ2 = dynamicsymbols('theta1 theta2')
```

The position of the endpoint in terms of the joint angles and link lengths is:

```python
r2_p = (l1*cos(θ1) + l2*cos(θ1 + θ2))*G.i + (l1*sin(θ1) + l2*sin(θ1 + θ2))*G.j
r2_p
```

With the components:

```python
r2_p.components
```

And in matrix form:

```python
r2 = Matrix((r2_p.dot(G.i), r2_p.dot(G.j)))
r2
```

### Joint and segment angles

Note that  $\theta_2$ is a joint angle (referred as measured in the joint space); the angle of the segment 2 with respect to the horizontal is  $\theta_1+\theta_2$ and is referred as an angle in the segmental space.
Joint and segment angles are also referred as relative and absolute angles, respectively.

### Inverse kinematics

Using the [cosine rule](http://en.wikipedia.org/wiki/Law_of_cosines), in terms of the endpoint position, the angle $\theta_2$ is:

\begin{equation}
x_P^2 + y_P^2 = \ell_1^2+\ell_2^2 - 2\ell_1 \ell_2 cos(\pi-\theta_2)
\end{equation}

\begin{equation}
\theta_2 = \arccos\left(\frac{x_P^2 + y_P^2 - \ell_1^2 - \ell_2^2}{2\ell_1 \ell_2}\;\;\right)
\end{equation}

To find the angle $\theta_1$, if we now look at the triangle in red in the figure below, its angle $\phi$ is:

\begin{equation}
\phi = \arctan\left(\frac{\ell_2 \sin(\theta_2)}{\ell_1 + \ell_2 \cos(\theta_2)}\right)
\end{equation}

And the angle of its hypotenuse with the horizontal is:

\begin{equation}
\theta_1 + \phi = \arctan\left(\frac{y_P}{x_P}\right)
\end{equation}

Then, the angle $\theta_1$ is:

\begin{equation}
\theta_1 = \arctan\left(\frac{y_P}{x_P}\right) - \arctan\left(\frac{\ell_2 \sin(\theta_2)}{\ell_1+\ell_2 \cos(\theta_2)}\right)
\end{equation}

Note that there are two possible sets of $(\theta_1, \theta_2)$ angles for the same $(x_P, y_P)$ coordinate that satisfy the equations above. The figure below shows in orange another possible configuration of the kinematic chain with the same endpoint coordinate. The other solution is $\theta_2'=2\pi - \theta_2$, but $\sin(\theta_2')=-sin(\theta_{2})$ and then the $arctan()$ term in the last equation becomes negative.
Even for a simple two-link chain we already have a problem of redundancy, there is more than one joint configuration for the same endpoint position; this will be much more problematic for chains with more links (more degrees of freedom).

Figure. Indetermination in the inverse kinematics approach to determine one of the joint angles for a two-link chain with hinge joints in a plane.

## Differential kinematics

The linear velocity of the endpoint is:

```python
vel2 = r2.diff(t)
vel2
```

The linear velocity of the endpoint is the sum of the velocities at each joint, i.e., it is the velocity of the endpoint in relation to joint 2, for instance,
$\ell_2cos(\theta_1 + \theta_2)\dot{\theta}_1$, plus the velocity of joint 2 in relation to joint 1, for instance, $\ell_1\dot{\theta}_1 cos(\theta_1)$, and this last term we already saw for the one-link example. In classical mechanics this is known as [relative velocity](http://en.wikipedia.org/wiki/Relative_velocity), an example of [Galilean transformation](http://en.wikipedia.org/wiki/Galilean_transformation).

The linear acceleration of the endpoint is:

```python
acc2 = r2.diff(t, 2)
acc2
```

We can separate the equation above for the linear acceleration in three types of terms: proportional to $\ddot{\theta}$ and to $\dot{\theta}^2$, as we already saw for the one-link case, and a new term, proportional to $\dot{\theta}_1\dot{\theta}_2$:

```python
acc2 = acc2.expand()
A = θ1.diff(t, 2)
B = θ2.diff(t, 2)
tg = A*Matrix((acc2[0].coeff(A),acc2[1].coeff(A)))+B*Matrix((acc2[0].coeff(B),acc2[1].coeff(B)))

A = θ1.diff(t)**2
B = θ2.diff(t)**2
ct = A*Matrix((acc2[0].coeff(A),acc2[1].coeff(A)))+B*Matrix((acc2[0].coeff(B),acc2[1].coeff(B)))

A = θ1.diff(t)*θ2.diff(t)
co = A*Matrix((acc2[0].coeff(A),acc2[1].coeff(A)))

display(Math(mlatex(r'Tangential:\:') + mlatex(tg)))
display(Math(mlatex(r'Centripetal:') + mlatex(ct)))
display(Math(mlatex(r'Coriolis:\;\;\;\;\:') + mlatex(co)))
```

$\displaystyle Tangential:\:\left[\begin{matrix}- \ell_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) \ddot{\theta}_{2} + \left(- \ell_{1} \operatorname{sin}\left(\theta_{1}\right) - \ell_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right)\right) \ddot{\theta}_{1}\\\ell_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) \ddot{\theta}_{2} + \left(\ell_{1} \operatorname{cos}\left(\theta_{1}\right) + \ell_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right)\right) \ddot{\theta}_{1}\end{matrix}\right]$

$\displaystyle Centripetal:\left[\begin{matrix}- \ell_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) \dot{\theta}_{2}^{2} + \left(- \ell_{1} \operatorname{cos}\left(\theta_{1}\right) - \ell_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right)\right) \dot{\theta}_{1}^{2}\\- \ell_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) \dot{\theta}_{2}^{2} + \left(- \ell_{1} \operatorname{sin}\left(\theta_{1}\right) - \ell_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right)\right) \dot{\theta}_{1}^{2}\end{matrix}\right]$

$\displaystyle Coriolis:\;\;\;\;\:\left[\begin{matrix}- 2 \ell_{2} \operatorname{cos}\left(\theta_{1} + \theta_{2}\right) \dot{\theta}_{1} \dot{\theta}_{2}\\- 2 \ell_{2} \operatorname{sin}\left(\theta_{1} + \theta_{2}\right) \dot{\theta}_{1} \dot{\theta}_{2}\end{matrix}\right]$

This new term is called the [Coriolis acceleration](http://en.wikipedia.org/wiki/Coriolis_effect); it is 'felt' by the endpoint when its distance to the instantaneous center of rotation varies, due to the links' constraints, and as consequence the endpoint motion is deflected (its direction is perpendicular to the relative linear velocity of the endpoint with respect to the linear velocity at the second joint, $\mathbf{v} - \mathbf{v}_{joint2}$.

Let's now deduce the Jacobian for this planar two-link chain:

$$
\mathbf{J} =
\large
\begin{bmatrix}
\frac{\partial x_P}{\partial \theta_{1}} & \frac{\partial x_P}{\partial \theta_{2}} \\
\frac{\partial y_P}{\partial \theta_{1}} & \frac{\partial y_P}{\partial \theta_{2}} \\
\end{bmatrix}
$$

We could manually run:
```python
J = Matrix([[r2[0].diff(theta1), r2[0].diff(theta2)], [r2[1].diff(theta1), r2[1].diff(theta2)]])
```
But it's shorter with the Jacobian function from Sympy:

```python
J2 = r2.jacobian([θ1, θ2])
J2
```

Using the Jacobian, the linear velocity of the endpoint is:

$$ \mathbf{v_J} = \mathbf{J} \cdot \begin{bmatrix}\dot{\theta_1}\\
\dot{\theta_2}\\
\end{bmatrix} $$

Where:

```python
ω2 = Matrix((θ1, θ2)).diff(t)
ω2
```

Then:

```python
vel2J = J2*ω2
vel2J
```

This expression derived with the Jacobian is the same as the first-order derivative  of the equation for the endpoint position. We can show this equality by comparing the two expressions with Sympy:

```python
vel2.expand() == vel2J.expand()
```

    True

Once again, the linear acceleration of the endpoint is given by the derivative of the product between the Jacobian and the angular velocity:

\begin{equation}
\mathbf{a} = \dot{\mathbf{J}} \cdot \mathbf{\omega} + \mathbf{J} \cdot \dot{\mathbf{\omega}}
\end{equation}

Let's calculate this derivative:

```python
acc2J = J2.diff(t)*ω2 + J2*ω2.diff(t)
acc2J
```

Once again, the expression above is the same as the second-order derivative of the equation for the endpoint position:

```python
acc2.expand() == acc2J.expand()
```

    True

Let's plot some simulated data to have an idea of the two-link kinematics.
Consider 1 s of movement duration, $\ell_1=\ell_2=0.5m, \theta_1(0)=\theta_2(0)=0$, $\theta_1(1)=\theta_2(1)=90^o$, and that the endpoint trajectory is a [minimum-jerk movement](https://nbviewer.jupyter.org/github/BMClab/bmc/blob/master/notebooks/MinimumJerkHypothesis.ipynb).

First, the simulated trajectories:

```python
t, p0, pf, d = symbols('t p0 pf d')
rx = dynamicsymbols('rx', real=True)  # or Function('rx')(t)
ry = dynamicsymbols('ry', real=True)  # or Function('ry')(t)

# minimum jerk kinematics
mjt = p0 + (pf - p0)*(10*(t/d)**3 - 15*(t/d)**4 + 6*(t/d)**5)
rfu = lambdify((t, p0, pf, d), mjt, 'numpy')
vfu = lambdify((t, p0, pf, d), diff(mjt, t, 1), 'numpy')
afu = lambdify((t, p0, pf, d), diff(mjt, t, 2), 'numpy')
jfu = lambdify((t, p0, pf, d), diff(mjt, t, 3), 'numpy')
# values
d, L1, L2 = 1, .5, .5
#initial values:
p0, pf = [-0.5, 0.5], [0, .5*np.sqrt(2)]
ts = np.arange(0.01, 1.01, .01)

# endpoint kinematics
x  = rfu(ts, p0[0], pf[0], d)
y  = rfu(ts, p0[1], pf[1], d)
vx = vfu(ts, p0[0], pf[0], d)
vy = vfu(ts, p0[1], pf[1], d)
ax = afu(ts, p0[0], pf[0], d)
ay = afu(ts, p0[1], pf[1], d)
jx = jfu(ts, p0[0], pf[0], d)
jy = jfu(ts, p0[1], pf[1], d)

# inverse kinematics
ang2b = np.arccos((x**2 + y**2 - L1**2 - L2**2)/(2*L1*L2))
ang1b = np.arctan2(y, x) - (np.arctan2(L2*np.sin(ang2b), (L1+L2*np.cos(ang2b))))

ang2 = acos((rx**2 + ry**2 - l1**2 - l2**2)/(2*l1*l2))
ang2fu = lambdify((rx ,ry, l1, l2), ang2, 'numpy');
ang2 = ang2fu(x, y, L1, L2)
ang1 = atan2(ry, rx) - (atan(l2*sin(acos((rx**2 + ry**2 - l1**2 - l2**2)/(2*l1*l2)))/ \
                            (l1+l2*cos(acos((rx**2 + ry**2 - l1**2 - l2**2)/(2*l1*l2))))))
ang1fu = lambdify((rx, ry, l1, l2), ang1, 'numpy');
ang1 = ang1fu(x, y, L1, L2)

ang2b = acos((rx**2 + ry**2 - l1**2 - l2**2)/(2*l1*l2))
ang1b = atan2(ry, rx) - (atan(l2*sin(acos((rx**2 + ry**2 - l1**2 - l2**2)/(2*l1*l2)))/ \
                            (l1 + l2*cos(acos((rx**2 + ry**2-l1**2 - l2**2)/(2*l1*l2))))))
X, Y, Xd, Yd, Xdd, Ydd, Xddd, Yddd = symbols('X Y Xd Yd Xdd Ydd Xddd Yddd')
dicti = {rx:X, ry:Y, rx.diff(t, 1):Xd, ry.diff(t, 1):Yd, \
        rx.diff(t, 2):Xdd, ry.diff(t, 2):Ydd, rx.diff(t, 3):Xddd, ry.diff(t, 3):Yddd, l1:L1, l2:L2}
vang1 = diff(ang1b, t, 1)
vang1 = vang1.subs(dicti)
vang1fu = lambdify((X, Y, Xd, Yd, l1, l2), vang1, 'numpy')
vang1 = vang1fu(x, y, vx, vy, L1, L2)
vang2 = diff(ang2b, t, 1)
vang2 = vang2.subs(dicti)
vang2fu = lambdify((X, Y, Xd, Yd, l1, l2), vang2, 'numpy')
vang2 = vang2fu(x, y, vx, vy, L1, L2)

aang1 = diff(ang1b, t, 2)
aang1 = aang1.subs(dicti)
aang1fu = lambdify((X, Y, Xd, Yd, Xdd, Ydd, l1, l2), aang1, 'numpy')
aang1 = aang1fu(x, y, vx, vy, ax, ay, L1, L2)
aang2 = diff(ang2b, t, 2)
aang2 = aang2.subs(dicti)
aang2fu = lambdify((X, Y, Xd, Yd, Xdd, Ydd, l1, l2), aang2, 'numpy')
aang2 = aang2fu(x, y, vx, vy, ax, ay, L1, L2)

jang1 = diff(ang1b, t, 3)
jang1 = jang1.subs(dicti)
jang1fu = lambdify((X, Y, Xd, Yd, Xdd, Ydd, Xddd, Yddd, l1, l2), jang1, 'numpy')
jang1 = jang1fu(x, y, vx, vy, ax, ay, jx, jy, L1, L2)
jang2 = diff(ang2b, t, 3)
jang2 = jang2.subs(dicti)
jang2fu = lambdify((X, Y, Xd, Yd, Xdd, Ydd, Xddd, Yddd, l1, l2), jang2, 'numpy')
jang2 = jang2fu(x, y, vx, vy, ax, ay, jx, jy, L1, L2)
```

And the plots for the trajectories:

```python
fig, hax = plt.subplots(2, 4, sharex = True, figsize=(14, 7))

hax[0, 0].plot(ts, x, 'r', linewidth=3, label = 'x')
hax[0, 0].plot(ts, y, 'k', linewidth=3, label = 'y')
hax[0, 0].set_title('Linear displacement [$m$]')
hax[0, 0].legend(loc='best').get_frame().set_alpha(0.8)
hax[0, 0].set_ylabel('Endpoint')

hax[0, 1].plot(ts, vx, 'r', linewidth=3)
hax[0, 1].plot(ts, vy, 'k', linewidth=3)
hax[0, 1].set_title('Linear velocity [$m/s$]')

hax[0, 2].plot(ts, ax, 'r', linewidth=3)
hax[0, 2].plot(ts, ay, 'k', linewidth=3)
hax[0, 2].set_title('Linear acceleration [$m/s^2$]')

hax[0, 3].plot(ts, jx, 'r', linewidth=3)
hax[0, 3].plot(ts, jy, 'k', linewidth=3)
hax[0, 3].set_title('Linear jerk [$m/s^3$]')

hax[1, 0].plot(ts, ang1*180/np.pi, 'b', linewidth=3, label = 'Ang1')
hax[1, 0].plot(ts, ang2*180/np.pi, 'g', linewidth=3, label = 'Ang2')
hax[1, 0].set_title('Angular displacement [ $^o$]')
hax[1, 0].legend(loc='best').get_frame().set_alpha(0.8)
hax[1, 0].set_ylabel('Joint')

hax[1, 1].plot(ts, vang1*180/np.pi, 'b', linewidth=3)
hax[1, 1].plot(ts, vang2*180/np.pi, 'g', linewidth=3)
hax[1, 1].set_title('Angular velocity [ $^o/s$]')

hax[1, 2].plot(ts, aang1*180/np.pi, 'b', linewidth=3)
hax[1, 2].plot(ts, aang2*180/np.pi, 'g', linewidth=3)
hax[1, 2].set_title('Angular acceleration [ $^o/s^2$]')

hax[1, 3].plot(ts, jang1*180/np.pi, 'b', linewidth=3)
hax[1, 3].plot(ts, jang2*180/np.pi, 'g', linewidth=3)
hax[1, 3].set_title('Angular jerk [ $^o/s^3$]')

tit = fig.suptitle('Minimum jerk trajectory kinematics of a two-link chain', fontsize=20)
for i, hax2 in enumerate(hax.flat):
    hax2.locator_params(nbins=5)
    hax2.grid(True)
    if i > 3:
        hax2.set_xlabel('Time [$s$]')

plt.subplots_adjust(hspace=0.15, wspace=0.25) #plt.tight_layout()

fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(14, 4))

ax1.plot(x, y, 'r', linewidth=3)
ax1.set_xlabel('Displacement in x [$m$]')
ax1.set_ylabel('Displacement in y [$m$]')
ax1.set_title('Endpoint space', fontsize=14)
ax1.axis('equal')
ax1.grid(True)

ax2.plot(ang1*180/np.pi, ang2*180/np.pi, 'b', linewidth=3)
ax2.set_xlabel('Displacement in joint 1 [ $^o$]')
ax2.set_ylabel('Displacement in joint 2 [ $^o$]')
ax2.set_title('Joint sapace', fontsize=14)
ax2.axis('equal')
ax2.grid(True)
```

## Further reading

 - Read pages 477-494 of the 10th chapter of the [Ruina and Rudra's book](http://ruina.tam.cornell.edu/Book/index.html) for a review of differential equations and kinematics.

## Video lectures on the Internet

 - Khan Academy: [Differential Calculus Review](https://khanacademy.org/math/differential-calculus)
 - Khan Academy: [Chain Rule Review](https://khanacademy.org/math/differential-calculus/dc-chain)
 - [Multivariate Calculus – Jacobian applied](https://www.youtube.com/watch?v=57q-2YxIZss)

## Problems

1. For the numerical example of the two-link chain plotted above, calculate and plot the values for the each type of acceleration (tangential, centripetal and Coriolis). See solution below.

2. For the two-link chain, calculate and interpret the Jacobian and the expressions for the position, velocity, and acceleration of the endpoint for the following cases:
 a) When the first joint (the joint at the base) is fixed at   $0^o$.
 b) When the second joint is fixed at $0^o$.

3. For the two-link chain, a special case of movement occurs when the endpoint moves along a line passing through the first joint (the joint at the base). A system with this behavior is known as a polar manipulator (Mussa-Ivaldi, 1986). For simplicity, consider that the lengths of the two links are equal to $\ell$. In this case, the two joint angles are related by: $2\theta_1+\theta_2=\pi$.
 a) Calculate the Jacobian for this polar manipulator and compare it with the Jacobian for the  standard two-link chain. Note the difference between the off-diagonal terms.
 b) Calculate the expressions for the endpoint position, velocity, and acceleration.
 c) For the endpoint acceleration of the polar manipulator, identify the tangential, centrifugal, and Coriolis components and compare them with the expressions for the standard two-link chain.

4. Deduce the equations for the kinematics of a two-link pendulum with the angles in relation to the vertical.

5. Deduce the equations for the kinematics of a two-link system using segment angles and compare with the deduction employing joint angles.

6. Calculate the Jacobian matrix for the following function:

\begin{equation}
f(x, y) = \begin{bmatrix}
x^2 y \\
5 x + \sin y \end{bmatrix}
\end{equation}

#### Calculation of each type of acceleration of the endpoint for the numerical example of the two-link system

```python
# tangential acceleration
A1, A2, A1d, A2d, A1dd, A2dd = symbols('A1 A2 A1d A2d A1dd A2dd')
dicti = {θ1:A1, θ2:A2, θ1.diff(t, 1):A1d, θ2.diff(t,1):A2d, \
         θ1.diff(t, 2):A1dd, θ2.diff(t, 2):A2dd, l1:L1, l2:L2}
tg2 = tg.subs(dicti)
tg2fu = lambdify((A1, A2, A1dd, A2dd), tg2, 'numpy');
tg2n = tg2fu(ang1, ang2, aang1, aang2)
tg2n = tg2n.reshape((2, 100)).T
# centripetal acceleration
ct2 = ct.subs(dicti)
ct2fu = lambdify((A1, A2, A1d, A2d), ct2, 'numpy');
ct2n = ct2fu(ang1, ang2, vang1, vang2)
ct2n = ct2n.reshape((2, 100)).T
# coriolis acceleration
co2 = co.subs(dicti)
co2fu = lambdify((A1, A2, A1d, A2d), co2, 'numpy');
co2n = co2fu(ang1, ang2, vang1, vang2)
co2n = co2n.reshape((2, 100)).T
# total acceleration (it has to be the same calculated before)
acc_tot = tg2n + ct2n + co2n
```

#### And the corresponding plots

```python
fig, hax = plt.subplots(1, 3, sharex = True, sharey = True, figsize=(12, 5))
hax[0].plot(ts, acc_tot[:, 0], color=(1, 0, 0, .3), linewidth=5, label = 'x total')
hax[0].plot(ts, acc_tot[:, 1], color=(0, 0, 0, .3), linewidth=5, label = 'y total')
hax[0].plot(ts, tg2n[:, 0], 'r', linewidth=2, label = 'x')
hax[0].plot(ts, tg2n[:, 1], 'k', linewidth=2, label = 'y')
hax[0].set_title('Tangential')
hax[0].set_ylabel('Endpoint acceleration [$m/s^2$]')
hax[0].set_xlabel('Time [$s$]')
hax[1].plot(ts, acc_tot[:, 0], color=(1,0,0,.3), linewidth=5, label = 'x total')
hax[1].plot(ts, acc_tot[:, 1], color=(0,0,0,.3), linewidth=5, label = 'y total')
hax[1].plot(ts, ct2n[:, 0], 'r', linewidth=2, label = 'x')
hax[1].plot(ts, ct2n[:, 1], 'k', linewidth=2, label = 'y')
hax[1].set_title('Centripetal')
hax[1].set_xlabel('Time [$s$]')
hax[1].legend(loc='best').get_frame().set_alpha(0.8)
hax[2].plot(ts, acc_tot[:, 0], color=(1,0,0,.3), linewidth=5, label = 'x total')
hax[2].plot(ts, acc_tot[:, 1], color=(0,0,0,.3), linewidth=5, label = 'y total')
hax[2].plot(ts, co2n[:, 0], 'r', linewidth=2, label = 'x')
hax[2].plot(ts, co2n[:, 1], 'k', linewidth=2, label = 'y')
hax[2].set_title('Coriolis')
hax[2].set_xlabel('Time [$s$]')
tit = fig.suptitle('Acceleration terms for the minimum jerk trajectory of a two-link chain',
                   fontsize=16)
for hax2 in hax:
    hax2.locator_params(nbins=5)
    hax2.grid(True)
# plt.subplots_adjust(hspace=0.15, wspace=0.25) #plt.tight_layout()
```

## References

- Mussa-Ivaldi FA (1986) Compliance.  In: Morasso P Tagliasco V (eds), [Human Movement Understanding: from computational geometry to artificial Intelligence](http://books.google.com.br/books?id=ZlZyLKNoAtEC). North-Holland, Amsterdam.
- Ruina A, Rudra P (2019) [Introduction to Statics and Dynamics](http://ruina.tam.cornell.edu/Book/index.html). Oxford University Press.
- Siciliano B et al. (2009) [Robotics - Modelling, Planning and Control](http://books.google.com.br/books/about/Robotics.html?hl=pt-BR&id=jPCAFmE-logC). Springer-Verlag London.
- Zatsiorsky VM (1998) [Kinematics of Human Motions](http://books.google.com.br/books/about/Kinematics_of_Human_Motion.html?id=Pql_xXdbrMcC&redir_esc=y). Champaign, Human Kinetics.


# Non-Linear Shooting Method
#### John S Butler john.s.butler@tudublin.ie   [Course Notes](https://johnsbutler.netlify.com/files/Teaching/Numerical_Analysis_for_Differential_Equations.pdf)    [Github](https://github.com/john-s-butler-dit/Numerical-Analysis-Python)

## Overview
This notebook illustates the implentation of a the non-linear shooting method to a non-linear boundary value problem.

The non-linear shooting method is a bit like the game Angry Birds to make a first guess and then you refine.
The video below walks through the code.

```python
from IPython.display import HTML
HTML('')
```

## Introduction
To numerically approximate the non-linear Boundary Value Problem
\begin{equation}
y^{''}=f(x,y,y^{'}), \ \ \ a < x < b, \end{equation}
with the boundary conditions$$y(a)=\alpha,\end{equation} and
\begin{equation}y(b) =\beta,\end{equation}
using the non-linear shooting method, the Boundary Value Problem is divided into two Initial Value Problems:
1. The first 2nd order non-linear Initial Value Problem is the same as the original Boundary Value Problem with an extra initial condtion $y_1^{'}(a)=\lambda_0$.
\begin{equation}
 y^{''}_1=f(x,y,y^{'}), \ \    y_1(a)=\alpha, \ \ \color{red}{y^{'}_1(a)=\lambda_0},\\
\end{equation}
2. The second 2nd order Initial Value Problem is with respect to $z=\frac{\partial y}{\partial \lambda}$  with the initial condtions $z(a)=0$ and $z^{'}(a)=1$.

\begin{equation}
z^{''}=\frac{\partial f}{\partial y^{'}}z^{'}(x,\lambda)+\frac{\partial f}{\partial y}z(x,\lambda), \ \ \color{green}{z(a)=0, \ \ z^{'}(a)=1}.
\end{equation}
combining these results together to get the unique solution
\begin{equation}
y(x)=y_1(x).
\end{equation}
Unlike the linear method, the non-linear shooting method is iterative to get the value of $\lambda$ that results in the same solution as the Boundary Value Problem.

The first choice of $\lambda_0$ is a guess, then after the first iteration a Newton Raphson method is used to update $\lambda,$

\begin{equation}\lambda_k=\lambda_{k-1}-\frac{y_1(b,\lambda_{k-1})-\beta}{\frac{dy}{d \lambda}(b,\lambda_{k-1})},$$
which can be re-written as,
\begin{equation}\lambda_k=\lambda_{k-1}-\frac{y_1(b,\lambda_{k-1})-\beta}{z(b,\lambda_{k-1})},$$

until $|\lambda_{k}-\lambda_{k-1}|<tol$.

The introduction of $\lambda$ is the introduction of a new variable that motivates the second 2nd order Initial Value Problem.

```python
import numpy as np
import math
import matplotlib.pyplot as plt
import warnings
warnings.filterwarnings("ignore")
```

## Example Non-linear Boundary Value Problem
To illustrate the shooting method we shall apply it to the non-linear Boundary Value Problem:
\begin{equation} y^{''}=-2yy^{'}, \end{equation}
with boundary conditions
\begin{equation}y(0) = -2.5, \end{equation}
\begin{equation}y(1) = 3. \end{equation}
The __boundary value problem__ is broken into two second order __Initial Value Problems:__
1. The first 2nd order Initial Value Problem is the same as the original Boundary Value Problem with an extra initial condtion $y^{'}(0)=\lambda_0$.
\begin{equation}
u^{''} =-2uu', \ \ \ \ u(0)=-2.5, \ \ \ \color{green}{u^{'}(0)=\lambda_0},
\end{equation}

which can be written a two first order initial value problems
\begin{equation}
u_1^{'} =u_2, \ \ \ \ u_1(0)=-2.5,
\end{equation}
\begin{equation}
u_2^{'} =-2u_1u_2, \ \ \ \ \color{green}{u_2^{'}(0)=\lambda_0}.
\end{equation}

2. The second 2nd order Initial Value Problem  $z=\frac{\partial y}{\partial \lambda}$  with the initial condtions $z(a)=0$ and $z^{'}(a)=1$.with the initial condtions $z^{'}(0)=0$ and $z^{'}(0)=1$.
\begin{equation}
z^{''} =-2y^{'}z-2yz^{'}, \ \ \ \ \color{green}{z(0)=0}, \ \ \ \color{green}{z^{'}(0)=1}
\end{equation}
which can be written a two first order initial value problems
\begin{equation}
z_1^{'} =z_2, \ \ \ \ z_1(0)=0,
\end{equation}
\begin{equation}
z_2^{'} =-2u_2z_1-2u_1z_2, \ \ \ \ \color{green}{z_2^{'}(0)=1}.
\end{equation}

The first choice of $\lambda$ we guess $\lambda_0=0.2$. Then after the first iteration a Newton Raphson method is used to update $\lambda$

\begin{equation}\lambda_k=\lambda_{k-1}-\frac{u_1(1,\lambda_{k-1})-3}{z_1(1,\lambda_{k-1})}\end{equation}
until $\lambda_{k}-\lambda_{k-1}<0.001$.

## Discrete Axis
The stepsize is defined as
\begin{equation}h=\frac{b-a}{N}\end{equation}
here it is
\begin{equation}h=\frac{1-0}{10}\end{equation}
giving
\begin{equation}x_i=0+0.1 i\end{equation}
for $i=0,1,...10.$
The plot shows the discrete time intervals.

```python
## BVP
N=10
h=1/N
x=np.linspace(0,1,N+1)
fig = plt.figure(figsize=(10,4))
plt.plot(x,0*x,'o:',color='red')
plt.xlim((0,1))
plt.xlabel('x',fontsize=16)
plt.title('Illustration of discrete time points for h=%s'%(h),fontsize=32)
plt.show()
```

## Initial conditions
The initial conditions for the discrete equations are:
\begin{equation} u_1[0]=-2.5\end{equation}
\begin{equation} u_2[0]=\lambda_0\end{equation}
\begin{equation} z_1[0]=0\end{equation}
\begin{equation} z_2[0]=1\end{equation}
\begin{equation}\beta=3\end{equation}
Let $\lambda_0=0.2$

```python
U1=np.zeros(N+1)
U2=np.zeros(N+1)
Z1=np.zeros(N+1)
Z2=np.zeros(N+1)

lambda_app=[0.2]
U1[0]=-2.5
U2[0]=lambda_app[0]

Z1[0]=0
Z2[0]=1
beta=3
```

## Numerical method
The Euler method is applied to numerically approximate the solution of the system of the two second order non-linear initial value problems they are converted in to two pairs of two first order non-linear initial value problems:
1. Discrete form of Equation 1
\begin{equation}u_{1 }[i+1]=u_{1}[i] + h u_{2 }[i]\end{equation}
\begin{equation}u_{2 }[i+1]=u_{2 }[i] + h (-2u_{1}[i]u_{2 }[i])\end{equation}
with the initial conditions $u_{1}[0]=-2.5$ and $u_{2}[0]=\lambda_0.$

2. Discrete form of Equation 2
\begin{equation}z_{1}[i+1]=z_{1 }[i] + h z_{2 }[i]\end{equation}
\begin{equation}z_{2}[i+1]=z_{2 }[i] + h (-2z_{2}[i]y_{1 }[i]-2z_{1 }[i] y_{2 }[i])\end{equation}
with the initial conditions $z_{1}[0]=0$ and $z_{2}[0]=1$.

At the end of each iteration
\begin{equation} u_1[i] \approx y(x_i).\end{equation}
The initial choice of $\lambda_0=0.2$ is up to the user then for each iteration of the system $\lambda$ is updated using the formula:
\begin{equation}\lambda_k=\lambda_{k-1}-\frac{u_1[10]-3}{z_1[10]}.\end{equation}

The plot below shows the numerical approximation of the solution to the non-linear Boundary Value Problem for each iteration.

The stopping criteria for the iterative process is
\begin{equation}|\lambda_k-\lambda_{k-1}|<tol,\end{equation}
with $tol=0.0001.$

```python
tol=0.0001
k=0
fig = plt.figure(figsize=(10,4))
while  k<20:
    k=k+1
    for i in range (0,N):
        U1[i+1]=U1[i]+h*(U2[i])
        U2[i+1]=U2[i]+h*(-2*U2[i]*U1[i])

        Z1[i+1]=Z1[i]+h*(Z2[i])
        Z2[i+1]=Z2[i]+h*(-2*U2[i]*Z1[i]-2*Z2[i]*U1[i])

    lambda_app.append(lambda_app[k-1]-(U1[N]-beta)/Z1[N])

    plt.plot(x,U1,':o',label=r"$\lambda$ %s"%(k))
    plt.xlabel('x',fontsize=16)
    plt.ylabel('U1',fontsize=16)
    U2[0]=lambda_app[k]
    if abs(lambda_app[k]-lambda_app[k-1])<tol:
        break

plt.legend(loc='best')
plt.title(r"Iterations of $\lambda_k$",fontsize=32)
plt.show()
```

## Results
The plot below shows the final iteration of the numerical approximation for the non-linear boundary value problem.

```python
fig = plt.figure(figsize=(10,4))
plt.grid(True)
plt.plot(x,U1,'b:o')
plt.title("Numerical Approximation of the non-linear Boundary Value Problem",fontsize=32)
plt.xlabel('x',fontsize=16)
plt.ylabel("u_1$",fontsize=16)
plt.show()

```

## $\lambda$ Iteration
The plot below shows the iterations of $\lambda_k$, after $4$ iterations the value of $\lambda$ stabilies.

```python
fig = plt.figure(figsize=(10,4))
plt.grid(True)
plt.plot(lambda_app,'o')
#plt.title("Values of $\lambda$ for each interation ",fontsize=32)
plt.xlabel('Iterations (k)',fontsize=16)
plt.ylabel("lambda",fontsize=16)
plt.show()
```

```python

```

Chapter 06 - Boundary Value Problems/602_Non-Linear Shooting Method.ipynb


```python
import sympy
from phasor.utilities.ipynb.displays import *
from phasor.utilities.ipynb.ipy_sympy import *
import scipy.linalg

import numpy.testing as np_test
import declarative as decl
from declarative.bunch import (
    DeepBunch,
)

#import numpy as np

from phasor import system
from phasor import readouts
from phasor import optics
from phasor.utilities.print import pprint

from phasor.optics.models import AOMTestStand
```

    Populating the interactive namespace from numpy and matplotlib
    Sympy version:  1.0

```python
db = DeepBunch()
db.test.LO.phase.val = 0
db.test.LO.phase.units = 'deg'
db.test.aom.N_ode = 100
db.test.aom.solution_order = 0
sys = system.BGSystem(
    ctree = db,
)
sys.own.test = AOMTestStand.AOMTestStand()
db = sys.ctree_shadow()
sys.test.LO.amplitude
print(sys.test.DC_R1.DC_readout)
print(sys.test.DC_R2.DC_readout)
print(sys.test.DC_R1.DC_readout + sys.test.DC_R2.DC_readout)
```

    Number of states:  10
    6.143540920213429e-05
    1.00006168857196
    1.00012312398

```python
db = DeepBunch()
db.test.LO.phase.val = 0
db.test.LO.phase.units = 'deg'
db.test.LO.amplitude = np.linspace(0, 1, 100)
db.test.aom.N_ode = 100
db.test.aom.solution_order = 0
sys = system.BGSystem(
    ctree = db,
)
sys.own.test = AOMTestStand.AOMTestStand()
db = sys.ctree_shadow()
#sys.test.LO.amplitude

```

```python
axB = mplfigB(Nrows=1)
test = sys.test
axB.ax0.plot(test.LO.amplitude, test.DC_R1.DC_readout, color = 'red')
axB.ax0.plot(test.LO.amplitude, test.DC_R2.DC_readout, color = 'blue')
axB.ax0.plot(test.LO.amplitude, test.DC_R1.DC_readout + test.DC_R2.DC_readout, color = 'black')
axB.ax0.set_ylim(0, 1.1)
```

```python
db = DeepBunch()
db.test.aoms.AOM1.N_ode = 100
db.test.aoms.VCO_AOM1.generate.amplitude = np.linspace(.01, 1.5, 100)
db.test.aoms.VCO_AOM2.generate.amplitude = .01
sys = system.BGSystem(
    ctree = db,
)
sys.own.test = AOMTestStand.AOM2VCOTestStand(
    VCO2_use = True,
)
db = sys.ctree_shadow()
#sys.test.LO.amplitude

```

```python
axB = mplfigB(Nrows=2)
test = sys.test
X = sys.test.aoms.VCO_AOM1.generate.amplitude
Y = sys.test.aoms.VCO_AOM2.generate.amplitude
axB.ax0.plot(X, test.DC_R.DC_readout, color = 'red')
axB.ax1.plot(X, test.DC_RR.DC_readout / X / Y**2, color = 'red')
axB.ax1.plot(X, test.AC_R.AC_sensitivity / X, color = 'green')
axB.ax1.plot(X, test.AC_R.AC_sensitivity, color = 'purple')
#axB.ax0.plot(X, test.DC_R2.DC_readout, color = 'blue')
#axB.ax0.plot(X, test.DC_R1.DC_readout + test.DC_R2.DC_readout, color = 'black')
axB.ax0.set_ylim(0, 1.1)
axB.ax1.axhline(np.pi/4)
```

```python
axB = mplfigB(Nrows=2)
test = sys.test
X = sys.test.aoms.VCO_AOM1.generate.amplitude
Y = sys.test.aoms.VCO_AOM2.generate.amplitude
axB.ax0.plot(X, test.DC_R.DC_readout, color = 'red')
def plot_sensitivity(ax, X, obj):
    axB.ax1.plot(X, obj.AC_sensitivity, label = str(obj))
plot_sensitivity(axB.ax1, X, test.AC_R_Q_phase)
plot_sensitivity(axB.ax1, X, test.AC_R_I_phase)
plot_sensitivity(axB.ax1, X, test.AC_R_Q_amp)
plot_sensitivity(axB.ax1, X, test.AC_R_I_amp)
axB.ax1.plot(X, np.pi/2 * X*sin(X * np.pi) / sin(X * np.pi/2)**2 / 4)
axB.ax1.legend(fontsize = 6)
#axB.ax0.plot(X, test.DC_R2.DC_readout, color = 'blue')
#axB.ax0.plot(X, test.DC_R1.DC_readout + test.DC_R2.DC_readout, color = 'black')
axB.ax0.set_ylim(0, 1.1)
#axB.ax1.axhline(np.pi/4)
```

```python

```