LoggerControl.py

        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_int[rounded, 3:7]).toRotationMatrix())
            quat[:, 0] = utils_mpc.EulerToQuaternion([0.0, 0.0, RPY[2]])
            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_int[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

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

    # Load data from .npz file
    logger.loadAll(loggerSensors)

    # Call all ploting functions
    logger.plotAll(loggerSensors)

    # logger.slider_predicted_trajectory()