import numpy as np
import matplotlib
matplotlib.use('TkAgg')
import matplotlib.pyplot as plt
import os

# Plot configuration
PLOT_VERTICALLY = 0
PLOT_HORIZONTALLY = 1

# number of figures in this plot
num_figures = 5

def create_figures(subfigure_width=480, subfigure_height=600, starting_figure_no=1, starting_col_index = 0, starting_row_index=0, plot_configuration=PLOT_HORIZONTALLY):
    figure_number = starting_figure_no
    col_index = starting_col_index
    row_index = starting_row_index

    file_path = os.getcwd() + "/../test_data/"

    ## read files
    bezier_pos = np.genfromtxt(file_path+'bezier_pos.txt', delimiter=None, dtype=(float))
    bezier_vel = np.genfromtxt(file_path+'bezier_vel.txt', delimiter=None, dtype=(float))
    t = np.genfromtxt(file_path+'bz_time.txt', delimiter='\n', dtype=(float))

    bspline_pos = np.genfromtxt(file_path+'bspline_pos.txt', delimiter=None, dtype=(float))
    bspline_vel = np.genfromtxt(file_path+'bspline_vel.txt', delimiter=None, dtype=(float))
    bspline_acc = np.genfromtxt(file_path+'bspline_acc.txt', delimiter=None, dtype=(float))
    bs_t = np.genfromtxt(file_path+'bs_time.txt', delimiter='\n', dtype=(float))

    numeric_bezier_vel = np.copy(bezier_pos)
    bz_length = len(t)
    for i in range(bz_length-1):
        numeric_bezier_vel[i,:] = (bezier_pos[i+1,:] - bezier_pos[i,:])/(t[i+1]-t[i])
    
    numeric_bspline_vel = np.copy(bspline_pos)
    numeric_bspline_acc = np.copy(bspline_pos)
    bs_length = len(bs_t)
    for i in range(bs_length-1):
        numeric_bspline_vel[i,:] = (bspline_pos[i+1,:] - bspline_pos[i,:])/(bs_t[i+1]-bs_t[i])
        numeric_bspline_acc[i,:] = (bspline_vel[i+1,:] - bspline_vel[i,:])/(bs_t[i+1]-bs_t[i])
 
    ## plot bezier pos
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) + "x" + str(subfigure_height) +  "+" + str(subfigure_width*col_index) + "+" + str(subfigure_height*row_index))
    fig.canvas.set_window_title('bezier_pos')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i+1)
        plt.plot(t, bezier_pos[:,i], "b-")
        plt.grid(True)
    plt.xlabel('time (sec)')
    ## increment figure number and index
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index +=1

    ## plot bezier_vel
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) + "x" + str(subfigure_height) +  "+" + str(subfigure_width*col_index) + "+" + str(subfigure_height*row_index))
    fig.canvas.set_window_title('bezier_vel')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i+1)
        plt.plot(t, bezier_vel[:,i], "b-")
        plt.plot(t[:-1], numeric_bezier_vel[:-1,i], "r-")
        plt.grid(True)
    plt.xlabel('time (sec)')
    ## increment figure number and index
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index +=1

    ## plot bspline pos
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) + "x" + str(subfigure_height) +  "+" + str(subfigure_width*col_index) + "+" + str(subfigure_height*row_index))
    fig.canvas.set_window_title('bspline_pos')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i+1)
        plt.plot(bs_t, bspline_pos[:,i], "b-")
        plt.grid(True)
    plt.xlabel('time (sec)')
    ## increment figure number and index
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index +=1

    ## plot bspline_vel
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) + "x" + str(subfigure_height) +  "+" + str(subfigure_width*col_index) + "+" + str(subfigure_height*row_index))
    fig.canvas.set_window_title('bspline_vel')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i+1)
        plt.plot(bs_t, bspline_vel[:,i], "b-")
        plt.plot(bs_t[:-1], numeric_bspline_vel[:-1,i], "r-")
        plt.grid(True)
    plt.xlabel('time (sec)')
    ## increment figure number and index
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index +=1

    ## plot bspline_acc
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) + "x" + str(subfigure_height) +  "+" + str(subfigure_width*col_index) + "+" + str(subfigure_height*row_index))
    fig.canvas.set_window_title('bspline_acc')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i+1)
        plt.plot(bs_t, bspline_acc[:,i], "b-")
        plt.plot(bs_t[:-1], numeric_bspline_acc[:-1,i], "r-")
        plt.grid(True)
    plt.xlabel('time (sec)')
    ## increment figure number and index
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index +=1
        
