Segway Dynamics Analysis and Controller Design

In this notebook the dynamics of the segway system will be explored, and the equations of motion will be developed. Then the equations of motion will be simplified and linearized, and then the state space representation will be generated for use in the controls analysis.

The method to analyze the dynamics of the system is the Newton-Euler method. The forces are analyzed by seperating the segway into two bodies, the wheels and the pendulum body. It is assumed that the motion is two dimensional and that the wheels do not slip.

Import Required Python Libraries



In [1]:

import sympy #symbolic algebra library
import sympy.physics.mechanics as mech
import control #control analysis library
sympy.init_printing(use_latex='mathjax')
from IPython.display import display
%matplotlib inline
import px4_logutil
import pylab as pl



Define BKE Function



In [2]:

def bke(vector, frame_i, frame_b, t):
return (vector.diff(t, frame_b) + frame_b.ang_vel_in(frame_i).cross(vector))



Define Symbolic Variables



In [3]:

T, r, m_w, m_p, l, F, x, g, alpha, theta, t, R_x, R_z, N, J_p, J_w, v_x, omega, k_emf, b_damp, V, J_motor, a = \
sympy.symbols('T r m_w m_p l F x g alpha theta t R_x R_z N J_p J_w v_x omega k_emf b_damp V J_motor a')



Dynamics Analysis

Define Reference Frames



In [4]:

frame_i = mech.ReferenceFrame('i') #inertial frame
frame_b = frame_i.orientnew('b', 'Axis', [theta(t), frame_i.y]) #fixed in pendulum
frame_w = frame_b.orientnew('w', 'Axis', [-alpha(t), frame_i.y]) #fixed in wheel



Define Points of Interest



In [5]:

point_o = mech.Point('o')
point_o.set_vel(frame_i, 0) #point o is inertially fixed

point_W = point_o.locatenew('W', frame_i.x*x(t))  #wheel c.m.
point_W.set_vel(frame_b, 0) #point W is fixed in pendulum frame, too
point_W.set_vel(frame_i, point_W.pos_from(point_o).diff(t, frame_i))

point_P = point_W.locatenew('P', frame_b.z*(-l)) #pendulum c.m.
point_P.set_vel(frame_b, 0)
point_P.v2pt_theory(point_W, frame_i, frame_b);



Creation of the Bodies



In [6]:

# Wheel Creation
J_wheel = mech.inertia(frame_w, 0, J_w, 0)
wheel = mech.RigidBody('wheel', point_W, frame_w, m_w, (J_wheel, point_W))

# Pendulum Creation
J_pend = mech.inertia(frame_b, 0, J_p, 0)
pend = mech.RigidBody('pend', point_P, frame_b, m_p, (J_pend, point_P)) #change inertia point to point_p



Pendulum Dynamics



In [7]:

# Pendulum F=ma equation of motion
eom_pend_Newt = bke(pend.linear_momentum(frame_i), frame_i, frame_b, t) \
- (R_x(t)*frame_i.x) \
- (-R_z(t)*frame_i.z + m_p*g*frame_i.z)
eom_pend_Newt = eom_pend_Newt.simplify()

#Pendulum Euler's Law
eom_pend_Euler = bke(pend.angular_momentum(point_P, frame_i), frame_i, frame_b, t) \
- R_x(t)*sympy.cos(theta(t))*(l)*frame_b.y \
- R_z(t)*sympy.sin(theta(t))*(l)*frame_b.y - (T(t)*frame_b.y)




In [8]:

eom_pend_Newt




Out[8]:

$$(m_{p} \ddot{x} - R_{x})\mathbf{\hat{i}_x} + (- g m_{p} + R_{z})\mathbf{\hat{i}_z} - l m_{p} \ddot{\theta}\mathbf{\hat{b}_x} + l m_{p} \left(\dot{\theta}\right)^{2}\mathbf{\hat{b}_z}$$



