Commit 57e459d4 authored by Nicolas Van der Noot's avatar Nicolas Van der Noot

first commit

parents
*.pyc
*~
*.m
*.anim
*.res
**/*.m
__pycache__
stderr.txt
stdout.txt
build
debug
doc/html
doc/latex
.DS_Store
This diff is collapsed.
# import libraries
import numpy as np
from matplotlib import pyplot as plt
plt.switch_backend('Qt4Agg')
# check if the curve index is in the arguments
# return 1 if it is the case (or if no argument), 0 otherwise
# index: index of the current curve
# args: arguments with indexes to plot
def check_args(index, args):
# no argument
if len(args) == 0:
return 1
# loop on all the arguments
for i in args:
if i == index:
return 1
return 0
# plot result function
# res_name: result file name (without extension)
# args: arguments with indexes to plot
def plot_res(res_name, *args):
#get file name
filename = '{}.res'.format(res_name)
# get data
data = np.loadtxt(filename)
# get size of the data matrix
[a, b] = np.shape(data)
# at least two columns are requested
if b < 2:
print('error: only {} colums !'.format(b))
return
# time
t = data[:,0]
# new figure
plt.figure()
# plot curves
nb_curves = 0
for i in range(1, b):
if check_args(i, args):
plt.plot(t, data[:,i], label='{}'.format(i))
nb_curves += 1
if nb_curves == 0:
return
# plot properties
plt.title(res_name)
plt.xlabel('time [s]')
plt.xlim([min(t),max(t)])
# legends
if b > 2:
plt.legend(loc=4, numpoints=1)
# Use the 'plot_res' function to plot the requested .res file.
# The first argument is the name of the file, without its extension.
# You can add extra arguments (after the file name) to specify the indexes of the curves
# to plot (starting at 1). Each index must be separated by a coma. If no indexes are indicated,
# all the curves are plotted.
# You can also use the line 'plt.savefig('./figure.pdf')' to save a pdf of the plotted figure.
# Use this line after the corresponding 'plot_res()' (before 'plt.show()'). This works also
# with other formats like pgf.
#
# examples:
# plot_res('dirdyn_q')
# plt.savefig('./dirdyn_q.pdf')
# plot_res('dirdyn_q', 1, 3, 9)
# plot_res('my_var')
# plt.savefig('./my_var.pdf')
#
plot_res('dirdyn_q')
# show plots
plt.show()
# The analysis of the CPG signals is diplayed using this file.
# The evolution of the corresponding signals is presented.
# import libraries
import numpy as np
import matplotlib
from matplotlib import pyplot as plt
plt.switch_backend('Qt4Agg')
matplotlib.rc('text', usetex=True)
matplotlib.rcParams['text.latex.preamble']=[r"\usepackage{amsmath}"]
i0 = 8813
i1 = 9875
# void tables
x_data = []
x_tab = []
x_height = []
# output folder
output_folder = '../graphs'
# 1 to save the figure, 0 otherwise
save_fig = 0
# get data
x_data.append(np.loadtxt('data/dirdyn_x_RG_1.res'))
x_data.append(np.loadtxt('data/dirdyn_x_RG_2.res'))
x_data.append(np.loadtxt('data/dirdyn_x_RG_3.res'))
x_data.append(np.loadtxt('data/dirdyn_x_RG_4.res'))
x_data.append(np.loadtxt('data/dirdyn_x_PF_A.res'))
x_data.append(np.loadtxt('data/dirdyn_x_PF_B.res'))
x_data.append(np.loadtxt('data/dirdyn_x_PF_C.res'))
x_data.append(np.loadtxt('data/dirdyn_x_PF_D.res'))
x_data.append(np.loadtxt('data/dirdyn_x_PF_E.res'))
x_data.append(np.loadtxt('data/dirdyn_x_PF_F.res'))
x_data.append(np.loadtxt('data/dirdyn_x_PF_G.res'))
x_data.append(np.loadtxt('data/dirdyn_x_PF_H.res'))
# time
t = x_data[0][i0:i1,0]
# % vector
x = np.linspace(0, 100, len(t))
# get vectors
for i in range(12):
x_tab.append(x_data[i][i0:i1,1])
x_height.append(11.0 - 1.0 * i)
# figure
fig = plt.figure(figsize=(10, 13.5))
ax = plt.gca()
for i in range(12):
ax.fill_between(x, x_height[i], x_tab[i] + x_height[i], facecolor='black', alpha=1.0)
# left foot strike
plt.plot([50, 50], [0, 11.7], 'k--', lw=1.5)
# horizontal axis
y_hor_axis = -0.5
plt.plot([0, 100], [y_hor_axis, y_hor_axis], 'black', lw=2)
for i in range(6):
plt.plot([20*i, 20*i], [y_hor_axis, y_hor_axis - 0.2], 'black', lw=2)
ax.text(20*i, y_hor_axis - 0.65, 20*i, horizontalalignment='center', verticalalignment='center', fontsize=16, color='black')
ax.text(50, y_hor_axis - 1.2, r'$\boldsymbol{gait~cycle~(\%)}$', horizontalalignment='center', verticalalignment='center', fontsize=20, color='black', fontweight='bold')
# vertical axes
x_vert_axis = -3.5
y_labels = [ \
r'$\boldsymbol{x_H}$', r'$\boldsymbol{x_G}$', r'$\boldsymbol{x_F}$', r'$\boldsymbol{x_E}$', r'$\boldsymbol{x_D}$', r'$\boldsymbol{x_C}$', r'$\boldsymbol{x_B}$', r'$\boldsymbol{x_A}$', \
r'$\boldsymbol{x_4}$', r'$\boldsymbol{x_3}$', r'$\boldsymbol{x_2}$', r'$\boldsymbol{x_1}$']
for i in range(len(y_labels)):
i0 = i*1.0
i1 = i*1.0+0.5
i_mid = (i0 + i1) / 2
plt.plot([x_vert_axis, x_vert_axis], [i0, i1], 'black', lw=2)
plt.plot([x_vert_axis - 1.5, x_vert_axis], [i0, i0], 'black', lw=2)
plt.plot([x_vert_axis - 1.5, x_vert_axis], [i1, i1], 'black', lw=2)
ax.text(x_vert_axis - 5.0, i0, 0, horizontalalignment='center', verticalalignment='center', fontsize=16, color='black')
ax.text(x_vert_axis - 5.0, i1, 0.5, horizontalalignment='center', verticalalignment='center', fontsize=16, color='black')
ax.text(x_vert_axis - 12.0, i_mid, y_labels[i], horizontalalignment='center', verticalalignment='center', fontsize=20, color='black', fontweight='bold')
# remove axis
ax.set_axis_off()
plt.xlim([-20, 105])
plt.ylim([-1.7, 11.9])
if save_fig:
plt.savefig('{}/cpg_analysis.eps'.format(output_folder), bbox_inches='tight', pad_inches=0)
plt.show()
This folder is used to save the graphs generated in the *resultsR* folder.
# This file studies the robustness of the controller when subject
# to external pushes, at different forward speed references.
import numpy as np
import matplotlib.pyplot as plt
# speed reference [m/s]
speed_ref = np.array([0.4, 0.45, 0.5, 0.55, 0.6, 0.65, 0.7, 0.75, 0.8, 0.85, 0.9])
# push force [N]
push_vec = np.array([0.0, 2.5, 5.0, 7.5, 10.0, 12.5, 15.0, 17.5, 20.0, 22.5, 25.0, 27.5, 30.0])
# results
push_mat = np.array([
[ 0.2 , 0 , 0.8 , 0.4 , 0.8 , 1.2 , 0.6 , 1.4 , 3.2 , 0.4 , 0.8 ],
[ 0.2 , 0.8 , 0 , 1.2 , 1.4 , 0.4 , 0.2 , 2.8 , 0.8 , 3.6 , 0.8 ],
[ 0 , 0.6 , 0.6 , 2.6 , 2.8 , 3 , 2.2 , 4 , 6.6 , 5.2 , 0.2 ],
[ 0.2 , 0.6 , 0.2 , 0.4 , 3 , 1 , 3 , 2.8 , 5 , 5.8 , 3.8 ],
[ 0.6 , 2.4 , 0.8 , 1 , 1.2 , 3 , 8.2 , 5.4 , 7.2 , 8 , 1.8 ],
[ 1 , 1.6 , 2 , 1 , 6.8 , 4.6 , 9.8 , 5.4 , 7 , 10 , 4.8 ],
[ 2 , 3.8 , 3.8 , 7.4 , 5.6 , 5.6 , 5.2 , 8 , 6.6 , 9 , 3.8 ],
[ 1.4 , 5.4 , 5.6 , 7 , 8.2 , 8.2 , 10 , 8 , 10 , 10 , 4.8 ],
[ 8 , 8.4 , 10 , 9.4 , 7.8 , 10 , 6.6 , 10 , 10 , 10 , 7.2 ],
[ 8.2 , 9.8 , 10 , 8.2 , 9 , 10 , 9 , 10 , 10 , 10 , 10 ],
[ 8.2 , 10 , 8.6 , 10 , 10 , 9.4 , 10 , 10 , 10 , 10 , 10 ],
[ 7.4 , 10 , 9 , 9 , 8.2 , 10 , 10 , 10 , 10 , 10 , 10 ],
[ 10 , 10 , 7.6 , 10 , 9.4 , 10 , 10 , 10 , 10 , 10 , 10 ]
])
# output folder
output_folder = '../graphs'
# 1 to save the figure, 0 otherwise
save_fig = 0
# figure
fig = plt.figure()
ax = plt.gca()
im_l = ax.matshow(push_mat, cmap='YlOrRd', interpolation='none')
cbar = plt.colorbar(im_l, ticks=[np.amin(push_mat), 2.0, 4.0, 6.0, 8.0, np.amax(push_mat)])
cbar.ax.tick_params(axis='y', labelsize=15, direction='out', width=1.5)
ax.set_xticks((0, 2, 4, 6, 8, 10))
ax.set_yticks((12, 10, 8, 6, 4, 2, 0))
ax.set_xticklabels(speed_ref[0:11:2], fontsize=12)
ax.set_yticklabels(push_vec[0:13:2], fontsize=12)
ax.xaxis.tick_bottom()
ax.set_xlabel("speed reference (m/s)" , fontsize=18, fontweight='bold')
ax.set_ylabel("force (N)", fontsize=18, fontweight='bold')
# label space
ax.xaxis.labelpad = 10
ax.yaxis.labelpad = 10
# only show ticks on left and bottom spines
ax.yaxis.set_ticks_position('left')
ax.xaxis.set_ticks_position('bottom')
# ticks parameters
plt.tick_params(axis='both', which='major', labelsize=15, direction='out', width=2)
if save_fig:
plt.savefig('{}/push_torso.eps'.format(output_folder), bbox_inches='tight', pad_inches=0.25)
# show graphs
plt.show()
# This file studies the robustness of the controller when walking
# on irregular grounds, randomly generated.
# This is studied at different forward speed references and with different
# ranges of ground point heights.
import numpy as np
import matplotlib.pyplot as plt
# speed reference [m/s]
speed_ref = np.array([0.4, 0.45, 0.5, 0.55, 0.6, 0.65, 0.7, 0.75, 0.8, 0.85, 0.9])
# maximal random ground height [m]
rnd_height_vec = np.array([ 0.0, 3.0e-3, 6.0e-3, 9.0e-3, 12.0e-3, 15.0e-3, 18.0e-3, 21.0e-3, 24.0e-3, 27.0e-3, 30.0e-3, 33.0e-3, 36.0e-3])
# conversion from [m] to [mm]
rnd_height_vec = 1.0e3 * rnd_height_vec
# results
rnd_ground_mat = np.array([
[ 0.103577 , 0.119489 , 0.168457 , 0.684351 , 1.12295 , 1.20027 , 3.8989 , 4.44034 , 3.06257 , 3.93777 , 3.48661 ],
[ 0.0676315 , 0.233276 , 0.201764 , 0.919159 , 1.33989 , 0.589182 , 2.27745 , 4.13665 , 3.06516 , 6.41571 , 7.13911 ],
[ 0 , 0.292967 , 0.236973 , 0.716915 , 0.564593 , 3.01769 , 3.401 , 2.15774 , 5.45134 , 5.63352 , 3.13265 ],
[ 0.148422 , 0.634929 , 1.23316 , 1.16333 , 2.49709 , 3.48268 , 4.29693 , 5.30253 , 7.19584 , 6.18432 , 3.62666 ],
[ 0.126668 , 0.285842 , 1.73757 , 1.26308 , 4.03195 , 3.49154 , 3.65439 , 6.89492 , 9.33387 , 8.55205 , 7.13057 ],
[ 1.1835 , 0.570974 , 0.260571 , 2.42031 , 5.89837 , 4.21026 , 7.13301 , 7.05487 , 9.20491 , 8.91289 , 6.91676 ],
[ 0.1156 , 0.838412 , 1.54405 , 2.7777 , 6.98409 , 6.33578 , 10 , 9.28761 , 9.12752 , 9.10658 , 7.16108 ],
[ 1.10673 , 0.788146 , 3.42418 , 6.79961 , 6.51081 , 8.30155 , 7.38367 , 8.98521 , 9.15974 , 10 , 9.21709 ],
[ 1.76023 , 2.67716 , 8.69934 , 6.17147 , 6.84471 , 8.42652 , 9.3941 , 10 , 7.83654 , 10 , 8.66733 ],
[ 4.98498 , 4.46884 , 4.97005 , 8.6538 , 9.13096 , 10 , 7.67472 , 10 , 8.66525 , 10 , 10 ],
[ 4.48622 , 8.06353 , 9.29447 , 10 , 10 , 10 , 10 , 10 , 10 , 10 , 10 ],
[ 7.46756 , 9.00859 , 10 , 8.73838 , 9.0343 , 10 , 9.55338 , 10 , 10 , 10 , 10 ],
[ 9.18494 , 10 , 10 , 10 , 10 , 10 , 10 , 10 , 10 , 10 , 8 ],
])
# output folder
output_folder = '../graphs'
# 1 to save the figures, 0 otherwise
save_fig = 0
# figure
fig = plt.figure()
ax = plt.gca()
im_l = ax.matshow(rnd_ground_mat, cmap='YlOrRd', interpolation='none')
cbar = plt.colorbar(im_l, ticks=[np.amin(rnd_ground_mat), 2.0, 4.0, 6.0, 8.0, np.amax(rnd_ground_mat)])
cbar.ax.tick_params(axis='y', labelsize=15, direction='out', width=1.5)
ax.set_xticks((0, 2, 4, 6, 8, 10))
ax.set_yticks((12, 10, 8, 6, 4, 2, 0))
ax.set_xticklabels(speed_ref[0:11:2], fontsize=12)
ax.set_yticklabels(rnd_height_vec[0:13:2], fontsize=12)
ax.xaxis.tick_bottom()
ax.set_xlabel("speed reference (m/s)", fontsize=18, fontweight='bold')
ax.set_ylabel("obstacle maximal height (mm)", fontsize=18, fontweight='bold')
# label space
ax.xaxis.labelpad = 10
ax.yaxis.labelpad = 10
# only show ticks on left and bottom spines
ax.yaxis.set_ticks_position('left')
ax.xaxis.set_ticks_position('bottom')
# ticks parameters
plt.tick_params(axis='both', which='major', labelsize=15, direction='out', width=2)
if save_fig:
plt.savefig('{}/rnd_ground.eps'.format(output_folder), bbox_inches='tight', pad_inches=0.25)
# show graphs
plt.show()
# This file studies the robustness of the controller when walking
# on rising and descending slopes, at different forward speeds.
import numpy as np
import matplotlib.pyplot as plt
# convert rad vector to deg
def rad_vec_to_deg(vec):
out_vec = vec
for i in range(len(vec)):
out_vec[i] = round(vec[i] * (180.0 / np.pi), 2)
return out_vec
# speed reference [m/s]
speed_ref = np.array([0.4, 0.45, 0.5, 0.55, 0.6, 0.65, 0.7, 0.75, 0.8, 0.85, 0.9])
# slope angle: negative slope [rad]
neg_slope_vec = np.array([
0.0, -5.0e-3, -10.0e-3, -15.0e-3, -20.0e-3, -25.0e-3, -30.0e-3, -35.0e-3, -40.0e-3, -45.0e-3, -50.0e-3,
-55.0e-3, -60.0e-3, -65.0e-3, -70.0e-3])
# slope angle: positive slope [rad]
pos_slope_vec = np.array([0.0, 5.0e-3, 10.0e-3, 15.0e-3, 20.0e-3, 25.0e-3, 30.0e-3, 35.0e-3, 40.0e-3, 45.0e-3,
50.0e-3, 55.0e-3, 60.0e-3, 65.0e-3, 70.0e-3])
# conversion from rad to deg
neg_slope_vec = rad_vec_to_deg(neg_slope_vec)
pos_slope_vec = rad_vec_to_deg(pos_slope_vec)
# negative slope
neg_slope_mat = np.array([
[ 1.35896 , 2.4456 , 2.11091 , 2.51482 , 3.97435 , 1.75414 , 2.14254 , 2.90571 , 4.52627 , 3.7915 , 4.40631 ],
[ 4.52751 , 3.67731 , 3.09698 , 3.02055 , 2.10587 , 2.32269 , 3.04498 , 3.06816 , 3.21347 , 4.16128 , 2.70367 ],
[ 5.181 , 4.71414 , 4.66475 , 3.47767 , 2.82121 , 5.32739 , 2.71108 , 5.23124 , 2.60278 , 5.62191 , 5.75012 ],
[ 4.27866 , 3.83721 , 5.72057 , 2.8445 , 5.36146 , 4.6628 , 4.03056 , 4.53921 , 4.95741 , 5.78801 , 5.75566 ],
[ 6.84708 , 8.35663 , 6.48315 , 6.72785 , 5.14582 , 2.35935 , 4.66139 , 5.04525 , 5.50799 , 6.62558 , 6.34797 ],
[ 8.38612 , 6.59492 , 9.14252 , 4.03452 , 4.34544 , 6.74915 , 6.08969 , 7.05868 , 8.98642 , 8.43713 , 6.88055 ],
[ 10 , 10 , 9.07075 , 5.29608 , 5.31831 , 8.80791 , 8.77964 , 8.73027 , 9.33014 , 9.67424 , 7.28774 ],
[ 10 , 10 , 8.21653 , 10 , 6.58749 , 7.85045 , 8.70587 , 8.37029 , 9.95257 , 9.04094 , 8.99837 ],
[ 8 , 10 , 10 , 9.33798 , 10 , 9.88393 , 7.49486 , 10 , 10 , 10 , 8.45983 ],
[ 10 , 10 , 10 , 10 , 7.53421 , 9.96176 , 7.51119 , 8.34261 , 10 , 10 , 10 ],
[ 10 , 10 , 10 , 10 , 10 , 8.25679 , 9.62482 , 10 , 10 , 10 , 10 ],
[ 10 , 10 , 10 , 10 , 8.73275 , 9.97755 , 10 , 10 , 10 , 10 , 10 ],
[ 10 , 10 , 10 , 10 , 10 , 10 , 10 , 10 , 10 , 10 , 10 ],
[ 10 , 10 , 10 , 10 , 10 , 8.96161 , 10 , 10 , 10 , 10 , 8.23436 ],
[ 6.65131 , 10 , 10 , 8.35228 , 8.44875 , 10 , 10 , 10 , 10 , 10 , 10 ]
])
# positive slope
pos_slope_mat = np.array([
[ 0 , 0 , 0.00386807 , 0 , 0.0207818 , 0.0433482 , 0.0946459 , 0.179159 , 0.202089 , 0.403508 , 0.464195 ],
[ 0 , 0 , 0.00888487 , 0.0159661 , 0.0788571 , 0.040276 , 0.161383 , 0.195497 , 0.327714 , 0.461673 , 0.743087 ],
[ 0 , 0 , 0.0283778 , 0.0223558 , 0.0325022 , 0.0966485 , 0.127124 , 0.294648 , 0.437531 , 0.49413 , 1.22887 ],
[ 0 , 0 , 0.0251356 , 0.0366941 , 0.0778504 , 0.180349 , 0.151725 , 0.64994 , 0.590218 , 1.06267 , 1.98199 ],
[ 0 , 0 , 0.0310347 , 0.076894 , 0.129517 , 0.295583 , 0.365443 , 0.399327 , 1.06421 , 1.7638 , 4.99483 ],
[ 0.00178405 , 0.00516504 , 0.0516908 , 0.053578 , 0.204719 , 0.248474 , 0.339068 , 0.92077 , 1.24166 , 6.96147 , 10 ],
[ 0.00892214 , 0.051077 , 0.0769846 , 0.161803 , 0.177436 , 0.515228 , 0.78249 , 1.63398 , 4.28592 , 10 , 10 ],
[ 0.0109476 , 0.0177357 , 0.110165 , 0.325263 , 0.359242 , 0.53532 , 1.31449 , 3.91586 , 10 , 8.103 , 10 ],
[ 0.026632 , 0.169398 , 0.209529 , 0.38547 , 0.529407 , 1.2623 , 2.91883 , 10 , 10 , 10 , 10 ],
[ 0.0234668 , 0.146083 , 0.393477 , 0.581085 , 0.88234 , 4.23401 , 10 , 10 , 10 , 10 , 10 ],
[ 0.131305 , 0.335067 , 1.00825 , 0.992989 , 3.27954 , 7.67062 , 10 , 10 , 10 , 10 , 10 ],
[ 0.0829176 , 0.67379 , 0.939541 , 2.56654 , 6.6016 , 10 , 10 , 10 , 10 , 10 , 10 ],
[ 0.390535 , 1.37979 , 2.67621 , 7.65626 , 10 , 9.38528 , 10 , 10 , 10 , 10 , 10 ],
[ 3.11831 , 7.44358 , 10 , 8.39106 , 10 , 10 , 10 , 10 , 10 , 10 , 10 ],
[ 6.65131 , 10 , 10 , 8.35228 , 8.44875 , 10 , 10 , 10 , 10 , 10 , 10 ]
])
# output folder
output_folder = '../graphs'
# 1 to save the figures, 0 otherwise
save_fig = 0
# negative slope
fig = plt.figure()
ax = plt.gca()
im_l = ax.matshow(neg_slope_mat, cmap='YlOrRd', interpolation='none', vmin=0, vmax=10)
cbar = plt.colorbar(im_l, ticks=[0.0, 2.0, 4.0, 6.0, 8.0, 10.0])
cbar.ax.tick_params(axis='y', labelsize=15, direction='out', width=1.5)
ax.set_xticks((0, 2, 4, 6, 8, 10))
ax.set_yticks((14, 12, 10, 8, 6, 4, 2, 0))
ax.set_xticklabels(speed_ref[0:11:2], fontsize=12)
ax.set_yticklabels(neg_slope_vec[0:15:2], fontsize=12)
ax.xaxis.tick_bottom()
ax.set_xlabel("speed reference (m/s)" , fontsize=18, fontweight='bold')
ax.set_ylabel("slope angle (deg)", fontsize=18, fontweight='bold')
# label space
ax.xaxis.labelpad = 10
ax.yaxis.labelpad = 10
# only show ticks on left and bottom spines
ax.yaxis.set_ticks_position('left')
ax.xaxis.set_ticks_position('bottom')
# ticks parameters
plt.tick_params(axis='both', which='major', labelsize=15, direction='out', width=2)
if save_fig:
plt.savefig('{}/neg_slope.eps'.format(output_folder), bbox_inches='tight', pad_inches=0.25)
# positive slope
fig = plt.figure()
ax = plt.gca()
im_l = ax.matshow(pos_slope_mat, cmap='YlOrRd', interpolation='none')
cbar = plt.colorbar(im_l, ticks=[0.0, 2.0, 4.0, 6.0, 8.0, 10.0])
cbar.ax.tick_params(axis='y', labelsize=15, direction='out', width=1.5)
ax.set_xticks((0, 2, 4, 6, 8, 10))
ax.set_yticks((14, 12, 10, 8, 6, 4, 2, 0))
ax.set_xticklabels(speed_ref[0:11:2], fontsize=12)
ax.set_yticklabels(pos_slope_vec[0:15:2], fontsize=12)
ax.xaxis.tick_bottom()
ax.set_xlabel("speed reference (m/s)" , fontsize=18, fontweight='bold')
ax.set_ylabel("slope angle (deg)", fontsize=18, fontweight='bold')
# label space
ax.xaxis.labelpad = 10
ax.yaxis.labelpad = 10
# only show ticks on left and bottom spines
ax.yaxis.set_ticks_position('left')
ax.xaxis.set_ticks_position('bottom')
# ticks parameters
plt.tick_params(axis='both', which='major', labelsize=15, direction='out', width=2)
if save_fig:
plt.savefig('{}/pos_slope.eps'.format(output_folder), bbox_inches='tight', pad_inches=0.25)
# show graphs
plt.show()
This diff is collapsed.
This diff is collapsed.
# This file analyses the speed tracking of COMAN when
# a speed reference is provided (and modulated).
# import libraries
import numpy as np
from matplotlib import pyplot as plt
plt.switch_backend('Qt4Agg')
# compute the running average (period = t_av) of a vector x, evolving with time t
def running_average(t, x, t_av):
# vector length
vec_len = len(t)
# safety
if vec_len != len(x):
print("error: vector sizes do not match !")
return
# total time
diff_t = t[-1] - t[0]
# semi length of the running average
av_semi_len = int(0.5 * (t_av / diff_t) * vec_len)
# total length of the running average
av_len = 2 * av_semi_len + 1
# cumulative sum
cum_sum = np.cumsum(x, dtype=float)
# output vector
y = np.zeros((vec_len))
# loop on all elements, except borders
for i in range(av_semi_len, vec_len - av_semi_len):
y[i] = float(cum_sum[i+av_semi_len] - cum_sum[i-av_semi_len] + x[i-av_semi_len]) / float(av_len)
# initial border
for i in range(0, av_semi_len):
y[i] = float(cum_sum[2*i]) / float(2*i+1)
# final border
for i in range(vec_len - av_semi_len, vec_len):
i_comp = vec_len - 1 - i
y[i] = float(cum_sum[i+i_comp] - cum_sum[i-i_comp] + x[i-i_comp]) / float(2*i_comp+1)
# return result
return y
# linear interpolation between (t_0, y_0) and (t_end, y_end) at time 't'
def linear_interpol(y_0, y_end, t_0, t_end, t):
if (t < t_0) :
return y_0
elif (t < t_end) :
return y_0 + (t - t_0) / (t_end - t_0) * (y_end - y_0)
else :
return y_end
# get the speed reference
def get_sp_ref(t):
# vector length
vec_len = len(t)
# output vector
y = np.zeros((vec_len))
for i in range(vec_len):
cur_t = t[i]
if (cur_t < 7.0) :
y[i] = 0.65
elif (cur_t < 8.0) :
y[i] = linear_interpol(0.65, 0.9, 7.0, 8.0, cur_t)
elif (cur_t < 13.0) :
y[i] = 0.9
elif (cur_t < 13.5) :
y[i] = linear_interpol(0.9, 0.4, 13.0, 13.5, cur_t)
elif (cur_t < 20.0) :
y[i] = 0.4
elif (cur_t < 20.5) :
y[i] = linear_interpol(0.4, 0.9, 20.0, 20.5, cur_t)
elif (cur_t < 26.0) :
y[i] = 0.9
elif (cur_t < 26.5) :
y[i] = linear_interpol(0.9, 0.525, 26.0, 26.5, cur_t)
elif (cur_t < 32.0) :
y[i] = 0.525
elif (cur_t < 32.5) :
y[i] = linear_interpol(0.525, 0.775, 32.0, 32.5, cur_t)
elif (cur_t < 39.0) :
y[i] = 0.775
elif (cur_t < 39.5) :
y[i] = linear_interpol(0.775, 0.65, 39.0, 39.5, cur_t)
else :
y[i] = 0.65
return y
# get data
data_real = np.loadtxt('real_speed.res')
# time
t = data_real[:,0]
one_sec_nb_meas = int(len(t) / (t[-1] - t[0]))
# real speed
real_sp = data_real[:,1]
real_av = running_average(t, real_sp, 1.0)
# target speed
targ_sp = get_sp_ref(t)
# movie properties
movie_fqc = 60
movie_time = 55
# output folder
output_folder = '../graphs'
# 1 to save the figure, 0 otherwise
save_fig = 0
# figure
fig = plt.figure(figsize=(10,5))
ax = plt.gca()
plt.plot(t, targ_sp, 'b',linestyle='--', dashes=(12, 3), lw=3.5, label='target speed')
plt.plot(t, real_av, 'r', lw=3.5, label='actual speed')
plt.xlabel("time (s)", fontsize=15, fontweight='bold')
plt.ylabel("speed (m/s)", fontsize=15, fontweight='bold')
plt.xlim([0, 50.5])
plt.ylim([-0.1, 1.01])
# space for labels
ax.xaxis.labelpad = 10
ax.yaxis.labelpad = 15
# hide right and top spines
ax.spines['right'].set_visible(False)
ax.spines['top'].set_visible(False)
# spines width
ax.spines['left'].set_linewidth(2.0)
ax.spines['bottom'].set_linewidth(2.0)
# only show ticks on left and bottom spines
ax.yaxis.set_ticks_position('left')
ax.xaxis.set_ticks_position('bottom')
# ticks parameters
plt.tick_params(axis='both', which='major', labelsize=15, direction='out', width=2)
# legend
plt.legend(loc='lower right', frameon=False, fontsize=20)
# screenshots time
plt.plot([18.8, 22.0], [0.27, 0.27], 'black', lw=3)
plt.plot([18.8, 18.8], [0.25, 0.29], 'black', lw=3)
plt.plot([22.0, 22.0], [0.25, 0.29], 'black', lw=3)
ax.text((22.0+18.8)/2.0, 0.18, 'snapshots', horizontalalignment='center', verticalalignment='center', fontsize=20, color='black')
if save_fig:
plt.savefig('{}/sp_track.eps'.format(output_folder), bbox_inches='tight', pad_inches=0.0)
plt.show()
This diff is collapsed.
# This file is used by both "straight_temporal.py.
# The class implemented in this file provides fields to stock the data
# and different methods to analyze them.
# import libraries
import numpy as np
import math as mt
from matplotlib