<SYSTEM_TASK:>
Given the following text description, write Python code to implement the functionality described below step by step
<END_TASK>
<USER_TASK:>
Description:
 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
Step1:  Define BKE Function
Step2:  Define Symbolic Variables
Step3:  Dynamics Analysis
Define Reference Frames
Step4:  Define Points of Interest
Step5:  Creation of the Bodies
Step6:  Pendulum Dynamics
Step7:  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.
Step8:  Wheel Dynamics
Step9:  Motor Dynamics
Step10:  Constraints on the System
Step11:  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.
Step12:  Below the system of equations is solved for the following variables
Step13:  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
Step14:  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)
Step15:  Create state vector X, input vector U, derivative of state vector Xdot
Step16:  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
Step17:  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.
Step18:  Figure out pole zero cancellation in python control toolbox.
Step19:  System Identification - Pendulum
<ASSISTANT_TASK:>
Python Code:
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
Explanation: 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
End of explanation
def bke(vector, frame_i, frame_b, t):
    return (vector.diff(t, frame_b) + frame_b.ang_vel_in(frame_i).cross(vector))
Explanation: Define BKE Function
End of explanation
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')
Explanation: Define Symbolic Variables
End of explanation
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
Explanation: Dynamics Analysis
Define Reference Frames
End of explanation
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);
Explanation: Define Points of Interest
End of explanation
# 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
Explanation: Creation of the Bodies
End of explanation
# 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)
    
eom_pend_Newt
Explanation: Pendulum Dynamics
End of explanation
eom_pend_Euler
Explanation: 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.
End of explanation
# 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)
eom_wheel_Newt
eom_wheel_Euler
Explanation: Wheel Dynamics
End of explanation
eom_motor = T(t) + k_emf*V(t) - b_damp*alpha(t).diff(t) - J_motor*alpha(t).diff(t,2)
eom_motor
Explanation: Motor Dynamics
End of explanation
no_slip = r*(alpha(t) - theta(t)) - x(t) #no slip of the wheels
no_slip
Explanation: Constraints on the System
End of explanation
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
Explanation: 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.
End of explanation
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
Explanation: 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.
End of explanation
#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
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
Explanation: 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
End of explanation
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
Explanation: 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)
End of explanation
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
Explanation: Create state vector X, input vector U, derivative of state vector Xdot
End of explanation
A = Xdot.jacobian(X)
B = Xdot.jacobian(U)
C = X.jacobian(X)
D = X.jacobian(U)
ss = [A, B, C, D]
Explanation: 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
End of explanation
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
sub_const = {
    J_p: 2,
    b_damp: 0,
    b_emf: 1,
    g: 9.8,
    l: 1,
    r: 0.1,
    m_p: 1,
}
import pylab as pl
ss0
sys0 = control.ss(*[pl.array(mat_i.subs(sub_const)).astype(float) for mat_i in ss0])
sys0
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)
tf_20 = tf_clean(control.ss2tf(sys0[2,0]))
tf_20
Explanation: 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.
End of explanation
tf_20 = control.tf([-11],[1,0,-9.8])
tf_20
control.bode(tf_20, omega=pl.logspace(-2,4));
control.rlocus(tf_20);
pl.axis(20*pl.array([-1,1,-1,1]))
K, S, E = control.lqr(sys0.A, sys0.B, pl.eye(sys0.A.shape[0]), pl.eye(sys0.B.shape[1]))
K, S, E
eom
import scipy.integrate
sim = scipy.integrate.ode(
print Xdot
print X
print U
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
Xdot.subs(sub_const).subs(ss_sub)
print x
print Xdot.subs(sub_const)
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]))
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)
    
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()
Explanation: Figure out pole zero cancellation in python control toolbox.
End of explanation
eom_pend_Euler
pend_sysID_const = theta(t) + alpha(t)
pend_sysID_const
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)])
sol_pend_sysID
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
eom_motor
motor_sysID = eom_motor.subs({T(t):0})
motor_sysID
with open('data/segway/motor_sysid/sess001/log001.csv', 'r') as loadfile:
    data1 = px4_logutil.px4_log_to_namedtuple(loadfile)
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)
Explanation: System Identification - Pendulum
End of explanation
<END_TASK>