<jupyter_start><jupyter_text>Quadrotor SimulatorIn order to easily simulate different algorithms, a simulation test bed must be created. This Python class deals with the plumbing between the controller, estimation, and the model of a quadrotor system. This notebook also provides abstract base classes for `Controller` and `Estimator`.<jupyter_code>%%capture
%run quadrotor_model.ipynb
%run utils.ipynb
%run sensors.ipynb
%matplotlib inline
import numpy as np
import matplotlib.pyplot as plt
class Controller(object):
    """Controller
    """
    def __init__(self):
        self.name = "Manual"
    
    def __str__(self):
        return self.name
    
    def update(self, commanded, state, pkt, Ts):
        return np.array([[10, 0, 0, 0]]).T, commanded
class Estimator(object):
    """Estimator
    """
    def __init__(self, body=False):
        self.name = "Truth"
        
        # should the translational velocity be expressed
        # in the body-fixed frame or the inertial frame?
        self.body = body
        
    def __str__(self):
        return self.name
    
    def get_truth(self, quad):
        """Get Truth
        
        Obtain the true state from the simulated
        quadrotor object.
        """
        state = np.zeros((12,1))
        state[0:3] = quad.r
        state[3:6] = Rot_i_to_b(*quad.Phi.flatten()).dot(quad.v) if self.body else quad.v
        state[6:9] = quad.Phi
        state[9:12] = quad.omega
        return state
    
    def update(self, quad, u, Ts):
        """Update
        
        The default estimator is to use truth.
        """
        state = self.get_truth(quad)
        return state, state
class Commander(object):
    """Commander
    
    Allows the user to create flexible vehicle commands
    for the given controller. The default commander is
    a set point at 0.
    
    This class also allows the user to command any subset
    of vehicle states.
    """
    def __init__(self, default=False):
        # which subset of states should be commanded?
        self.pos = None
        self.vel = None
        self.Phi = None
        self.omega = None
        
        #
        # Default: Command everything to zero
        #
        
        if default:
            self.position(np.array([0.0, 0.0, 0.0]))
            self.velocity(np.array([None, None, None]))
            self.attitude(np.array([0.0, 0.0, 0.0]))
            self.angular_rates(np.array([None, None, None]))
        
    def position(self, pos):
        self.pos = pos
        
    def velocity(self, vel):
        self.vel = vel
        
    def attitude(self, Phi):
        self.Phi = Phi
        
    def angular_rates(self, omega):
        self.omega = omega
        
    def get_commands(self, i, Ts):
        pos = self.pos(i, Ts) if callable(self.pos) else self.pos
        Phi = self.Phi(i, Ts) if callable(self.Phi) else self.Phi
        commanded = np.hstack((pos, self.vel, Phi, self.omega))
        return commanded
