# Introduction

Now that we have the symbolic equations of motion we need to transform them into Python functions that can be evaluated for use in numerical integration. [Numerical integration](http://en.wikipedia.org/wiki/Numerical_methods_for_ordinary_differential_equations) is required to solve the ordinary differential initial value problem and allow us to see how the states change through time.

# Setup

Load the solutions from the previous notebooks:


```python
from solution.equations_of_motion import *
```

To setup the numerical values and integrate the equations of motion we will need some functions from NumPy for numerical arrays:


```python
from numpy import deg2rad, rad2deg, array, zeros, linspace
```

We will need a ODE numerical integration routine from SciPy:


```python
from scipy.integrate import odeint
```

We can use PyDy's ODE function generator to transform the symbolic equations into numerical functions:


```python
from pydy.codegen.ode_function_generators import generate_ode_function
```

Once again, let's display the symbolics nicely.


```python
from sympy.physics.vector import init_vprinting, vlatex
```


```python
init_vprinting(use_latex='mathjax', pretty_print=False)
```

Once we get a solution for how the state changes over time, it is nice to visualize it. The simplest way to do this is to plot the trajectories versus time. We can use the matplotlib library to do this. First enable inline plotting:


```python
%matplotlib inline
```

Import a few functions for plotting:


```python
from matplotlib.pyplot import plot, legend, xlabel, ylabel, rcParams
```

And set the default figure size to be larger:


```python
rcParams['figure.figsize'] = (14.0, 6.0)
```

# Variables

The first step is to gather all of the variables in the equations of motion into lists. We will need the constants, coordinates, speeds, and the specified inputs.

## Constants

There are twelve constants in the equations. Put them into a list.


```python
constants = [lower_leg_length,
             lower_leg_com_length,
             lower_leg_mass,
             lower_leg_inertia,
             upper_leg_length,
             upper_leg_com_length,
             upper_leg_mass,
             upper_leg_inertia,
             torso_com_length,
             torso_mass,            
             torso_inertia,
             g]
constants
             
```

## Time Varying

The coordinates and speeds make up the states and there are three time varying specified inputs to the system, the joint torques.


```python
coordinates = [theta1, theta2, theta3]
coordinates
```


```python
speeds = [omega1, omega2, omega3]
speeds
```

## Exercise

Make a list called `specified` that contains the three torque magnitude variables: $T_a$, $T_k$, and $T_h$.


```python

```


```python
%load exercise_solutions/n07_simulation_torque-magnitude.py
```

# Generate the Numerical ODE Function

Ordinary differential equation integrators, like `scipy.integrate.odeint`, require a function that numerically evaluates the right hand side of the coupled first order ordinary differential equations. We have the symbolic form of the mass matrix and the forcing vector available. The `generate_ode_function` function generates a function from the symbolic expressions that fits the form needed for `odeint`.

`odeint` is an ODE integrator based on the `lsoda` routine from ODEPACK that works well for both non-stiff and stiff ODEs. Notice that it requres the right hand side function, the initial conditions of the state, a time vector. We will also pass in extra arguments, `args`, of the right hand side function.


```python
help(odeint)
```

To create the function, simply pass in $\mathbf{M}$, $\mathbf{f}$, and the lists of variables in the system.


```python
right_hand_side = generate_ode_function(forcing_vector, coordinates,
                                        speeds, constants,
                                        mass_matrix=mass_matrix,
                                        specifieds=specified)
```

We see that the result is a function.


```python
type(right_hand_side)
```

And the doc string gives information on the type of the arguments needed to evaluate it:


```python
help(right_hand_side)
```

# Set the Initial Conditions, Parameter Values, and Time Array

We will set the intial values of the speeds to be zero and the coordinates to be offset from vertical at 2 degrees. First make an array of zeros:


```python
x0 = zeros(6)
x0
```

And then set the first three values, $\theta_{1,2,3}$, to 2 degrees:


```python
x0[:3] = deg2rad(2.0)
x0
```

The right hand side function requires numerical values of all the constants to be passed in and values for the specified joint torques. Here we will use typical values from body segment parameter measurements which were generated from the [Yeadon](http://yeadon.readthedocs.org/en/latest/) Python package (`male1.txt`). Make sure the units are all consistent!


```python
numerical_constants = array([0.611,  # lower_leg_length [m]
                             0.387,  # lower_leg_com_length [m]
                             6.769,  # lower_leg_mass [kg]
                             0.101,  # lower_leg_inertia [kg*m^2]
                             0.424,  # upper_leg_length [m]
                             0.193,  # upper_leg_com_length
                             17.01,  # upper_leg_mass [kg]
                             0.282,  # upper_leg_inertia [kg*m^2]
                             0.305,  # torso_com_length [m]
                             32.44,  # torso_mass [kg]
                             1.485,  # torso_inertia [kg*m^2]
                             9.81],  # acceleration due to gravity [m/s^2]
                            ) 
```

## Exercise

For this first simulation we will set the three joint torques equal to zero for the duration of the simulation. Created a Python variale `numerical_specified` which is a NumPy array of length three and each entry is equal to zero.


```python

```

We can use the `linspace` function to generate a time vector over 10 secs such that `odeint` returns results at 60 Hz.


```python

```


```python
%load exercise_solutions/n07_simulation_sim-setup.py
```

# Integrate the Equations of Motion

The right hand side function can now be evaluated numerically given a current value of the states, a value for time, and the numerical values for all the constants and specified values in the equations of motion:


```python
right_hand_side(x0, 0.0, numerical_specified, numerical_constants)
```

Now we can solve the initial value problem and simulate the motion. As shown above, `odeint` requires the function to integrate `right_hand_side`, the initial conditions `x0`, the time vector `t`, and the extra arguments `args`:


```python
y = odeint(right_hand_side, x0, t, args=(numerical_specified, numerical_constants))
```

The `y` variable now contains a 2D array that gives the trajectories of the states as a function of time.


```python
y.shape
```

# Plot the results

We can plot the first 3 columns of `y` versus `t` so see how the three angles change throughout time.


```python
plot(t, rad2deg(y[:, :3]))
xlabel('Time [s]')
ylabel('Angle [deg]')
legend(["${}$".format(vlatex(c)) for c in coordinates])
```

What does this graph tell us? How does the system behave?

## Exercise

Now as an exercise, plot the generalized speeds (i.e. the last three states).


```python

```


```python
%load exercise_solutions/n07_simulation_plot-speeds.py
```

937c6b3c402d26754a87d9fcbeee326f2ec52610
ipynb
Jupyter Notebook
notebooks/n07_simulation.ipynb
pydy/pydy-tutorial-human-standing
72b1d8513e339e9b10e501bd3490caa3fa997bc4
CC-BY-4.0
2015-05-19T15:24:18.000Z
2022-03-12T09:39:03.000Z
notebooks/n07_simulation.ipynb
pydy/pydy-tutorial-human-standing
72b1d8513e339e9b10e501bd3490caa3fa997bc4
CC-BY-4.0
2015-05-05T18:08:20.000Z
2022-01-28T11:12:42.000Z
notebooks/n07_simulation.ipynb
pydy/pydy-tutorial-pycon-2014
72b1d8513e339e9b10e501bd3490caa3fa997bc4
CC-BY-4.0
2015-06-16T01:50:51.000Z
2022-02-26T07:39:41.000Z
Qwen/Qwen-72B
1. YES
2. YES