Below is the equation describing the rotational motion of the pendulum body. Note that the angular momentum and torques are about the body's center of mass. It is also important to note that $J_p$ is about the center of mass of the pendulum.



In [10]:

eom_pend_Euler




Out[10]:

$$(J_{p} \ddot{\theta} - l R_{x} \operatorname{cos}\left(\theta\right) - l R_{z} \operatorname{sin}\left(\theta\right) - T)\mathbf{\hat{b}_y}$$



Wheel Dynamics



In [11]:

# Wheel F=ma equation of motion, with reaction force at pin included
eom_wheel_Newt = wheel.linear_momentum(frame_i).diff(t, frame_i) \
- (F(t)*frame_i.x) - (-R_x(t)*frame_i.x) \
- R_z(t)*frame_i.z - (-N(t)*frame_i.z) - m_w*g*frame_i.z

#Wheel Euler's Law
eom_wheel_Euler = bke(wheel.angular_momentum(point_W, frame_i), frame_i, frame_w, t) \
- (-T(t)*frame_w.y) - (F(t)*r*frame_w.y)




In [12]:

eom_wheel_Newt




Out[12]:

$$(m_{w} \ddot{x} - F + R_{x})\mathbf{\hat{i}_x} + (- g m_{w} + N - R_{z})\mathbf{\hat{i}_z}$$




In [13]:

eom_wheel_Euler




Out[13]:

$$(J_{w} \left(- \ddot{\alpha} + \ddot{\theta}\right) - r F + T)\mathbf{\hat{w}_y}$$



Motor Dynamics



In [14]:

eom_motor = T(t) + k_emf*V(t) - b_damp*alpha(t).diff(t) - J_motor*alpha(t).diff(t,2)
eom_motor




Out[14]:

$$- J_{motor} \frac{d^{2}}{d t^{2}} \alpha{\left (t \right )} - b_{damp} \frac{d}{d t} \alpha{\left (t \right )} + k_{emf} V{\left (t \right )} + T{\left (t \right )}$$



Constraints on the System



In [15]:

no_slip = r*(alpha(t) - theta(t)) - x(t) #no slip of the wheels
no_slip




Out[15]:

$$r \left(\alpha{\left (t \right )} - \theta{\left (t \right )}\right) - x{\left (t \right )}$$



Gather All EOM's and Solve the System

Dynamics of each component of the body were analyzed. There are two equations of motion for the wheel and the pendulum, one equation for the motor, and three equations from the contraint equation (the eq. and its 1st and 2nd derivatives). Furthermore, the equations for the linear dynamics of the wheel and pendulum are vector equations, each consisting of two scalar equations (one in the x direction and one in the z direction). This yields a total of 10 equations. With 10 equations, 10 unknown variables can be solved for. This is done below.



In [16]:

eoms = sympy.Matrix([eom_pend_Newt.dot(frame_i.x), eom_wheel_Newt.dot(frame_i.x), #in the x direction
eom_pend_Euler.dot(frame_i.y), eom_wheel_Euler.dot(frame_i.y), #in the y direction (moments)
eom_pend_Newt.dot(frame_i.z), eom_wheel_Newt.dot(frame_i.z), #in the z direction
eom_motor, #the equation of motion for the motor
no_slip,
no_slip.diff(t),
no_slip.diff(t,2)]) #the constraint equation
eoms #display all 10 eoms




Out[16]:

