my_trajectory.solver()
'''
Fill the global vector with the desired variables in each iteration
'''
g_acce_Tx[:, k] = my_trajectory.getacce()[:, 0]
g_acce_Ty[:, k] = my_trajectory.getacce()[:, 1]
g_acce_Tz[:, k] = my_trajectory.getacce()[:, 2]
g_angl_Tx[:, k] = my_trajectory.getangl()[:, 0]
g_angl_Ty[:, k] = my_trajectory.getangl()[:, 1]
g_angl_Tz[:, k] = my_trajectory.getangl()[:, 2]
g_posi_Tx[:, k] = my_trajectory.getposi()[:, 0]
g_posi_Ty[:, k] = my_trajectory.getposi()[:, 1]
g_posi_Tz[:, k] = my_trajectory.getposi()[:, 2]
g_ks_x1[:, k] = my_trajectory.getks()[:, 0]
g_ks_x2[:, k] = my_trajectory.getks()[:, 1]
g_ks_x3[:, k] = my_trajectory.getks()[:, 2]
g_ks_y1[:, k] = my_trajectory.getks()[:, 3]
g_ks_y2[:, k] = my_trajectory.getks()[:, 4]
g_ks_y3[:, k] = my_trajectory.getks()[:, 5]
g_ks_z1[:, k] = my_trajectory.getks()[:, 6]
g_ks_z2[:, k] = my_trajectory.getks()[:, 7]
g_ks_z3[:, k] = my_trajectory.getks()[:, 8]
k = k + 1
Tx_min = min(g_posi_Tx[-1, :])
Ty_min = min(g_posi_Ty[-1, :])
Tz_min = min(g_posi_Tz[-1, :])
