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
%load_ext autoreload
%autoreload 2
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]:

    
with open('data/segway/motor_sysid/sess001/log001.csv', 'r') as loadfile:
    data1 = px4_logutil.px4_log_to_namedtuple(loadfile)



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 [ ]: