!pip install control import numpy as np import control as ct import matplotlib.pyplot as plt import matplotlib.animation as animation from IPython.display import HTML g = 9.81 class Segway: def __init__(self, model_params, simul_params, init_state): self.x0 = np.zeros(4) self.x0[0] = init_state ['rod angle' ] * np.pi / 180 self.x0[1] = init_state ['wheel angle' ] * np.pi / 180 self.x0[2] = init_state ['rod velocity' ] * np.pi / 180 self.x0[3] = init_state ['wheel velocity'] * np.pi / 180 self.m_w = model_params['wheel mass' ] self.r = model_params['wheel radius' ] self.m_r = model_params['rod mass' ] self.d = model_params['rod length' ] self.sig_tht = model_params['imu std' ] self.sig_phid = model_params['encoder std' ] self.alp = model_params['actuator gain' ] self.T = simul_params['runtime' ] self.dt = simul_params['time interval' ] self.frames = int(self.T/self.dt) self.setpoint = 0 def run_simulation(self): self.x = self.x0 self.xhist = np.zeros((4, self.frames)) self.pwhist = np.zeros((2,self.frames)) self.vwhist = np.zeros((2,self.frames)) self.prhist = np.zeros((2,self.frames)) self.vrhist = np.zeros((2,self.frames)) self.uhist = np.zeros((1,self.frames)) self.uchist = np.zeros((1,self.frames)) self.thtmhist = np.zeros((1,self.frames)) self.phidmhist = np.zeros((1,self.frames)) self.thtfhist = np.zeros((1,self.frames)) self.phidfhist = np.zeros((1,self.frames)) self.thist = np.linspace(0, self.T, self.frames) tht_m = [self.x[0] ,self.x[0]] phid_m = [self.x[3] ,self.x[3]] tht_f = [self.x[0] ,self.x[0]] phid_f = [self.x[3] ,self.x[3]] tau_cmd = [0,0] tau = 0 self.var1_cum = 0 self.var2_cum = 0 self.var3_cum = 0 self.var4_cum = 0 self.var5_cum = 0 for i in range(self.frames): self.xhist [:,i] = self.x self.pwhist[:,i] = self._get_p_w() self.prhist[:,i] = self._get_p_r() self.vwhist[:,i] = self._get_v_w() self.vrhist[:,i] = self._get_v_r() self.uchist[:,i] = tau_cmd[1] self.uhist [:,i] = tau self.thtmhist[:,i] = tht_m[1] self.phidmhist[:,i] = phid_m[1] self.thtfhist[:,i] = tht_f[1] self.phidfhist[:,i] = phid_f[1] phid_f, tht_f = self.filter_1o(phid_f, tht_f, phid_m, tht_m) tau_cmd[1] = self.controller(self.setpoint, phid_f, tht_f) tau = self._actuator(tau_cmd, tau) self.x = self._propagate_state(f = self._segway_dynamics, x = self.x, u = tau, dt = self.dt) tht_m[0] = tht_m[1] tht_m[1] = self.x[0] + np.random.randn()*self.sig_tht phid_m[0] = phid_m[1] phid_m[1] = self.x[3] + np.random.randn()*self.sig_phid tau_cmd[0] = tau_cmd[1] def filter_1o(self, phid_f, tht_f, phid_m, tht_m): return phid_m, tht_m def controller(self, setpoint, phid_m, tht_m): return 0 def set_filter_1o(self, filter_1o): self.filter_1o = filter_1o def set_controller(self, controller): self.controller = controller def set_setpoint(self, setpoint): self.setpoint = setpoint def run_static_plot(self): self.run_simulation() self.fig_stat = plt.figure(figsize=(20,5)) self.ax_stat_phi = self.fig_stat.add_subplot(231) self.ax_stat_tht = self.fig_stat.add_subplot(232) self.ax_stat_pos = self.fig_stat.add_subplot(233) self.ax_stat_phid = self.fig_stat.add_subplot(234) self.ax_stat_thtd = self.fig_stat.add_subplot(235) self.ax_stat_posd = self.fig_stat.add_subplot(236) self.ax_stat_phi.set_title('Wheel angle') self.ax_stat_tht.set_title('Rod angle') self.ax_stat_pos.set_title('Position') self.ax_stat_phid.set_title('Wheel angular velocity') self.ax_stat_thtd.set_title('Rod angular velocity') self.ax_stat_posd.set_title('Velocity') self.ax_stat_phid.set_xlabel('time [s]') self.ax_stat_thtd.set_xlabel('time [s]') self.ax_stat_posd.set_xlabel('time [s]') self.ax_stat_phi.set_ylabel('angle [deg]') self.ax_stat_tht.set_ylabel('angle [deg]') self.ax_stat_pos.set_ylabel('position [m]') self.ax_stat_phid.set_ylabel('angular velocity [deg/s]') self.ax_stat_thtd.set_ylabel('angular velocity [deg/s]') self.ax_stat_posd.set_ylabel('velocity [m/s]') self.ax_stat_phi.set_xlim(0,self.T) self.ax_stat_tht.set_xlim(0,self.T) self.ax_stat_pos.set_xlim(0,self.T) self.ax_stat_phid.set_xlim(0,self.T) self.ax_stat_thtd.set_xlim(0,self.T) self.ax_stat_posd.set_xlim(0,self.T) self.ax_stat_phi.set_ylim(-180,180) self.ax_stat_tht.set_ylim(-180,180) self.ax_stat_pos.set_ylim(-10,30) self.ax_stat_phid.set_ylim(-180,180) self.ax_stat_thtd.set_ylim(-180,180) self.ax_stat_posd.set_ylim(-6,6) self.ax_stat_phi.grid(True) self.ax_stat_tht.grid(True) self.ax_stat_pos.grid(True) self.ax_stat_phid.grid(True) self.ax_stat_thtd.grid(True) self.ax_stat_posd.grid(True) self.ax_stat_phi.set_xticklabels([]) self.ax_stat_tht.set_xticklabels([]) self.ax_stat_pos.set_xticklabels([]) self.ax_stat_phi.set_yticks(np.arange(-180,181,90)) self.ax_stat_tht.set_yticks(np.arange(-180,181,90)) self.ax_stat_pos.set_yticks(np.arange(-10,31,5)) self.ax_stat_phid.set_yticks(np.arange(-180,181,90)) self.ax_stat_thtd.set_yticks(np.arange(-180,181,90)) self.ax_stat_posd.set_yticks(np.arange(-10,11,5)) self.ax_stat_tht.plot(self.thist, self.xhist[0]*180/np.pi, color="black", linestyle="-", label="True" ) self.ax_stat_tht.plot(self.thist, self.thtmhist[0]*180/np.pi, color="black", linestyle="--", label="Measured") # self.ax_stat_tht.plot(self.thist, self.thtfhist[0]*180/np.pi, color="red", linestyle="-", label="Filtered") self.ax_stat_phi.plot(self.thist, self.xhist[1]*180/np.pi, color="black", linestyle="-", label="True" ) self.ax_stat_pos.plot(self.thist, self.pwhist[0], color="black", linestyle="-", label="True" ) self.ax_stat_thtd.plot(self.thist, self.xhist[2]*180/np.pi, color="black", linestyle="-", label="True" ) self.ax_stat_phid.plot(self.thist, self.xhist[3]*180/np.pi, color="black", linestyle="-", label="True" ) self.ax_stat_phid.plot(self.thist, self.phidmhist[0]*180/np.pi, color="black", linestyle="--", label="Measured") # self.ax_stat_phid.plot(self.thist, self.phidfhist[0]*180/np.pi, color="red", linestyle="-", label="Filtered") self.ax_stat_posd.plot(self.thist, self.vwhist[0], color="black", linestyle="-", label="True" ) self.ax_stat_phi.legend(loc="upper right") self.ax_stat_tht.legend(loc="upper right") self.ax_stat_pos.legend(loc="upper right") self.ax_stat_phid.legend(loc="upper right") self.ax_stat_thtd.legend(loc="upper right") self.ax_stat_posd.legend(loc="upper right") def run_dynamic_plot(self): self.run_simulation() self.fig_dyn, self.ax_dyn = plt.subplots(figsize=(18, 18/8)) self.ax_dyn.set_xlim(-10, 30) self.ax_dyn.set_ylim(-1, 4) self.ax_dyn.set_box_aspect(5/40) self.ax_dyn.grid(False) self.ax_dyn.set_yticks([]) self.rod, = self.ax_dyn.plot([], [], 'o-', lw=3, color="black") self.wheel, = self.ax_dyn.plot([], [], '-', lw=1, color="black") self.wheel_marker = self.ax_dyn.scatter([], [], s=50, color="black") self._wheel = np.array([[self.r*np.cos(ang), self.r*np.sin(ang)] for ang in np.linspace(0, 2*np.pi, 50)]) self.time_template = 'time = %.1fs' self.time_text = self.ax_dyn.text(0.05, 0.9, '', transform=self.ax_dyn.transAxes) anim = animation.FuncAnimation\ (\ fig = self.fig_dyn, func = self._animate, init_func = self._plot_init, frames = self.frames, interval = self.dt*1000, blit =True\ ) plt.close() return anim def _plot_init(self): self.rod.set_data([], []) self.wheel.set_data([], []) self.time_text.set_text('') return self.rod, self.wheel, self.time_text def _animate(self, i): x = self.xhist[:, i] p_w, p_r = self.pwhist[:, i], self.prhist[:, i] self.rod.set_data([p_w[0], p_r[0]], [p_w[1], p_r[1]]) self.wheel.set_data(p_w[0]+self._wheel[:, 0], p_w[1]+self._wheel[:, 1]) self.wheel_marker.set_offsets(np.c_[0.7*self.r*np.sin(x[1])+p_w[0], 0.7*self.r*np.cos(x[1])+p_w[1]]) self.time_text.set_text(self.time_template % (i*self.dt)) return self.rod, self.wheel, self.time_text def _segway_dynamics(self, x, u): tht = x[0] phi = x[1] tht_dot = x[2] phi_dot = x[3] tau = u x_dot = np.zeros(4) # your code here ----------------------------------------------------------# # Note. you can access class global parameters with self.~ # -------------------------------------------------------------------------# return x_dot def _propagate_state(self, f, x, u, dt): # your code here ----------------------------------------------------------# # Note. "f" is a function you implemented above( f(x,u) = self._segway_dynamics(x,u) ). # -------------------------------------------------------------------------# return x_propagated def _actuator(self, u_cmd, u_p): u_cmd_p = u_cmd[0] u_cmd_c = u_cmd[1] a0 = self.alp a1 = self.alp b0 = 2/self.dt + self.alp b1 =-2/self.dt + self.alp u_c = a0/b0 * u_cmd_c + a1/b0 * u_cmd_p - b1/b0 * u_p return u_c def _get_p_w(self): phi = self.x[1] return np.array([self.r*phi, 0]) def _get_p_r(self): tht = self.x[0] phi = self.x[1] return np.array([self.r*phi+self.d*np.sin(tht), self.d*np.cos(tht)]) def _get_v_w(self): phi_dot = self.x[3] return np.array([self.r*phi_dot, 0]) def _get_v_r(self): tht = self.x[0] tht_dot = self.x[2] phi_dot = self.x[3] return np.array([self.r*phi_dot+0.5*self.d*np.sin(tht)*tht_dot, -0.5*self.d*np.cos(tht)*tht_dot]) model_params = \ { # unit 'wheel mass' : 1.0, # [m] 'wheel radius' : 1.0, # [m] 'rod mass' : 1.0, # [m] 'rod length' : 3.0, # [m] 'imu std' : 0.0, # [rad] 'encoder std' : 0.0, # [rad/s] 'actuator gain' : 100 # [Hz] } simul_params = \ { 'runtime' : 10.0, # [s] 'time interval' : 0.05 # [s] } init_state = \ { 'wheel angle' : 0.0, # [deg] 'rod angle' : 45.0, # [deg] 'wheel velocity': 0.0, # [deg/s] 'rod velocity' : 0.0 # [deg/s] } segway = Segway(model_params, simul_params, init_state) # segway.run_static_plot() # this shows plots of the state over the time HTML(segway.run_dynamic_plot().to_jshtml()) # this shows an animation of the segway. Pretty long time required for rendering ... ( Not recommended for debugging... ) # @title m_1 = 1/2 * segway.m_r * segway.r * segway.d m_2 = 3/2 * segway.m_w * segway.r**2 + segway.m_r * segway.r**2 m_3 = 1/3 * segway.m_r * segway.d**2 m_4 = 1/2 * segway.m_r * segway.r * segway.d b = 1/2 * segway.m_r * segway.d * g alp = 100 det_M = m_1*m_4 - m_2*m_3 # Note. you can adjust the gain and Kp/Ki for better pole placement Kp_frac_Kd = 10 gain = 500 # your code here --------------------------------------------------------------# #------------------------------------------------------------------------------# G = ct.tf(num, den) fig = plt.figure(figsize=(5,5)) ax = fig.add_subplot(111) ct.rlocus(G, ax=ax, title=r"Root locus for $H(s)=G_\tau(s)G_\theta(s)G_c(s)$", color="k") ax.set_title(f"PD controlled system ( Kp/kd = {Kp_frac_Kd}, gain = {gain} )") ax.set_xlabel(r"Re$(\omega)$") ax.set_ylabel(r"Im$(\omega)$") ax.set_xticks(np.arange(-200, 201, 50)) ax.set_yticks(np.arange(-200, 201, 50)) ax.set_xlim([-150, 50]) ax.set_ylim([-100, 100]) ax.hlines(0, -150, 50, color="k", linewidth=0.5) ax.vlines(0, -100, 100, color="k", linewidth=0.5) ax.set_box_aspect(1) poles, _ = ct.rlocus(G, gains=gain, plot=False) ax.scatter(poles.real, poles.imag, color="r", marker="x", zorder=10, label="sd") ax.legend(["poles","zeros"]) plt.show() def tht_controller(setpoint, phid_m, tht_m): phid_m_p = phid_m[0] # previous measurement of wheel angular velocity (phi dot) phid_m_c = phid_m[1] # current measurement of wheel angular velocity (phi dot) tht_m_p = tht_m[0] # previous measurement of rod angle (theta) tht_m_c = tht_m[1] # current measurement of rod angle (theta) tht_cmd = setpoint # control setpoint : rod angle (theta) # your code here ------------------------------------------------------------# # Note. you can use a global cumulative variable (segway.var1_cum) if you need # ( is initialized to 0 ) # Note. if you need to use parameters of segway class, you can access it by follows # ( ex) segway.dt ) #----------------------------------------------------------------------------# return u segway.set_controller(tht_controller) # update the controller segway.set_setpoint(np.deg2rad(0)) # update the control setpoint ( tht_cmd = 0 deg ) segway.run_static_plot() anim = segway.run_dynamic_plot() HTML(anim.to_jshtml()) # your code here --------------------------------------------------------------# # Note. you can use m_1,...m_4, det_M defined above # set C and D to eye(3) and zeros((4,1)), respectively # -----------------------------------------------------------------------------# sys = ct.ss(A,B,C,D) K, S, E = ct.lqr(sys, Q, R) print("Optimal gain : ", K) def v_controller(setpoint, phid_m, tht_m): phid_m_p = phid_m[0] # previous measurement of wheel angular velocity (phi dot) phid_m_c = phid_m[1] # current measurement of wheel angular velocity (phi dot) tht_m_p = tht_m[0] # previous measurement of rod angle (theta) tht_m_c = tht_m[1] # current measurement of rod angle (theta) v_cmd = setpoint # control setpoint : velocity phid_cmd = v_cmd / segway.r # your code here ------------------------------------------------------------# # Note. you can use a global cumulative variable (segway.var2_cum) if you need # ( is initialized to 0 ) # Note. if you need to use parameters of segway class, you can access it by follows # ( ex) segway.dt ) #----------------------------------------------------------------------------# return u segway.set_controller(v_controller) # update the controller segway.set_setpoint(3) # update the control setpoint ( v_cmd = 3 m/s ) segway.run_static_plot() anim = segway.run_dynamic_plot() HTML(anim.to_jshtml()) segway.sig_tht = 0.3 segway.sig_phid = 0.3 segway.set_controller(tht_controller) # update the controller segway.set_setpoint(np.deg2rad(0)) # update the control setpoint ( tht_cmd = 0 deg ) def filter_1o(phid_f, tht_f, phid_m, tht_m): return phid_m, tht_m segway.set_filter_1o(filter_1o) segway.run_static_plot() def filter_1o(phid_f, tht_f, phid_m, tht_m): phid_m_p = phid_m[0] # previous measurement of wheel angular velocity (phi dot) phid_m_c = phid_m[1] # current measurement of wheel angular velocity (phi dot) tht_m_p = tht_m[0] # previous measurement of rod angle (theta) tht_m_c = tht_m[1] # current measurement of rod angle (theta) phid_f_p = phid_f[0] # previous filtered measurement of wheel angular velocity (phi dot) phid_f_c = phid_f[1] # current filtered measurement of wheel angular velocity (phi dot) tht_f_p = tht_f[0] # previous filtered measurement of rod angle (theta) tht_f_c = tht_f[1] # current filtered measurement of rod angle (theta) # your code here -------------------------------------------------------------# # Note. you can use a global cumulative variable (segway.var3_cum) if you need # ( is initialized to 0 ) # Note. if you need to use parameters of segway class, you can access it by follows # ( ex) segway.dt ) #-----------------------------------------------------------------------------# phid_f[0] = phid_f_c phid_f[1] = phid_filtered tht_f[0] = tht_f_c tht_f[1] = tht_filtered return phid_f, tht_f segway.set_filter_1o(filter_1o) segway.run_static_plot()