class Simulator(object):
    """Simulator
        
    This class deals with the high-level simulation plumbing for a quadrotor system
    """
    def __init__(self, quad=None, ctrl=None, estm=None, cmdr=None, sens=None):
        self.quad = quad if quad else Quadrotor()
        self.ctrl = ctrl if ctrl else Controller()
        self.estm = estm if estm else Estimator()
        self.cmdr = cmdr if cmdr else Commander(default=True)
        self.sens = sens if sens else SensorManager()
        
        # Keep a history for plotting
        self.hist = {}
        
        # Simulation parameters
        self.Tf = 0
        self.Ts = 0
        self.N = 0
        
    def __str__(self):
        s  = "Simulation"
        return s

    def run(self, Tf, Ts=0.01):
        
        try:
            if _NO_SIM == True:
                return
        except Exception as e:
            pass
        
        # save simulation parameters
        self.Tf = Tf
        self.Ts = Ts
        
        # How many iterations are needed
        self.N = int(Tf/Ts)
        
        # quadrotor state
        state = truth = np.zeros((12,1))
        truth[0:3] = self.quad.r
        truth[3:6] = self.quad.v
        truth[6:9] = self.quad.Phi
        truth[9:12] = self.quad.omega
        
        # initialize data packet
        pkt = {}
        
        # initialize the plot history
        self.hist['u'] = np.zeros((4,self.N))
        self.hist['commanded'] = np.zeros((12,self.N))
        self.hist['state'] = np.zeros((12,self.N))
        self.hist['truth'] = np.zeros((12,self.N))
        
        #
        # Main Simulation Loop
        #
        
        for i in range(self.N):
            # determine the desired command
            commanded = self.cmdr.get_commands(i, Ts)
            
            # calculate control
            u, commanded = self.ctrl.update(commanded, truth, pkt, Ts)
                      
            # actuate physical model
            usat = self.quad.update(u, Ts)
            
            # read sensors
            pkt = self.sens.get_data_packet(self.quad, i, Ts)
            
            # run estimator
            state, truth = self.estm.update(self.quad, usat, Ts)
                
            # Update history
            self.hist['u'][:, i] = usat.flatten()
            self.hist['commanded'][:, i] = commanded.flatten()
            self.hist['state'][:, i] = state.flatten()
            self.hist['truth'][:, i] = truth.flatten()
    
    def plot(self):
        """Plot
        Create plot(s) of the evolution of the quadrotor state
        over the simulation horizon.
        """
        # Make sure that there is even data to plot
        if 'state' not in self.hist:
            return
        
        plt.ioff()
        fig = plt.figure(figsize=(12,10))
        fig.subplots_adjust(wspace=0.25)
        fig.suptitle('Vehicle State', fontsize=16)
        
        tvec = np.arange(self.N)*self.Ts
        
        #
        # Position
        #
        
        # for convenience
        xpos = self.hist['truth'][0, :]
        ypos = self.hist['truth'][1, :]
        zpos = self.hist['truth'][2, :]
        
        xcmd = self.hist['commanded'][0, :]
        ycmd = self.hist['commanded'][1, :]
        zcmd = self.hist['commanded'][2, :]
        
        ax = fig.add_subplot(6,2,1)
        if not np.isnan(xcmd).any():
            ax.plot(tvec, xcmd, 'r-', label='command')
        ax.plot(tvec, xpos, 'b-', label='truth')
        ax.set_ylabel('x'); ax.grid()
        ax.legend()

        ax = fig.add_subplot(6,2,3)
        if not np.isnan(ycmd).any():
            ax.plot(tvec, ycmd, 'r-', label='command')
        ax.plot(tvec, ypos, 'b-', label='truth')
        ax.set_ylabel('y'); ax.grid()
        
        ax = fig.add_subplot(6,2,5)
        if not np.isnan(zcmd).any():
            ax.plot(tvec, zcmd, 'r-', label='command')
        ax.plot(tvec, zpos, 'b-', label='truth')
        ax.set_ylabel('z'); ax.grid()
        
        #
        # Velocity
        #
        
        # for convenience
        xvel = self.hist['truth'][3, :]
        yvel = self.hist['truth'][4, :]
        zvel = self.hist['truth'][5, :]
        
        xvelhat = self.hist['state'][3, :]
        yvelhat = self.hist['state'][4, :]
        zvelhat = self.hist['state'][5, :]
        
        xcmd = self.hist['commanded'][3, :]
        ycmd = self.hist['commanded'][4, :]
        zcmd = self.hist['commanded'][5, :]
        
        ax = fig.add_subplot(6,2,2)
        if not np.isnan(xcmd).any():
            ax.plot(tvec, xcmd, 'r-', label='command')
        ax.plot(tvec, xvel, 'b-', label='truth')
        if not np.isnan(xvelhat).any() and self.estm.name != 'Truth':
            ax.plot(tvec, xvelhat, 'k:', label='estimate', linewidth=2)
        ax.set_ylabel('vx'); ax.grid()

        ax = fig.add_subplot(6,2,4)
        if not np.isnan(ycmd).any():
            ax.plot(tvec, ycmd, 'r-', label='command')
        ax.plot(tvec, yvel, 'b-', label='truth')
        if not np.isnan(yvelhat).any() and self.estm.name != 'Truth':
            ax.plot(tvec, yvelhat, 'k:', label='estimate', linewidth=2)
        ax.set_ylabel('vy'); ax.grid()
        
        ax = fig.add_subplot(6,2,6)
        if not np.isnan(zcmd).any():
            ax.plot(tvec, zcmd, 'r-', label='command')
        ax.plot(tvec, zvel, 'b-', label='truth')
        if not np.isnan(zvelhat).any() and self.estm.name != 'Truth':
            ax.plot(tvec, zvelhat, 'k:', label='estimate', linewidth=2)
        ax.set_ylabel('vz'); ax.grid()
        
        #
        # Attitude
        #
        
        # for convenience
        ph = self.hist['truth'][6, :]
        th = self.hist['truth'][7, :]
        ps = self.hist['truth'][8, :]
        
        phcmd = self.hist['commanded'][6, :]
        thcmd = self.hist['commanded'][7, :]
        pscmd = self.hist['commanded'][8, :]
        
        ax = fig.add_subplot(6,2,7)
        if not np.isnan(phcmd).any():
            ax.plot(tvec, np.degrees(phcmd), 'r-', label='command')
        ax.plot(tvec, np.degrees(ph), 'b-', label='truth')
        ax.set_ylabel(r'$\phi$'); ax.grid()

        ax = fig.add_subplot(6,2,9)
        if not np.isnan(thcmd).any():
            ax.plot(tvec, np.degrees(thcmd), 'r-', label='command')
        ax.plot(tvec, np.degrees(th), 'b-', label='truth')
        ax.set_ylabel(r'$\theta$'); ax.grid()
        
        ax = fig.add_subplot(6,2,11)
        if not np.isnan(pscmd).any():
            ax.plot(tvec, np.degrees(pscmd), 'r-', label='command')
        ax.plot(tvec, np.degrees(ps), 'b-', label='truth')
        ax.set_ylabel(r'$\psi$'); ax.grid()
        
        #
        # Angular Rates
        #
        
        # for convenience
        p = self.hist['truth'][9, :]
        q = self.hist['truth'][10, :]
        r = self.hist['truth'][11, :]

        pcmd = self.hist['commanded'][9, :]
        qcmd = self.hist['commanded'][10, :]
        rcmd = self.hist['commanded'][11, :]
        
        ax = fig.add_subplot(6,2,8)
        if not np.isnan(pcmd).any():
            ax.plot(tvec, pcmd, 'r-', label='command')
        ax.plot(tvec, p, 'b-', label='truth')
        ax.set_ylabel('p'); ax.grid()

        ax = fig.add_subplot(6,2,10)
        if not np.isnan(qcmd).any():
            ax.plot(tvec, qcmd, 'r-', label='command')
        ax.plot(tvec, q, 'b-', label='truth')
        ax.set_ylabel('q'); ax.grid()
        
        ax = fig.add_subplot(6,2,12)
        if not np.isnan(rcmd).any():
            ax.plot(tvec, rcmd, 'r-', label='command')
        ax.plot(tvec, r, 'b-', label='truth')
        ax.set_ylabel('r'); ax.grid()
        
        plt.show()
        
        #
        # Control Effort
        #
        
        thrust = self.hist['u'][0, :]
        tau_ph = self.hist['u'][1, :]
        tau_th = self.hist['u'][2, :]
        tau_ps = self.hist['u'][3, :]
        
        fig = plt.figure(figsize=(12,3))
        fig.subplots_adjust(wspace=0.25)
        fig.suptitle('Control Effort', fontsize=16)
        
        ax = fig.add_subplot(221)
        ax.plot(tvec, thrust, 'g-')
        ax.set_ylabel('Thrust'); ax.grid()
        
        ax = fig.add_subplot(222)
        ax.plot(tvec, tau_ph, 'g-')
        ax.set_ylabel(r'$\tau_\phi$'); ax.grid()
        
        ax = fig.add_subplot(223)
        ax.plot(tvec, tau_th, 'g-')
        ax.set_ylabel(r'$\tau_\theta$'); ax.grid()
        
        ax = fig.add_subplot(224)
        ax.plot(tvec, tau_ps, 'g-')
        ax.set_ylabel(r'$\tau_\psi$'); ax.grid()
        
        plt.show()
        
sim = Simulator()
sim.run(5, Ts=0.01)
sim.plot()

print(sim.quad)<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.15079910637566593, 'fraction_numerical': 0.014177693761814745, 'mean_word_length': 1.7639515554500118, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 5, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '1', 'dir': 'jupyter-structured-clean-dedup', 'id': '128589', 'license': 'MIT', 'n_tokens_mistral': 4011, 'n_tokens_neox': 3936, 'n_words': 835, 'path': 'quadsim.ipynb', 'repo_name': 'plusk01/nonlinearquad', 'size': '11638'}