LoggerControl.py

            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=5997)
    logger = LoggerControl(0.002, 100, logSize=5997)

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

    # Call all ploting functions
    #logger.plotAll(loggerSensors)

    logger.slider_predicted_trajectory()