if __name__ == "__main__":
    create_figures()
    plt.show()

d3e056851fdfc5630950ea623ddf4a5ba84245c2
<mask token>


def create_figures(subfigure_width=480, subfigure_height=600,
    starting_figure_no=1, starting_col_index=0, starting_row_index=0,
    plot_configuration=PLOT_HORIZONTALLY):
    figure_number = starting_figure_no
    col_index = starting_col_index
    row_index = starting_row_index
    file_path = os.getcwd() + '/../test_data/'
    bezier_pos = np.genfromtxt(file_path + 'bezier_pos.txt', delimiter=None,
        dtype=float)
    bezier_vel = np.genfromtxt(file_path + 'bezier_vel.txt', delimiter=None,
        dtype=float)
    t = np.genfromtxt(file_path + 'bz_time.txt', delimiter='\n', dtype=float)
    bspline_pos = np.genfromtxt(file_path + 'bspline_pos.txt', delimiter=
        None, dtype=float)
    bspline_vel = np.genfromtxt(file_path + 'bspline_vel.txt', delimiter=
        None, dtype=float)
    bspline_acc = np.genfromtxt(file_path + 'bspline_acc.txt', delimiter=
        None, dtype=float)
    bs_t = np.genfromtxt(file_path + 'bs_time.txt', delimiter='\n', dtype=float
        )
    numeric_bezier_vel = np.copy(bezier_pos)
    bz_length = len(t)
    for i in range(bz_length - 1):
        numeric_bezier_vel[i, :] = (bezier_pos[i + 1, :] - bezier_pos[i, :]
            ) / (t[i + 1] - t[i])
    numeric_bspline_vel = np.copy(bspline_pos)
    numeric_bspline_acc = np.copy(bspline_pos)
    bs_length = len(bs_t)
    for i in range(bs_length - 1):
        numeric_bspline_vel[i, :] = (bspline_pos[i + 1, :] - bspline_pos[i, :]
            ) / (bs_t[i + 1] - bs_t[i])
        numeric_bspline_acc[i, :] = (bspline_vel[i + 1, :] - bspline_vel[i, :]
            ) / (bs_t[i + 1] - bs_t[i])
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) +
        'x' + str(subfigure_height) + '+' + str(subfigure_width * col_index
        ) + '+' + str(subfigure_height * row_index))
    fig.canvas.set_window_title('bezier_pos')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i + 1)
        plt.plot(t, bezier_pos[:, i], 'b-')
        plt.grid(True)
    plt.xlabel('time (sec)')
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index += 1
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) +
        'x' + str(subfigure_height) + '+' + str(subfigure_width * col_index
        ) + '+' + str(subfigure_height * row_index))
    fig.canvas.set_window_title('bezier_vel')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i + 1)
        plt.plot(t, bezier_vel[:, i], 'b-')
        plt.plot(t[:-1], numeric_bezier_vel[:-1, i], 'r-')
        plt.grid(True)
    plt.xlabel('time (sec)')
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index += 1
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) +
        'x' + str(subfigure_height) + '+' + str(subfigure_width * col_index
        ) + '+' + str(subfigure_height * row_index))
    fig.canvas.set_window_title('bspline_pos')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i + 1)
        plt.plot(bs_t, bspline_pos[:, i], 'b-')
        plt.grid(True)
    plt.xlabel('time (sec)')
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index += 1
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) +
        'x' + str(subfigure_height) + '+' + str(subfigure_width * col_index
        ) + '+' + str(subfigure_height * row_index))
    fig.canvas.set_window_title('bspline_vel')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i + 1)
        plt.plot(bs_t, bspline_vel[:, i], 'b-')
        plt.plot(bs_t[:-1], numeric_bspline_vel[:-1, i], 'r-')
        plt.grid(True)
    plt.xlabel('time (sec)')
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index += 1
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) +
        'x' + str(subfigure_height) + '+' + str(subfigure_width * col_index
        ) + '+' + str(subfigure_height * row_index))
    fig.canvas.set_window_title('bspline_acc')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i + 1)
        plt.plot(bs_t, bspline_acc[:, i], 'b-')
        plt.plot(bs_t[:-1], numeric_bspline_acc[:-1, i], 'r-')
        plt.grid(True)
    plt.xlabel('time (sec)')
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index += 1


<mask token>

<mask token>
matplotlib.use('TkAgg')
<mask token>


def create_figures(subfigure_width=480, subfigure_height=600,
    starting_figure_no=1, starting_col_index=0, starting_row_index=0,
    plot_configuration=PLOT_HORIZONTALLY):
    figure_number = starting_figure_no
    col_index = starting_col_index
    row_index = starting_row_index
    file_path = os.getcwd() + '/../test_data/'
    bezier_pos = np.genfromtxt(file_path + 'bezier_pos.txt', delimiter=None,
        dtype=float)
    bezier_vel = np.genfromtxt(file_path + 'bezier_vel.txt', delimiter=None,
        dtype=float)
    t = np.genfromtxt(file_path + 'bz_time.txt', delimiter='\n', dtype=float)
    bspline_pos = np.genfromtxt(file_path + 'bspline_pos.txt', delimiter=
        None, dtype=float)
    bspline_vel = np.genfromtxt(file_path + 'bspline_vel.txt', delimiter=
        None, dtype=float)
    bspline_acc = np.genfromtxt(file_path + 'bspline_acc.txt', delimiter=
        None, dtype=float)
    bs_t = np.genfromtxt(file_path + 'bs_time.txt', delimiter='\n', dtype=float
        )
    numeric_bezier_vel = np.copy(bezier_pos)
    bz_length = len(t)
    for i in range(bz_length - 1):
        numeric_bezier_vel[i, :] = (bezier_pos[i + 1, :] - bezier_pos[i, :]
            ) / (t[i + 1] - t[i])
    numeric_bspline_vel = np.copy(bspline_pos)
    numeric_bspline_acc = np.copy(bspline_pos)
    bs_length = len(bs_t)
    for i in range(bs_length - 1):
        numeric_bspline_vel[i, :] = (bspline_pos[i + 1, :] - bspline_pos[i, :]
            ) / (bs_t[i + 1] - bs_t[i])
        numeric_bspline_acc[i, :] = (bspline_vel[i + 1, :] - bspline_vel[i, :]
            ) / (bs_t[i + 1] - bs_t[i])
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) +
        'x' + str(subfigure_height) + '+' + str(subfigure_width * col_index
        ) + '+' + str(subfigure_height * row_index))
    fig.canvas.set_window_title('bezier_pos')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i + 1)
        plt.plot(t, bezier_pos[:, i], 'b-')
        plt.grid(True)
    plt.xlabel('time (sec)')
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index += 1
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) +
        'x' + str(subfigure_height) + '+' + str(subfigure_width * col_index
        ) + '+' + str(subfigure_height * row_index))
    fig.canvas.set_window_title('bezier_vel')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i + 1)
        plt.plot(t, bezier_vel[:, i], 'b-')
        plt.plot(t[:-1], numeric_bezier_vel[:-1, i], 'r-')
        plt.grid(True)
    plt.xlabel('time (sec)')
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index += 1
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) +
        'x' + str(subfigure_height) + '+' + str(subfigure_width * col_index
        ) + '+' + str(subfigure_height * row_index))
    fig.canvas.set_window_title('bspline_pos')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i + 1)
        plt.plot(bs_t, bspline_pos[:, i], 'b-')
        plt.grid(True)
    plt.xlabel('time (sec)')
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index += 1
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) +
        'x' + str(subfigure_height) + '+' + str(subfigure_width * col_index
        ) + '+' + str(subfigure_height * row_index))
    fig.canvas.set_window_title('bspline_vel')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i + 1)
        plt.plot(bs_t, bspline_vel[:, i], 'b-')
        plt.plot(bs_t[:-1], numeric_bspline_vel[:-1, i], 'r-')
        plt.grid(True)
    plt.xlabel('time (sec)')
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index += 1
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) +
        'x' + str(subfigure_height) + '+' + str(subfigure_width * col_index
        ) + '+' + str(subfigure_height * row_index))
    fig.canvas.set_window_title('bspline_acc')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i + 1)
        plt.plot(bs_t, bspline_acc[:, i], 'b-')
        plt.plot(bs_t[:-1], numeric_bspline_acc[:-1, i], 'r-')
        plt.grid(True)
    plt.xlabel('time (sec)')
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index += 1


if __name__ == '__main__':
    create_figures()
    plt.show()

<mask token>
matplotlib.use('TkAgg')
<mask token>
PLOT_VERTICALLY = 0
PLOT_HORIZONTALLY = 1
num_figures = 5


def create_figures(subfigure_width=480, subfigure_height=600,
    starting_figure_no=1, starting_col_index=0, starting_row_index=0,
    plot_configuration=PLOT_HORIZONTALLY):
    figure_number = starting_figure_no
    col_index = starting_col_index
    row_index = starting_row_index
    file_path = os.getcwd() + '/../test_data/'
    bezier_pos = np.genfromtxt(file_path + 'bezier_pos.txt', delimiter=None,
        dtype=float)
    bezier_vel = np.genfromtxt(file_path + 'bezier_vel.txt', delimiter=None,
        dtype=float)
    t = np.genfromtxt(file_path + 'bz_time.txt', delimiter='\n', dtype=float)
    bspline_pos = np.genfromtxt(file_path + 'bspline_pos.txt', delimiter=
        None, dtype=float)
    bspline_vel = np.genfromtxt(file_path + 'bspline_vel.txt', delimiter=
        None, dtype=float)
    bspline_acc = np.genfromtxt(file_path + 'bspline_acc.txt', delimiter=
        None, dtype=float)
    bs_t = np.genfromtxt(file_path + 'bs_time.txt', delimiter='\n', dtype=float
        )
    numeric_bezier_vel = np.copy(bezier_pos)
    bz_length = len(t)
    for i in range(bz_length - 1):
        numeric_bezier_vel[i, :] = (bezier_pos[i + 1, :] - bezier_pos[i, :]
            ) / (t[i + 1] - t[i])
    numeric_bspline_vel = np.copy(bspline_pos)
    numeric_bspline_acc = np.copy(bspline_pos)
    bs_length = len(bs_t)
    for i in range(bs_length - 1):
        numeric_bspline_vel[i, :] = (bspline_pos[i + 1, :] - bspline_pos[i, :]
            ) / (bs_t[i + 1] - bs_t[i])
        numeric_bspline_acc[i, :] = (bspline_vel[i + 1, :] - bspline_vel[i, :]
            ) / (bs_t[i + 1] - bs_t[i])
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) +
        'x' + str(subfigure_height) + '+' + str(subfigure_width * col_index
        ) + '+' + str(subfigure_height * row_index))
    fig.canvas.set_window_title('bezier_pos')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i + 1)
        plt.plot(t, bezier_pos[:, i], 'b-')
        plt.grid(True)
    plt.xlabel('time (sec)')
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index += 1
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) +
        'x' + str(subfigure_height) + '+' + str(subfigure_width * col_index
        ) + '+' + str(subfigure_height * row_index))
    fig.canvas.set_window_title('bezier_vel')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i + 1)
        plt.plot(t, bezier_vel[:, i], 'b-')
        plt.plot(t[:-1], numeric_bezier_vel[:-1, i], 'r-')
        plt.grid(True)
    plt.xlabel('time (sec)')
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index += 1
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) +
        'x' + str(subfigure_height) + '+' + str(subfigure_width * col_index
        ) + '+' + str(subfigure_height * row_index))
    fig.canvas.set_window_title('bspline_pos')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i + 1)
        plt.plot(bs_t, bspline_pos[:, i], 'b-')
        plt.grid(True)
    plt.xlabel('time (sec)')
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index += 1
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) +
        'x' + str(subfigure_height) + '+' + str(subfigure_width * col_index
        ) + '+' + str(subfigure_height * row_index))
    fig.canvas.set_window_title('bspline_vel')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i + 1)
        plt.plot(bs_t, bspline_vel[:, i], 'b-')
        plt.plot(bs_t[:-1], numeric_bspline_vel[:-1, i], 'r-')
        plt.grid(True)
    plt.xlabel('time (sec)')
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index += 1
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) +
        'x' + str(subfigure_height) + '+' + str(subfigure_width * col_index
        ) + '+' + str(subfigure_height * row_index))
    fig.canvas.set_window_title('bspline_acc')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i + 1)
        plt.plot(bs_t, bspline_acc[:, i], 'b-')
        plt.plot(bs_t[:-1], numeric_bspline_acc[:-1, i], 'r-')
        plt.grid(True)
    plt.xlabel('time (sec)')
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index += 1


if __name__ == '__main__':
    create_figures()
    plt.show()

import numpy as np
import matplotlib
matplotlib.use('TkAgg')
import matplotlib.pyplot as plt
import os
PLOT_VERTICALLY = 0
PLOT_HORIZONTALLY = 1
num_figures = 5


def create_figures(subfigure_width=480, subfigure_height=600,
    starting_figure_no=1, starting_col_index=0, starting_row_index=0,
    plot_configuration=PLOT_HORIZONTALLY):
    figure_number = starting_figure_no
    col_index = starting_col_index
    row_index = starting_row_index
    file_path = os.getcwd() + '/../test_data/'
    bezier_pos = np.genfromtxt(file_path + 'bezier_pos.txt', delimiter=None,
        dtype=float)
    bezier_vel = np.genfromtxt(file_path + 'bezier_vel.txt', delimiter=None,
        dtype=float)
    t = np.genfromtxt(file_path + 'bz_time.txt', delimiter='\n', dtype=float)
    bspline_pos = np.genfromtxt(file_path + 'bspline_pos.txt', delimiter=
        None, dtype=float)
    bspline_vel = np.genfromtxt(file_path + 'bspline_vel.txt', delimiter=
        None, dtype=float)
    bspline_acc = np.genfromtxt(file_path + 'bspline_acc.txt', delimiter=
        None, dtype=float)
    bs_t = np.genfromtxt(file_path + 'bs_time.txt', delimiter='\n', dtype=float
        )
    numeric_bezier_vel = np.copy(bezier_pos)
    bz_length = len(t)
    for i in range(bz_length - 1):
        numeric_bezier_vel[i, :] = (bezier_pos[i + 1, :] - bezier_pos[i, :]
            ) / (t[i + 1] - t[i])
    numeric_bspline_vel = np.copy(bspline_pos)
    numeric_bspline_acc = np.copy(bspline_pos)
    bs_length = len(bs_t)
    for i in range(bs_length - 1):
        numeric_bspline_vel[i, :] = (bspline_pos[i + 1, :] - bspline_pos[i, :]
            ) / (bs_t[i + 1] - bs_t[i])
        numeric_bspline_acc[i, :] = (bspline_vel[i + 1, :] - bspline_vel[i, :]
            ) / (bs_t[i + 1] - bs_t[i])
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) +
        'x' + str(subfigure_height) + '+' + str(subfigure_width * col_index
        ) + '+' + str(subfigure_height * row_index))
    fig.canvas.set_window_title('bezier_pos')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i + 1)
        plt.plot(t, bezier_pos[:, i], 'b-')
        plt.grid(True)
    plt.xlabel('time (sec)')
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index += 1
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) +
        'x' + str(subfigure_height) + '+' + str(subfigure_width * col_index
        ) + '+' + str(subfigure_height * row_index))
    fig.canvas.set_window_title('bezier_vel')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i + 1)
        plt.plot(t, bezier_vel[:, i], 'b-')
        plt.plot(t[:-1], numeric_bezier_vel[:-1, i], 'r-')
        plt.grid(True)
    plt.xlabel('time (sec)')
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index += 1
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) +
        'x' + str(subfigure_height) + '+' + str(subfigure_width * col_index
        ) + '+' + str(subfigure_height * row_index))
    fig.canvas.set_window_title('bspline_pos')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i + 1)
        plt.plot(bs_t, bspline_pos[:, i], 'b-')
        plt.grid(True)
    plt.xlabel('time (sec)')
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index += 1
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) +
        'x' + str(subfigure_height) + '+' + str(subfigure_width * col_index
        ) + '+' + str(subfigure_height * row_index))
    fig.canvas.set_window_title('bspline_vel')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i + 1)
        plt.plot(bs_t, bspline_vel[:, i], 'b-')
        plt.plot(bs_t[:-1], numeric_bspline_vel[:-1, i], 'r-')
        plt.grid(True)
    plt.xlabel('time (sec)')
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index += 1
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) +
        'x' + str(subfigure_height) + '+' + str(subfigure_width * col_index
        ) + '+' + str(subfigure_height * row_index))
    fig.canvas.set_window_title('bspline_acc')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i + 1)
        plt.plot(bs_t, bspline_acc[:, i], 'b-')
        plt.plot(bs_t[:-1], numeric_bspline_acc[:-1, i], 'r-')
        plt.grid(True)
    plt.xlabel('time (sec)')
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index += 1


if __name__ == '__main__':
    create_figures()
    plt.show()

import numpy as np
import matplotlib
matplotlib.use('TkAgg')
import matplotlib.pyplot as plt
import os

# Plot configuration
PLOT_VERTICALLY = 0
PLOT_HORIZONTALLY = 1

# number of figures in this plot
num_figures = 5

