import numpy as np import matplotlib.pyplot as plt import control as ct import control.flatsys as fs from math import sin, cos from warnings import warn # Define some line styles for later use ebarstyle = {'elinewidth': 0.5, 'capsize': 2} xdstyle = {'color': 'k', 'linestyle': '--', 'linewidth': 0.5, 'marker': '+', 'markersize': 4} # get dynamics matrices def get_dynamics_matrices(x, params): # Get the parameter values L1 = params.get('L1', 0.5) # length of link 1 L2 = params.get('L2', 0.5) # length of link 2 m1 = params.get('m1', 1.) # mass of link 1 m2 = params.get('m2', 1.) # mass of link 2 m0 = params.get('m0', 4.) # mass of cart g = params.get('g',9.81) # gravitational acceleration l1,l2 = L1/2.,L2/2. # half lengths I1, I2 = 1/12*m1*L1**2, 1/12*m1*L2**2 # moment of inertia around center of mass x, th1, th2, xdot, th1dot, th2dot = x # Dynamics matrices c1, s1 = cos(th1), sin(th1) c2, s2 = cos(th2), sin(th2) c12,s12 = cos(th1-th2), sin(th1-th2) D = np.array([ [m0+m1+m2, (m1*l1+m2*L1)*c1, m2*l2*c2], [(m1*l1+m2*L1)*c1, m1*l1**2+m2*L1**2+I1, m2*L1*l2*c12], [m2*l2*c2, m2*L1*l2*c12, m2*l2**2+I2] ]) C = np.array([ [0, -(m1*l1+m2*L1)*s1*th1dot, -m2*l2*s2*th2dot], [0, 0, m2*L1*l2*s12*th2dot], [0, -m2*L1*l2*s12*th1dot, 0] ]) G = np.array([ [0], [-(m1*l1+m2*L1)*g*s1], [-m2*g*l2*s2] ]) H = np.array([ [1], [0], [0] ]) return D,C,G,H # dbpend dynamics def dbpend_update(t, x, u, params): # Get the parameter values L1 = params.get('L1', 0.5) # length of link 1 L2 = params.get('L2', 0.5) # length of link 2 m1 = params.get('m1', 1.) # mass of link 1 m2 = params.get('m2', 1.) # mass of link 2 m0 = params.get('m0', 4.) # mass of cart g = params.get('g',9.81) # gravitational acceleration l1,l2 = L1/2.,L2/2. # half lengths I1, I2 = 1/12*m1*L1**2, 1/12*m1*L2**2 # moment of inertia around center of mass # Get the states x, th1, th2, xdot, th1dot, th2dot = x F = u[0] # Dynamics matrices c1, s1 = cos(th1), sin(th1) c2, s2 = cos(th2), sin(th2) c12,s12 = cos(th1-th2), sin(th1-th2) D = np.array([ [m0+m1+m2, (m1*l1+m2*L1)*c1, m2*l2*c2], [(m1*l1+m2*L1)*c1, m1*l1**2+m2*L1**2+I1, m2*L1*l2*c12], [m2*l2*c2, m2*L1*l2*c12, m2*l2**2+I2] ]) C = np.array([ [0, -(m1*l1+m2*L1)*s1*th1dot, -m2*l2*s2*th2dot], [0, 0, m2*L1*l2*s12*th2dot], [0, -m2*L1*l2*s12*th1dot, 0] ]) G = np.array([ [0], [-(m1*l1+m2*L1)*g*s1], [-m2*g*l2*s2] ]) H = np.array([ [1], [0], [0] ]) # add damping c = 0.1 # Dynamics qdot = np.array([xdot,th1dot,th2dot]) qddot = np.linalg.inv(D)@(H@np.array([F])-C@qdot-G.squeeze()) xddot,th1ddot,th2ddot = qddot[0], qddot[1], qddot[2] return np.array([xdot, th1dot, th2dot, xddot + c*xdot, th1ddot + c*th1ddot, th2ddot + c*th2ddot]) # dbpend dynamics def dbpend_update_noisy(t, x, u, params): # Get the parameter values L1 = params.get('L1', 0.5) # length of link 1 L2 = params.get('L2', 0.5) # length of link 2 m1 = params.get('m1', 1.) # mass of link 1 m2 = params.get('m2', 1.) # mass of link 2 m0 = params.get('m0', 4.) # mass of cart g = params.get('g',9.81) # gravitational acceleration l1,l2 = L1/2.,L2/2. # half lengths I1, I2 = 1/12*m1*L1**2, 1/12*m1*L2**2 # moment of inertia around center of mass # Get the states x, th1, th2, xdot, th1dot, th2dot = x F,vx,vth1,vth2 = u # Dynamics matrices c1, s1 = cos(th1), sin(th1) c2, s2 = cos(th2), sin(th2) c12,s12 = cos(th1-th2), sin(th1-th2) D = np.array([ [m0+m1+m2, (m1*l1+m2*L1)*c1, m2*l2*c2], [(m1*l1+m2*L1)*c1, m1*l1**2+m2*L1**2+I1, m2*L1*l2*c12], [m2*l2*c2, m2*L1*l2*c12, m2*l2**2+I2] ]) C = np.array([ [0, -(m1*l1+m2*L1)*s1*th1dot, -m2*l2*s2*th2dot], [0, 0, m2*L1*l2*s12*th2dot], [0, -m2*L1*l2*s12*th1dot, 0] ]) G = np.array([ [0], [-(m1*l1+m2*L1)*g*s1], [-m2*g*l2*s2] ]) H = np.array([ [1], [0], [0] ]) # Dynamics qdot = np.array([xdot,th1dot,th2dot]) qddot = np.linalg.inv(D)@(H@np.array([F])-C@qdot-G.squeeze()) xddot,th1ddot,th2ddot = qddot[0], qddot[1], qddot[2] return np.array([xdot, th1dot, th2dot, xddot + vx, th1ddot + vth1, th2ddot + vth2]) # dbpend dynamics def dbpend_update_noisy_perturbed(t, x, u, params): # Get the parameter values L1 = params.get('L1', 0.5) # length of link 1 L2 = params.get('L2', 0.5) # length of link 2 m1 = params.get('m1', 1.) # mass of link 1 m2 = params.get('m2', 1.) # mass of link 2 m0 = params.get('m0', 4.) # mass of cart g = params.get('g',9.81) # gravitational acceleration l1,l2 = L1/2.,L2/2. # half lengths I1, I2 = 1/12*m1*L1**2, 1/12*m1*L2**2 # moment of inertia around center of mass # Get the states x, th1, th2, xdot, th1dot, th2dot = x F,vx,vth1,vth2 = u # Dynamics matrices c1, s1 = cos(th1), sin(th1) c2, s2 = cos(th2), sin(th2) c12,s12 = cos(th1-th2), sin(th1-th2) D = np.array([ [m0+m1+m2, (m1*l1+m2*L1)*c1, m2*l2*c2], [(m1*l1+m2*L1)*c1, m1*l1**2+m2*L1**2+I1, m2*L1*l2*c12], [m2*l2*c2, m2*L1*l2*c12, m2*l2**2+I2] ]) C = np.array([ [0, -(m1*l1+m2*L1)*s1*th1dot, -m2*l2*s2*th2dot], [0, 0, m2*L1*l2*s12*th2dot], [0, -m2*L1*l2*s12*th1dot, 0] ]) G = np.array([ [0], [-(m1*l1+m2*L1)*g*s1], [-m2*g*l2*s2] ]) H = np.array([ [1], [0], [0] ]) # Perturb dynamics matrices as a representation of model uncertainty D = D + 1.0*np.ones((3,3)) C = C + 1.0*np.ones((3,3)) # G = G + 10*np.ones((3,1)) # H = H + 10*np.ones((3,1)) # Dynamics qdot = np.array([xdot,th1dot,th2dot]) qddot = np.linalg.inv(D)@(H@np.array([F])-C@qdot-G.squeeze()) xddot,th1ddot,th2ddot = qddot[0], qddot[1], qddot[2] return np.array([xdot, th1dot, th2dot, xddot + vx, th1ddot + vth1, th2ddot + vth2]) def dbpend_output(t, x, u, params): return x dbpend = ct.NonlinearIOSystem( name='dbpend', updfcn=dbpend_update, outfcn=dbpend_output, states = ['x','th1','th2','xdot','th1dot','th2dot'], inputs = ['u'], outputs = ['x','th1','th2','xdot','th1dot','th2dot'], params = { 'L1': 0.5, # length of link 1 'L2': 0.5, # length of link 2 'm1': 1.0, # mass of link 1 'm2': 1.0, # mass of link 2 'm0': 4.0, # mass of cart 'g' : 9.81 # gravitational acceleration } ) dbpend_noisy = ct.NonlinearIOSystem( name='dbpend', updfcn=dbpend_update_noisy, outfcn=dbpend_output, states = ['x','th1','th2','xdot','th1dot','th2dot'], inputs = ['u','vx','vth1','vth2'], outputs = ['x','th1','th2','xdot','th1dot','th2dot'], params = { 'L1': 0.5, # length of link 1 'L2': 0.5, # length of link 2 'm1': 1.0, # mass of link 1 'm2': 1.0, # mass of link 2 'm0': 4.0, # mass of cart 'g' : 9.81 # gravitational acceleration } ) dbpend_noisy_perturbed = ct.NonlinearIOSystem( name='dbpend', updfcn=dbpend_update_noisy_perturbed, outfcn=dbpend_output, states = ['x','th1','th2','xdot','th1dot','th2dot'], inputs = ['u','vx','vth1','vth2'], outputs = ['x','th1','th2','xdot','th1dot','th2dot'], params = { 'L1': 0.5, # length of link 1 'L2': 0.5, # length of link 2 'm1': 1.0, # mass of link 1 'm2': 1.0, # mass of link 2 'm0': 4.0, # mass of cart 'g' : 9.81 # gravitational acceleration } ) def dbpend_A0(params): # Get the parameter values L1 = params.get('L1', 0.5) # length of link 1 L2 = params.get('L2', 0.5) # length of link 2 m1 = params.get('m1', 1.) # mass of link 1 m2 = params.get('m2', 1.) # mass of link 2 m0 = params.get('m0', 4.) # mass of cart g = params.get('g',9.81) # gravitational acceleration l1,l2 = L1/2.,L2/2. # half lengths I1, I2 = 1/12*m1*L1**2, 1/12*m1*L2**2 # moment of inertia around center of mass A21 = np.array([ [0, 3/2*(2*m1+m2)*(m1+2*m2), -3/2*m1*m2], [0, -3/(2*L1)*(4*m0+4*m1+m2)*(m1+2*m2), 9/(2*L1)*(2*m0+m1)*m2], [0, 9/(2*L2)*(2*m0+m1)*(m1+2*m2), -3/(2*L2)*(m1**2+4*m0*m1+4*m1*m2+12*m0*m2)] ]) return np.block([ [np.zeros((3,3)),np.eye(3)], [A21,np.zeros((3,3))] ]) def dbpend_B0(params): # Get the parameter values L1 = params.get('L1', 0.5) # length of link 1 L2 = params.get('L2', 0.5) # length of link 2 m1 = params.get('m1', 1.) # mass of link 1 m2 = params.get('m2', 1.) # mass of link 2 m0 = params.get('m0', 4.) # mass of cart g = params.get('g',9.81) # gravitational acceleration l1,l2 = L1/2.,L2/2. # half lengths I1, I2 = 1/12*m1*L1**2, 1/12*m1*L2**2 # moment of inertia around center of mass p = 1/(4*m0*m1+3*m0*m2+m1**2+m1*m2) B21 = np.array([ [p*(4*m1+3*m2)], [-3*p/L1*(2*m1+m2)], [3*p*m2/L2] ]) return np.block([ [np.zeros((3,1))], [B21] ]) # def dbpend_A(x,params): # D,C,G,H = get_dynamics_matrices(x, params) # return np.block([ # [np.zeros((3,3)),np.eye(3)], # [np.zeros((3,3)),-np.linalg.inv(D)@C] # ]) # def dbpend_B(x,params): # D,C,G,H = get_dynamics_matrices(x, params) # return np.block([ # [np.zeros((3,1))], # [np.linalg.inv(D)@H] # ]) # Plot the trajectory in xy coordinates def plot_results(t, x, u): # Set the size of the figure plt.figure(figsize=(10, 6)) # Top plot: x trajectory plt.subplot(2, 1, 1) plt.plot(t, x[0]) plt.xlabel('t [m]') plt.ylabel('x [m]') plt.axis('equal') # Time traces of the two angles and the input plt.subplot(2, 3, 4) plt.plot(t, x[1]) plt.xlabel('Time t [sec]') plt.ylabel(r'$\theta_1(t)$ [rad]') plt.subplot(2, 3, 5) plt.plot(t, x[2]) plt.xlabel('Time t [sec]') plt.ylabel(r'$\theta_2(t)$ [rad]') plt.subplot(2, 3, 6) plt.plot(t, u) plt.xlabel('Time t [sec]') plt.ylabel(r'$u(t)$ [N]') plt.suptitle("State and input response of inverted pendulum") plt.tight_layout() def plot_estimate(timepts,xhat,P,xd,label,color,obsv=[1,1,1,1,1,1]): plt.subplot(2, 3, 1) plt.plot(timepts, xhat[0], f'{color}-',label=label) # plt.errorbar(timepts, xhat[0], P[0, 0], fmt=f'{color}-', **ebarstyle,label=label) plt.plot(timepts, xd[0], 'k--', label='$x_d$') plt.ylabel(r"$x$ [m]") if obsv[0]==1:plt.title("Observed") else: plt.title("Unobserved") plt.subplot(2, 3, 2) plt.plot(timepts, xhat[1], f'{color}-',label=label) # plt.errorbar(timepts, xhat[1], P[1, 1], fmt=f'{color}-', **ebarstyle,label=label) plt.plot(timepts, xd[1], 'k--', label='$x_d$') plt.ylabel(r"$\theta_1$ [rad]") plt.xlabel(r"Time $t$ [s]"); if obsv[1]==1:plt.title("Observed") else: plt.title("Unobserved") plt.subplot(2, 3, 3) plt.plot(timepts, xhat[2], f'{color}-',label=label) # plt.errorbar(timepts, xhat[2], P[2, 2], fmt=f'{color}-', **ebarstyle,label=label) plt.plot(timepts, xd[2], 'k--', label='$x_d$') plt.ylabel(r"$\theta_2$ [rad]") plt.xlabel(r"Time $t$ [s]"); if obsv[2]==1:plt.title("Observed") else: plt.title("Unobserved") plt.subplot(2, 3, 4) plt.plot(timepts, xhat[3], f'{color}-',label=label) # plt.errorbar(timepts, xhat[3], P[3, 3], fmt=f'{color}-', **ebarstyle,label=label) plt.plot(timepts, xd[3], 'k--', label='$x_d$') plt.ylabel(r"$\dot x$ [rad]") plt.xlabel(r"Time $t$ [s]"); if obsv[3]==1:plt.title("Observed") else: plt.title("Unobserved") plt.subplot(2, 3, 5) plt.plot(timepts, xhat[4], f'{color}-',label=label) # plt.errorbar(timepts, xhat[4], P[4, 4], fmt=f'{color}-', **ebarstyle,label=label) plt.plot(timepts, xd[4], 'k--', label='$x_d$') plt.ylabel(r"$\dot \theta_1$ [rad]") plt.xlabel(r"Time $t$ [s]"); if obsv[4]==1:plt.title("Observed") else: plt.title("Unobserved") plt.subplot(2, 3, 6) plt.plot(timepts, xhat[5], f'{color}-',label=label) # plt.errorbar(timepts, xhat[5], P[5, 5], fmt=f'{color}-', **ebarstyle,label=label) plt.plot(timepts, xd[5], 'k--', label='$x_d$') plt.ylabel(r"$\dot \theta_2$ [rad]") plt.xlabel(r"Time $t$ [s]"); if obsv[5]==1:plt.title("Observed") else: plt.title("Unobserved")