LoggerControl.py

            h2, = plt.plot(log_t_ref, self.planner_xref[0, j, :], linestyle="--", marker='x', color="g", linewidth=2)
            h3, = plt.plot(np.array([k*self.dt for k in range(self.mpc_x_f.shape[0])]),
                           self.planner_xref[:, j+6, 0], linestyle=None, marker='x', color="r", linewidth=1)
            axs_vel.append(ax)
            h1s_vel.append(h1)
            h2s_vel.append(h2)

        #axcolor = 'lightgoldenrodyellow'
        #ax.margins(x=0)

        # Make a horizontal slider to control the time.
        axtime_vel = plt.axes([0.25, 0.03, 0.65, 0.03], facecolor=axcolor)
        time_slider_vel = Slider(
            ax=axtime_vel,
            label='Time [s]',
            valmin=0.0,
            valmax=self.logSize*self.dt,
            valinit=init_time,
        )

        # The function to be called anytime a slider's value changes
        def update(val, recursive=False):
            time_slider.val = np.round(val / (self.dt*10), decimals=0) * (self.dt*10)
            rounded = int(np.round(time_slider.val / self.dt, decimals=0))
            for j in range(6):
                h1s[j].set_xdata(log_t_pred + time_slider.val)
                h2s[j].set_xdata(log_t_ref + time_slider.val)
                y1 = self.mpc_x_f[rounded, j, :] - self.planner_xref[rounded, j, 1:]
                y2 = self.planner_xref[rounded, j, :] - self.planner_xref[rounded, j, :]
                h1s[j].set_ydata(y1)
                h2s[j].set_ydata(y2)
                axs[j].set_xlim([time_slider.val - self.dt * 3, time_slider.val+trange+self.dt * 3])
                ymin = np.min([np.min(y1), np.min(y2)])
                ymax = np.max([np.max(y1), np.max(y2)])
                axs[j].set_ylim([ymin - 0.05 * (ymax - ymin), ymax + 0.05 * (ymax - ymin)])
            fig.canvas.draw_idle()
            if not recursive:
                update_vel(time_slider.val, True)

        def update_vel(val, recursive=False):
            time_slider_vel.val = np.round(val / (self.dt*10), decimals=0) * (self.dt*10)
            rounded = int(np.round(time_slider_vel.val / self.dt, decimals=0))
            for j in range(6):
                h1s_vel[j].set_xdata(log_t_pred + time_slider.val)
                h2s_vel[j].set_xdata(log_t_ref + time_slider.val)
                y1 = self.mpc_x_f[rounded, j+6, :]
                y2 = self.planner_xref[rounded, j+6, :]
                h1s_vel[j].set_ydata(y1)
                h2s_vel[j].set_ydata(y2)
                axs_vel[j].set_xlim([time_slider.val - self.dt * 3, time_slider.val+trange+self.dt * 3])
                ymin = np.min([np.min(y1), np.min(y2)])
                ymax = np.max([np.max(y1), np.max(y2)])
                axs_vel[j].set_ylim([ymin - 0.05 * (ymax - ymin), ymax + 0.05 * (ymax - ymin)])
            fig_vel.canvas.draw_idle()
            if not recursive:
                update(time_slider_vel.val, True)

        # register the update function with each slider
        time_slider.on_changed(update)
        time_slider_vel.on_changed(update)

        plt.show()

    def slider_predicted_footholds(self):

        from matplotlib import pyplot as plt
        from matplotlib.widgets import Slider, Button
        import utils_mpc
        import pinocchio as pin

        self.planner_fsteps

        # Define initial parameters
        init_time = 0.0

        # Create the figure and the line that we will manipulate
        fig = plt.figure()
        ax = plt.gca()
        h1s = []

        f_c = ["r", "b", "forestgreen", "rebeccapurple"]
        quat = np.zeros((4, 1))

        fsteps = self.planner_fsteps[0]
        o_step = np.zeros((3*int(fsteps.shape[0]), 1))
        RPY = pin.rpy.matrixToRpy(pin.Quaternion(self.loop_o_q[0, 3:7]).toRotationMatrix())
        quat[:, 0] = pin.Quaternion(pin.rpy.rpyToMatrix(np.array([0.0, 0.0, RPY[2]]))).coeffs()
        oRh = pin.Quaternion(quat).toRotationMatrix()
        for j in range(4):
            o_step[0:3, 0:1] = oRh @ fsteps[0:1, (j*3):((j+1)*3)].transpose() + self.loop_o_q[0:1, 0:3].transpose()
            h1, = plt.plot(o_step[0::3, 0], o_step[1::3, 0], linestyle=None, linewidth=0, marker="o", color=f_c[j])
            h1s.append(h1)

        axcolor = 'lightgoldenrodyellow'

        # Make a horizontal slider to control the time.
        axtime = plt.axes([0.25, 0.03, 0.65, 0.03], facecolor=axcolor)
        time_slider = Slider(
            ax=axtime,
            label='Time [s]',
            valmin=0.0,
            valmax=self.logSize*self.dt,
            valinit=init_time,
        )

        ax.set_xlim([-0.3, 0.5])
        ax.set_ylim([-0.3, 0.5])

        # The function to be called anytime a slider's value changes
        def update(val):
            time_slider.val = np.round(val / (self.dt*10), decimals=0) * (self.dt*10)
            rounded = int(np.round(time_slider.val / self.dt, decimals=0))
            fsteps = self.planner_fsteps[rounded]
            o_step = np.zeros((3*int(fsteps.shape[0]), 1))
            RPY = pin.rpy.matrixToRpy(pin.Quaternion(self.loop_o_q[rounded, 3:7]).toRotationMatrix())
            quat[:, 0] = pin.Quaternion(pin.rpy.rpyToMatrix(np.array([0.0, 0.0, RPY[2]]))).coeffs()
            oRh = pin.Quaternion(quat).toRotationMatrix()
            for j in range(4):
                for k in range(int(fsteps.shape[0])):
                    o_step[(3*k):(3*(k+1)), 0:1] = oRh @ fsteps[(k):(k+1), (j*3):((j+1)*3)].transpose() + self.loop_o_q[rounded:(rounded+1), 0:3].transpose()
                h1s[j].set_xdata(o_step[0::3, 0].copy())
                h1s[j].set_ydata(o_step[1::3, 0].copy())
            fig.canvas.draw_idle()

        # register the update function with each slider
        time_slider.on_changed(update)

        plt.show()


if __name__ == "__main__":

    import LoggerSensors
    import sys
    import os
    from sys import argv
    sys.path.insert(0, os.getcwd()) # adds current directory to python path

    # Data file name to load
    file_name = "/home/odri/data_2021_10_07_17_40.npz"

    # Create loggers
    loggerSensors = LoggerSensors.LoggerSensors(logSize=20000-3)
    logger = LoggerControl(0.001, 30, logSize=20000-3)

    # Load data from .npz file
    logger.loadAll(loggerSensors, fileName= file_name)

    # Call all ploting functions
    logger.plotAll(loggerSensors)

    # logger.slider_predicted_trajectory()