def create_figures(subfigure_width=480, subfigure_height=600, starting_figure_no=1, starting_col_index = 0, starting_row_index=0, plot_configuration=PLOT_HORIZONTALLY):
    figure_number = starting_figure_no
    col_index = starting_col_index
    row_index = starting_row_index

    file_path = os.getcwd() + "/../test_data/"

    ## read files
    bezier_pos = np.genfromtxt(file_path+'bezier_pos.txt', delimiter=None, dtype=(float))
    bezier_vel = np.genfromtxt(file_path+'bezier_vel.txt', delimiter=None, dtype=(float))
    t = np.genfromtxt(file_path+'bz_time.txt', delimiter='\n', dtype=(float))

    bspline_pos = np.genfromtxt(file_path+'bspline_pos.txt', delimiter=None, dtype=(float))
    bspline_vel = np.genfromtxt(file_path+'bspline_vel.txt', delimiter=None, dtype=(float))
    bspline_acc = np.genfromtxt(file_path+'bspline_acc.txt', delimiter=None, dtype=(float))
    bs_t = np.genfromtxt(file_path+'bs_time.txt', delimiter='\n', dtype=(float))

    numeric_bezier_vel = np.copy(bezier_pos)
    bz_length = len(t)
    for i in range(bz_length-1):
        numeric_bezier_vel[i,:] = (bezier_pos[i+1,:] - bezier_pos[i,:])/(t[i+1]-t[i])
    
    numeric_bspline_vel = np.copy(bspline_pos)
    numeric_bspline_acc = np.copy(bspline_pos)
    bs_length = len(bs_t)
    for i in range(bs_length-1):
        numeric_bspline_vel[i,:] = (bspline_pos[i+1,:] - bspline_pos[i,:])/(bs_t[i+1]-bs_t[i])
        numeric_bspline_acc[i,:] = (bspline_vel[i+1,:] - bspline_vel[i,:])/(bs_t[i+1]-bs_t[i])
 
    ## plot bezier pos
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) + "x" + str(subfigure_height) +  "+" + str(subfigure_width*col_index) + "+" + str(subfigure_height*row_index))
    fig.canvas.set_window_title('bezier_pos')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i+1)
        plt.plot(t, bezier_pos[:,i], "b-")
        plt.grid(True)
    plt.xlabel('time (sec)')
    ## increment figure number and index
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index +=1

    ## plot bezier_vel
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) + "x" + str(subfigure_height) +  "+" + str(subfigure_width*col_index) + "+" + str(subfigure_height*row_index))
    fig.canvas.set_window_title('bezier_vel')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i+1)
        plt.plot(t, bezier_vel[:,i], "b-")
        plt.plot(t[:-1], numeric_bezier_vel[:-1,i], "r-")
        plt.grid(True)
    plt.xlabel('time (sec)')
    ## increment figure number and index
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index +=1

    ## plot bspline pos
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) + "x" + str(subfigure_height) +  "+" + str(subfigure_width*col_index) + "+" + str(subfigure_height*row_index))
    fig.canvas.set_window_title('bspline_pos')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i+1)
        plt.plot(bs_t, bspline_pos[:,i], "b-")
        plt.grid(True)
    plt.xlabel('time (sec)')
    ## increment figure number and index
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index +=1

    ## plot bspline_vel
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) + "x" + str(subfigure_height) +  "+" + str(subfigure_width*col_index) + "+" + str(subfigure_height*row_index))
    fig.canvas.set_window_title('bspline_vel')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i+1)
        plt.plot(bs_t, bspline_vel[:,i], "b-")
        plt.plot(bs_t[:-1], numeric_bspline_vel[:-1,i], "r-")
        plt.grid(True)
    plt.xlabel('time (sec)')
    ## increment figure number and index
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index +=1

    ## plot bspline_acc
    fig = plt.figure(figure_number)
    plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) + "x" + str(subfigure_height) +  "+" + str(subfigure_width*col_index) + "+" + str(subfigure_height*row_index))
    fig.canvas.set_window_title('bspline_acc')
    for i in range(3):
        ax1 = plt.subplot(3, 1, i+1)
        plt.plot(bs_t, bspline_acc[:,i], "b-")
        plt.plot(bs_t[:-1], numeric_bspline_acc[:-1,i], "r-")
        plt.grid(True)
    plt.xlabel('time (sec)')
    ## increment figure number and index
    figure_number += 1
    if plot_configuration == PLOT_HORIZONTALLY:
        col_index += 1
    elif plot_configuration == PLOT_VERTICALLY:
        row_index +=1
        
if __name__ == "__main__":
    create_figures()
    plt.show()