$$\left[\begin{matrix}l m_{p} \sin{\left (\theta{\left (t \right )} \right )} \left(\frac{d}{d t} \theta{\left (t \right )}\right)^{2} - l m_{p} \cos{\left (\theta{\left (t \right )} \right )} \frac{d^{2}}{d t^{2}} \theta{\left (t \right )} + m_{p} \frac{d^{2}}{d t^{2}} x{\left (t \right )} - \operatorname{R_{x}}{\left (t \right )}\\m_{w} \frac{d^{2}}{d t^{2}} x{\left (t \right )} - F{\left (t \right )} + \operatorname{R_{x}}{\left (t \right )}\\J_{p} \frac{d^{2}}{d t^{2}} \theta{\left (t \right )} - l \operatorname{R_{x}}{\left (t \right )} \cos{\left (\theta{\left (t \right )} \right )} - l \operatorname{R_{z}}{\left (t \right )} \sin{\left (\theta{\left (t \right )} \right )} - T{\left (t \right )}\\J_{w} \left(- \frac{d^{2}}{d t^{2}} \alpha{\left (t \right )} + \frac{d^{2}}{d t^{2}} \theta{\left (t \right )}\right) - r F{\left (t \right )} + T{\left (t \right )}\\- g m_{p} + l m_{p} \sin{\left (\theta{\left (t \right )} \right )} \frac{d^{2}}{d t^{2}} \theta{\left (t \right )} + l m_{p} \cos{\left (\theta{\left (t \right )} \right )} \left(\frac{d}{d t} \theta{\left (t \right )}\right)^{2} + \operatorname{R_{z}}{\left (t \right )}\\- g m_{w} + N{\left (t \right )} - \operatorname{R_{z}}{\left (t \right )}\\- J_{motor} \frac{d^{2}}{d t^{2}} \alpha{\left (t \right )} - b_{damp} \frac{d}{d t} \alpha{\left (t \right )} + k_{emf} V{\left (t \right )} + T{\left (t \right )}\\r \left(\alpha{\left (t \right )} - \theta{\left (t \right )}\right) - x{\left (t \right )}\\r \left(\frac{d}{d t} \alpha{\left (t \right )} - \frac{d}{d t} \theta{\left (t \right )}\right) - \frac{d}{d t} x{\left (t \right )}\\r \left(\frac{d^{2}}{d t^{2}} \alpha{\left (t \right )} - \frac{d^{2}}{d t^{2}} \theta{\left (t \right )}\right) - \frac{d^{2}}{d t^{2}} x{\left (t \right )}\end{matrix}\right]$$



Below the system of equations is solved for the following variables:

$T(t)$ (torque about axle)

$N(t)$ (normal force from the ground on the wheel)

$R_x(t)$ (x direction reaction force from axle)

$R_z(t)$ (z direction reaction force from axle)

$F(t)$ (friction force from wheel on ground)

$\ddot\theta(t)$ (body pitch angular acceleration)

$\ddot\alpha(t)$ (wheel/motor angular acceleration)

$\dot\alpha(t)$ (wheel/motor angular velocity)

$\alpha(t)$ (wheel/motor angle)

$\ddot{x}(t)$ (linear acceleration)

These variables are solved for because they should not be in the final equations of motion. By solving for them, they can then be substituted so that the resulting equations are only in terms of variables that can be measured.



In [17]:

eom_sol = sympy.solve(eoms, [T(t), N(t), R_x(t), R_z(t), F(t), theta(t).diff(t,2), alpha(t).diff(t,2), \
x(t).diff(t,2), alpha(t).diff(t), alpha(t)], simplify=False)
#eom_sol



Now we have solved for each of the unknown variables in terms of system properties and theta, the next step is to look specifically at the variables of most importance, x_dot and theta_dot



In [21]:

#simp_assump = {J_motor:0, J_w:0, V(t):0, x(t).diff(t):0, theta(t).diff(t):a, theta(t):0, a:theta(t).diff(t)}
simp_assump = {J_motor:0, J_w:0, V(x):0, b_damp:0}
theta_ddot = eom_sol[theta(t).diff(t,2)].expand().ratsimp().collect([theta(t), x(t), V(t), theta(t).diff(t), x(t).diff(t)], sympy.factor)
theta_ddot = theta_ddot.subs(simp_assump)
theta_ddot = theta_ddot.simplify()
theta_ddot




Out[21]:

