Config



In [1]:

    
from plotly.offline import init_notebook_mode, iplot # from plotly.offline import init_notebook_mode, iplot # 
from plotly.graph_objs import Scatter, Figure, Layout
init_notebook_mode(connected=True)
from IPython.display import display, HTML



In [2]:

    
import numpy as np
from scipy.integrate import odeint

Simplest Plot Ever



In [3]:

    
x = list(range(0,10))
y = [c*c for c in x]
iplot([{"x": x, "y": y}])

Dynamics (Equations of Motion)

$m\ddot{x} + b \dot{x} + kx = u$

$s = \begin{bmatrix}x \\ \dot{x}\end{bmatrix}$



In [4]:

    
# Equations of motion
def get_x_dotdot(x, x_dot, u):
    m = 10
    b = 1*0
    k = 1
    x_dotdot = (u-k*x-b*x_dot)/m
    return x_dotdot

# ODE
def get_s_dot(y,t,u):
    x, x_dot = y
    x_dotdot = get_x_dotdot(x,x_dot, u)
    s_dot = [x_dot, x_dotdot]
    return s_dot
    
# PD controller    
def get_u(x,x_dot):
    kp = 10
    kd = 6
    u = -kp*x-kd*x_dot
    return u

Simulation

Dynamics in continous time; controller in discrete time (zero order hold)



In [5]:

    
# Initial Conditions
s0 = [3, 0]
nStates = 2

# Initialize empty result vectors
t_all = np.empty(shape=[0, 1]) 
s_all = np.empty(shape=[0, 2]) 
end = -1

# Time settings
dt    = 0.5 # Controller sampling time
t_end = 10  # Simulation time
Tdis = np.arange(0,t_end,dt) # Discrete time samples

# At every sample do
for tnow in Tdis:
    
    # Given the current state, compute a control signal
    x,x_dot = s0
    u = get_u(x,x_dot)
    
    # Integrate equation of motion until next sample while holding control
    simsteps_per_sample = 10
    t = np.linspace(tnow,tnow+dt,simsteps_per_sample+1)
    s = odeint(get_s_dot, s0, t, args=(u,))
    
    # The final y state is the initial state for next time
    s0 = s[end,:]
    
    # Append the results, except the final state. That will be appended as the initial state of the next one
    t_all = np.append(t_all,t[0:end])
    s_all = np.append(s_all,s[0:end,:], axis = 0)
    
# Match time and output signals for plotting    
pos_plot = Scatter(x = t_all, y = s_all[:, 0], name = 'position')   
vel_plot = Scatter(x = t_all, y = s_all[:, 1], name = 'speed')   
    
# Plot results inline    
iplot([pos_plot,vel_plot])

Time interpolation



In [6]:

    
time_scale = 1         # Amount of time we speed up the animation. 1x = real time
fps = 20               # Frames per second
tpf = 1/fps            # Time per frame (miliseconds)

# Create time vector of display instants
t_display = np.arange(0,t_end,tpf)

# Real time samples corresponding to display time
t_sample = t_display*time_scale

# Interpolate state data to match the display time vector
s_display = np.stack([np.interp(t_sample,t_all,s_all[:,i]) for i in range(nStates)]).T

Animation

Below we draw two line segments. Each line has one end at the origin. The other end of one line represents the position, the other line is velocity.



In [7]:

    
# Origin coordinate in time (always 0,0)
coordinate_pair_ori = [[0,0]              for i, t in enumerate(t_display)]
# Position in time (horizontal = 0, vertical = position)
coordinate_pair_pos = [[0,s_display[i,0]] for i, t in enumerate(t_display)]
# Velocity in time (horizontal = 0, vertical = velocity)
coordinate_pair_vel = [[0,s_display[i,1]] for i, t in enumerate(t_display)]

# Put data in the frames format. At each time sample this is a set of coordinates making up the line segments
framedata = [ {'data': [{'x': coordinate_pair_ori[i], 'y': coordinate_pair_pos[i]},
                        {'x': coordinate_pair_ori[i], 'y': coordinate_pair_vel[i]}
                       ]}  for i, t in enumerate(t_display)]

# We'll plot the first time instant so there's something there until we start the animation
firstframe = framedata[0]['data']

# Associate a legend with the data. 
firstframe[0]['name'] = 'Position'
firstframe[1]['name'] = 'Velocity'

# We can only specify the inter frame time. So we have to account for how long it approximately takes to draw a frame
overhead_ms = 7

# Create the figure object
figure = {'data': firstframe,
          'layout': {'xaxis': {'range': [-1, 1], 'autorange': False},
                     'yaxis': {'range': [-3, 3], 'autorange': False},
                     'title': 'Mass Spring Damper',
                     'updatemenus': [{'type': 'buttons',
                                      'buttons': 
  [                 
    {'label': 'Start','method': 'animate',
      'args': [framedata, {'frame': {'duration': tpf*1000 - overhead_ms, 'redraw': False}}]
    },       
    {'label': 'Stop','method': 'animate',
      'args': [[None], {'frame': {'duration': 0, 'redraw': False}, 'mode': 'immediate','transition': {'duration': 0}}]
    }                    
  ]
                                     }]
                    }
         }

# Create an inline plot of the figure
iplot(figure)