first commit

.gitignore

+
+*.pyc
+*~
+*.m
+*.anim
+*.res
+**/*.m
+__pycache__
+stderr.txt
+stdout.txt
+build
+debug
+doc/html
+doc/latex
+.DS_Store

resultsR/analyze.py

+# 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()

resultsR/cpg_analysis/cpg_analysis.py

+# 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()

resultsR/graphs/Readme.md

+This folder is used to save the graphs generated in the *resultsR* folder.

resultsR/robustness/push_torso.py

+# 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()

resultsR/robustness/rnd_ground.py

+# 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()

resultsR/robustness/slope_ground.py

+# 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()

resultsR/speed_tracking/sp_track.py

+# 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])
+