$$\frac{1}{r \left(J_{p} m_{p} + J_{p} m_{w} + l^{2} m_{p}^{2} \sin^{2}{\left (\theta{\left (t \right )} \right )} + l^{2} m_{p} m_{w}\right)} \left(g l m_{p} r \left(m_{p} + m_{w}\right) \sin{\left (\theta{\left (t \right )} \right )} - k_{emf} \left(l m_{p} \cos{\left (\theta{\left (t \right )} \right )} + m_{p} r + m_{w} r\right) V{\left (t \right )} - \frac{l^{2} r}{2} m_{p}^{2} \sin{\left (2 \theta{\left (t \right )} \right )} \left(\frac{d}{d t} \theta{\left (t \right )}\right)^{2}\right)$$




In [25]:

x_ddot = eom_sol[x(t).diff(t,2)].expand().ratsimp().collect([theta(t), x(t), V(t), theta(t).diff(t), x(t).diff(t)], sympy.factor)
x_ddot = x_ddot.subs(simp_assump)
x_ddot




Out[25]:

$$\frac{1}{J_{p} m_{p} r^{2} + l^{2} m_{p}^{2} r^{2} \sin^{2}{\left (\theta{\left (t \right )} \right )}} \left(g l^{2} m_{p}^{2} r^{2} \sin{\left (\theta{\left (t \right )} \right )} \cos{\left (\theta{\left (t \right )} \right )} - k_{emf} r \left(J_{p} + l^{2} m_{p} \sin^{2}{\left (\theta{\left (t \right )} \right )} + l^{2} m_{p} \cos^{2}{\left (\theta{\left (t \right )} \right )} + l m_{p} r \cos{\left (\theta{\left (t \right )} \right )}\right) V{\left (t \right )} - l m_{p} r^{2} \left(J_{p} + l^{2} m_{p} \sin^{2}{\left (\theta{\left (t \right )} \right )} + l^{2} m_{p} \cos^{2}{\left (\theta{\left (t \right )} \right )}\right) \sin{\left (\theta{\left (t \right )} \right )} \left(\frac{d}{d t} \theta{\left (t \right )}\right)^{2}\right)$$



Next to make things look a little nicer, create a dict that can replace f(t) with a new variable that doesnt have the (t)



In [20]:

remove_t = {x(t).diff(t): v_x, x(t): x, theta(t).diff(t): omega, theta(t): theta, alpha(t): alpha, V(t): V, T(t): T} #defines the dict



Create state vector X, input vector U, derivative of state vector Xdot



In [21]:

X = sympy.Matrix([x(t), x(t).diff(t), theta(t), theta(t).diff(t)]).subs(remove_t) #state vector
U = sympy.Matrix([V(t)]).subs(remove_t) #Input torque
Xdot = sympy.Matrix([x(t).diff(t), x_ddot, theta(t).diff(t), theta_ddot]).subs(remove_t)
X, U




Out[21]:

$$\left ( \left[\begin{matrix}x\\v_{x}\\\theta\\\omega\end{matrix}\right], \quad \left[\begin{matrix}V\end{matrix}\right]\right )$$



Define the state space matricies A, B, C, and D. They are defined by the Jacobians. A Jacobian is a matrix consisting of partial derivatives of each function and all independent variables



In [22]:

A = Xdot.jacobian(X)
B = Xdot.jacobian(U)
C = X.jacobian(X)
D = X.jacobian(U)
ss = [A, B, C, D]



These matricies still are in terms of trig functions and angular rates squared and are thus non-linear. However, we can assume the system behaves linearly about the operating point, and so evaluate the elements of the state space matricies at the operating point.



In [23]:

stop_eq_point = {T: 0, omega: 0, theta: 0, v_x: 0} #a dict of the equilibrium points when the segway is not moving
ss0 = [A.subs(stop_eq_point), B.subs(stop_eq_point), C, D]
ss0




Out[23]:

$$\left [ \left[\begin{matrix}0 & 1 & 0 & 0\\0 & \frac{b_{damp} \left(J_{p} + l m_{p} r\right)}{J_{p} m_{p} r^{2} - l^{2} m_{p}^{2} r^{2}} & \frac{g l^{2} m_{p}^{2} r^{2}}{J_{p} m_{p} r^{2} - l^{2} m_{p}^{2} r^{2}} & \frac{b_{damp} r \left(J_{p} + l m_{p} r\right)}{J_{p} m_{p} r^{2} - l^{2} m_{p}^{2} r^{2}}\\0 & 0 & 0 & 1\\0 & \frac{b_{damp} \left(l m_{p} r + m_{p} r^{2}\right)}{r \left(J_{p} m_{p} r^{2} - l^{2} m_{p}^{2} r^{2}\right)} & \frac{g l m_{p}^{2} r^{2}}{J_{p} m_{p} r^{2} - l^{2} m_{p}^{2} r^{2}} & \frac{b_{damp} \left(l m_{p} r + m_{p} r^{2}\right)}{J_{p} m_{p} r^{2} - l^{2} m_{p}^{2} r^{2}}\end{matrix}\right], \quad \left[\begin{matrix}0\\- \frac{b_{emf} r \left(J_{p} + l m_{p} r\right)}{J_{p} m_{p} r^{2} - l^{2} m_{p}^{2} r^{2}}\\0\\- \frac{b_{emf} \left(l m_{p} r + m_{p} r^{2}\right)}{J_{p} m_{p} r^{2} - l^{2} m_{p}^{2} r^{2}}\end{matrix}\right], \quad \left[\begin{matrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right], \quad \left[\begin{matrix}0\\0\\0\\0\end{matrix}\right]\right ]$$




In [24]:

sub_const = {
J_p: 2,
b_damp: 0,
b_emf: 1,
g: 9.8,
l: 1,
r: 0.1,
m_p: 1,
}




In [25]:

import pylab as pl




In [26]:

ss0




Out[26]:

$$\left [ \left[\begin{matrix}0 & 1 & 0 & 0\\0 & \frac{b_{damp} \left(J_{p} + l m_{p} r\right)}{J_{p} m_{p} r^{2} - l^{2} m_{p}^{2} r^{2}} & \frac{g l^{2} m_{p}^{2} r^{2}}{J_{p} m_{p} r^{2} - l^{2} m_{p}^{2} r^{2}} & \frac{b_{damp} r \left(J_{p} + l m_{p} r\right)}{J_{p} m_{p} r^{2} - l^{2} m_{p}^{2} r^{2}}\\0 & 0 & 0 & 1\\0 & \frac{b_{damp} \left(l m_{p} r + m_{p} r^{2}\right)}{r \left(J_{p} m_{p} r^{2} - l^{2} m_{p}^{2} r^{2}\right)} & \frac{g l m_{p}^{2} r^{2}}{J_{p} m_{p} r^{2} - l^{2} m_{p}^{2} r^{2}} & \frac{b_{damp} \left(l m_{p} r + m_{p} r^{2}\right)}{J_{p} m_{p} r^{2} - l^{2} m_{p}^{2} r^{2}}\end{matrix}\right], \quad \left[\begin{matrix}0\\- \frac{b_{emf} r \left(J_{p} + l m_{p} r\right)}{J_{p} m_{p} r^{2} - l^{2} m_{p}^{2} r^{2}}\\0\\- \frac{b_{emf} \left(l m_{p} r + m_{p} r^{2}\right)}{J_{p} m_{p} r^{2} - l^{2} m_{p}^{2} r^{2}}\end{matrix}\right], \quad \left[\begin{matrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right], \quad \left[\begin{matrix}0\\0\\0\\0\end{matrix}\right]\right ]$$




In [27]:

sys0 = control.ss(*[pl.array(mat_i.subs(sub_const)).astype(float) for mat_i in ss0])
sys0




Out[27]:

A = [[ 0.   1.   0.   0. ]
[ 0.   0.   9.8  0. ]
[ 0.   0.   0.   1. ]
[ 0.   0.   9.8  0. ]]

B = [[  0.]
[-21.]
[  0.]
[-11.]]

C = [[ 1.  0.  0.  0.]
[ 0.  1.  0.  0.]
[ 0.  0.  1.  0.]
[ 0.  0.  0.  1.]]

D = [[ 0.]
[ 0.]
[ 0.]
[ 0.]]




In [28]:

def tf_clean(tf, tol=1e-3):
import copy
num = copy.deepcopy(tf.num)
den = copy.deepcopy(tf.den)
for i_u in range(tf.inputs):
for i_y in range(tf.outputs):
num[i_y][i_u] = pl.where(abs(num[i_y][i_u]) < tol, pl.zeros(num[i_y][i_u].shape), num[i_y][i_u])
den[i_y][i_u] = pl.where(abs(den[i_y][i_u]) < tol, pl.zeros(den[i_y][i_u].shape), den[i_y][i_u])
return control.tf(num,den)




In [29]:

tf_20 = tf_clean(control.ss2tf(sys0[2,0]))
tf_20




Out[29]:

-11
---------
s^2 - 9.8



Figure out pole zero cancellation in python control toolbox.



In [30]:

tf_20 = control.tf([-11],[1,0,-9.8])
tf_20




Out[30]:

-11
---------
s^2 - 9.8




In [31]:

control.bode(tf_20, omega=pl.logspace(-2,4));




/usr/local/lib/python2.7/dist-packages/control/freqplot.py:124: FutureWarning: comparison to None will result in an elementwise object comparison in the future.
if (omega == None):




In [32]:

control.rlocus(tf_20);
pl.axis(20*pl.array([-1,1,-1,1]))




Out[32]:

array([-20,  20, -20,  20])




In [33]:

K, S, E = control.lqr(sys0.A, sys0.B, pl.eye(sys0.A.shape[0]), pl.eye(sys0.B.shape[1]))
K, S, E




Out[33]:

(array([[  1.        ,   2.00048462, -14.2631815 ,  -6.44414536]]),
array([[  2.00048462,   1.50096935,  -6.44414536,  -2.95639604],
[  1.50096935,   2.27934366,  -9.93501762,  -4.5333365 ],
[ -6.44414536,  -9.93501762,  45.78496013,  20.26350468],
[ -2.95639604,  -4.5333365 ,  20.26350468,   9.2403829 ]]),
array([-23.94398880+0.j        ,  -1.00657153+0.j        ,
-1.96243072+0.46371317j,  -1.96243072-0.46371317j], dtype=complex64))




In [ ]:




In [34]:

eom




---------------------------------------------------------------------------
NameError                                 Traceback (most recent call last)
<ipython-input-34-e28a34415f1f> in <module>()
----> 1 eom

NameError: name 'eom' is not defined




In [ ]:

import scipy.integrate

sim = scipy.integrate.ode(




In [ ]:

print Xdot




In [ ]:

print X
print U




In [ ]:

x_vect = sympy.DeferredVector('x')
u_vect = sympy.DeferredVector('u')
ss_sub = {X[i]:x_vect[i] for i in range(len(X))}
ss_sub.update({U[i]:u_vect[i] for i in range(len(U))})
ss_sub




In [ ]:

Xdot.subs(sub_const).subs(ss_sub)




In [ ]:

print x




In [ ]:

print Xdot.subs(sub_const)




In [ ]:

import numpy
f_eval = sympy.lambdify([t, x_vect, u_vect], Xdot.subs(sub_const).subs(ss_sub),
[{'ImmutableMatrix': numpy.array}, 'numpy'])
f_eval(0, pl.array([0,0,0,0]), pl.array([0]))




In [ ]:

sim = scipy.integrate.ode(f_eval)
x0 = [0.5,0,0.5,0]
u = 0
sim.set_initial_value(x0)
sim.set_f_params(u)
dt = 0.01
tf = 2
data = {
't': [],
'u': [],
'y': [],
'x': [],
}

while sim.t + dt < tf:
x = sim.y
y = x # fix
u = -K.dot(x)
sim.set_f_params(u)
sim.integrate(sim.t + dt)
data['t'].append(sim.t)
data['u'].append(u)
data['x'].append(x)
data['y'].append(y)




In [ ]:

h_nl = pl.plot(data['t'], data['y'], 'r-');
sysc = sys0.feedback(K)
t, y, x = control.forced_response(sysc, X0=x0, T=pl.linspace(0,tf), transpose=True)
h_l = pl.plot(t, y, 'k--');
pl.legend([h_nl[0], h_l[0]], ['non-linear', 'linear'], loc='best')
pl.grid()



System Identification - Pendulum



In [148]:

eom_pend_Euler




Out[148]:

$$(J_{p} \ddot{\theta} - l R_{x} \operatorname{cos}\left(\theta\right) - l R_{z} \operatorname{sin}\left(\theta\right) - T)\mathbf{\hat{b}_y}$$




In [173]:

pend_sysID_const = theta(t) + alpha(t)
pend_sysID_const




Out[173]:

$$\alpha{\left (t \right )} + \theta{\left (t \right )}$$




In [174]:

sol_pend_sysID = sympy.solve(
[pend_sysID_const,pend_sysID_const.diff(t),pend_sysID_const.diff(t, 2)] +
list(eom_pend_Newt.to_matrix(frame_i)) +
list(eom_pend_Euler.to_matrix(frame_i)) + [eom_motor],
[R_x(t), R_z(t), theta(t).diff(t,2), T(t), alpha(t).diff(t, 2), alpha(t).diff(t)])




In [175]:

sol_pend_sysID




Out[175]:

$$\left \{ \operatorname{R_{x}}{\left (t \right )} : \frac{m_{p}}{J_{motor} + J_{p} + l^{2} m_{p}} \left(b_{damp} l \cos{\left (\theta{\left (t \right )} \right )} \frac{d}{d t} \theta{\left (t \right )} + k_{emf} l V{\left (t \right )} \cos{\left (\theta{\left (t \right )} \right )} - l^{2} m_{p} \left(g - l \cos{\left (\theta{\left (t \right )} \right )} \left(\frac{d}{d t} \theta{\left (t \right )}\right)^{2}\right) \sin{\left (\theta{\left (t \right )} \right )} \cos{\left (\theta{\left (t \right )} \right )} + \left(l \sin{\left (\theta{\left (t \right )} \right )} \left(\frac{d}{d t} \theta{\left (t \right )}\right)^{2} + \frac{d^{2}}{d t^{2}} x{\left (t \right )}\right) \left(J_{motor} + J_{p} + l^{2} m_{p} \sin^{2}{\left (\theta{\left (t \right )} \right )}\right)\right), \quad \operatorname{R_{z}}{\left (t \right )} : \frac{m_{p}}{J_{motor} + J_{p} + l^{2} m_{p}} \left(b_{damp} l \sin{\left (\theta{\left (t \right )} \right )} \frac{d}{d t} \theta{\left (t \right )} + k_{emf} l V{\left (t \right )} \sin{\left (\theta{\left (t \right )} \right )} - l^{2} m_{p} \left(l \sin{\left (\theta{\left (t \right )} \right )} \left(\frac{d}{d t} \theta{\left (t \right )}\right)^{2} + \frac{d^{2}}{d t^{2}} x{\left (t \right )}\right) \sin{\left (\theta{\left (t \right )} \right )} \cos{\left (\theta{\left (t \right )} \right )} + \left(g - l \cos{\left (\theta{\left (t \right )} \right )} \left(\frac{d}{d t} \theta{\left (t \right )}\right)^{2}\right) \left(J_{motor} + J_{p} + l^{2} m_{p} \cos^{2}{\left (\theta{\left (t \right )} \right )}\right)\right), \quad T{\left (t \right )} : - \frac{1}{J_{motor} + J_{p} + l^{2} m_{p}} \left(J_{motor} g l m_{p} \sin{\left (\theta{\left (t \right )} \right )} + J_{motor} l m_{p} \cos{\left (\theta{\left (t \right )} \right )} \frac{d^{2}}{d t^{2}} x{\left (t \right )} + J_{p} b_{damp} \frac{d}{d t} \theta{\left (t \right )} + J_{p} k_{emf} V{\left (t \right )} + b_{damp} l^{2} m_{p} \frac{d}{d t} \theta{\left (t \right )} + k_{emf} l^{2} m_{p} V{\left (t \right )}\right), \quad \frac{d}{d t} \alpha{\left (t \right )} : - \frac{d}{d t} \theta{\left (t \right )}, \quad \frac{d^{2}}{d t^{2}} \alpha{\left (t \right )} : \frac{1}{J_{motor} + J_{p} + l^{2} m_{p}} \left(b_{damp} \frac{d}{d t} \theta{\left (t \right )} - g l m_{p} \sin{\left (\theta{\left (t \right )} \right )} + k_{emf} V{\left (t \right )} - l m_{p} \cos{\left (\theta{\left (t \right )} \right )} \frac{d^{2}}{d t^{2}} x{\left (t \right )}\right), \quad \frac{d^{2}}{d t^{2}} \theta{\left (t \right )} : \frac{1}{J_{motor} + J_{p} + l^{2} m_{p}} \left(- b_{damp} \frac{d}{d t} \theta{\left (t \right )} + g l m_{p} \sin{\left (\theta{\left (t \right )} \right )} - k_{emf} V{\left (t \right )} + l m_{p} \cos{\left (\theta{\left (t \right )} \right )} \frac{d^{2}}{d t^{2}} x{\left (t \right )}\right)\right \}$$




In [176]:

pend_sysID = (sol_pend_sysID[theta(t).diff(t,2)].subs({x(t).diff(t,2):0, V(t):0}) - theta(t).diff(t,2))
pend_sysID




Out[176]:

$$\frac{1}{J_{motor} + J_{p} + l^{2} m_{p}} \left(- b_{damp} \frac{d}{d t} \theta{\left (t \right )} + g l m_{p} \sin{\left (\theta{\left (t \right )} \right )}\right) - \frac{d^{2}}{d t^{2}} \theta{\left (t \right )}$$




In [177]:

eom_motor




Out[177]:

$$- J_{motor} \frac{d^{2}}{d t^{2}} \alpha{\left (t \right )} - b_{damp} \frac{d}{d t} \alpha{\left (t \right )} + k_{emf} V{\left (t \right )} + T{\left (t \right )}$$




In [131]:

motor_sysID = eom_motor.subs({T(t):0})
motor_sysID




Out[131]:

$$- J_{motor} \frac{d^{2}}{d t^{2}} \alpha{\left (t \right )} - b_{damp} \frac{d}{d t} \alpha{\left (t \right )} + k_{emf} V{\left (t \right )}$$




In [4]:




In [21]:




In [22]:

data = data1
def do_plotting(data):
i_start = 400
i_end = -1
t = (data.TIME.StartTime[i_start:i_end] -  data1.TIME.StartTime[i_start])/1e6
V = data.BATT.V[i_start:i_end]
#pl.plot(t, data.ENCD.cnt0[i_start:i_end])
#pl.plot(t, data.ENCD.vel0[i_start:i_end])
pl.plot(t, V*(data.OUT1.Out0[i_start:i_end] - 1500)/1500)
do_plotting(data1)







In [17]:




Out[17]:

[<matplotlib.lines.Line2D at 0x7fc04c837310>]




In [ ]: