Skip to content
This repository has been archived by the owner on Feb 6, 2023. It is now read-only.

Commit

Permalink
Merge pull request #52 from uuvsimulator/feature/smac_decomposition
Browse files Browse the repository at this point in the history 
Feature/smac decomposition
  • Loading branch information
Musa Morena Marcusso Manhães authored Sep 27, 2018
2 parents ab8529a + 26450d9 commit 9e58bf3
Show file tree
Hide file tree
Showing 28 changed files with 2,854 additions and 1,487 deletions.
413 changes: 281 additions & 132 deletions uuv_simulation_evaluation/scripts/run_best_worst_comparison
View file

Large diffs are not rendered by default.

276 changes: 156 additions & 120 deletions uuv_simulation_evaluation/src/uuv_bag_evaluation/data_parsers/auv_command_data.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
import numpy as np
import os
import matplotlib.pyplot as plt
from simulation_data import SimulationData
from simulation_data import SimulationData, COLOR_RED, COLOR_GREEN, COLOR_BLUE


class AUVCommandData(SimulationData):
Expand All @@ -26,18 +26,6 @@ class AUVCommandData(SimulationData):
def __init__(self, bag):
super(AUVCommandData, self).__init__()

self._plot_configs = dict(auv_command=dict(
figsize=[12, 5],
linewidth=2,
label_fontsize=30,
xlim=None,
ylim=None,
zlim=None,
tick_labelsize=25,
labelpad=10,
legend=dict(loc='upper right',
fontsize=20)))

for x in bag.get_type_and_topic_info():
for k in x:
if 'uuv_auv_control_allocator/AUVCommand' in x[k][0]:
Expand All @@ -60,118 +48,166 @@ def __init__(self, bag):
self._recorded_data['surge_speed'].append(float(msg.surge_speed))
self._logger.info('%s=loaded' % self._topic_name)
except Exception as e:
self._logger.error('Error reading AUV command input topic, message=' + str(e))
self._logger.warning('Error reading AUV command input topic, message=' + str(e))

def get_as_dataframe(self, add_group_name=None):
try:
import pandas

if len(self._recorded_data['force']) == 0:
return None

data = dict()
data[self.LABEL + '_time'] = self._time
data[self.LABEL + '_force_x'] = [x[0] for x in self._recorded_data['force']]
data[self.LABEL + '_force_y'] = [x[1] for x in self._recorded_data['force']]
data[self.LABEL + '_force_z'] = [x[2] for x in self._recorded_data['force']]

data[self.LABEL + '_torque_x'] = [x[0] for x in self._recorded_data['torque']]
data[self.LABEL + '_torque_y'] = [x[1] for x in self._recorded_data['torque']]
data[self.LABEL + '_torque_z'] = [x[2] for x in self._recorded_data['torque']]

data[self.LABEL + '_surge_speed'] = self._recorded_data['surge_speed']

if add_group_name is not None:
data['group'] = [add_group_name for _ in range(len(self._time))]

return pandas.DataFrame(data)

except Exception as ex:
self._logger.error('Error while exporting as pandas.DataFrame, message=' + str(ex))
return None

def plot(self, output_dir):
if not os.path.isdir(output_dir):
self._logger.error('Invalid output directory, dir=' + str(output_dir))
raise Exception('Invalid output directory')
fig_tm, ax_tm = plt.subplots(
3, 1,
figsize=(self._plot_configs['auv_command']['figsize'][0],
3 * self._plot_configs['auv_command']['figsize'][1]))

try:
##############################################################################
# Plot AUV input command
##############################################################################
self._logger.info('Plotting the input wrench of the thruster manager')

min_y = 0
max_y = 0

ax_tm[0].plot(self._time, [f[0] for f in self._recorded_data['force']],
linewidth=self._plot_configs['auv_command']['linewidth'],
label=r'$X$')
min_y = min(min_y, np.min([f[0] for f in self._recorded_data['force']]))
max_y = max(max_y, np.max([f[0] for f in self._recorded_data['force']]))
ax_tm[0].plot(self._time, [f[1] for f in self._recorded_data['force']],
linewidth=self._plot_configs['auv_command']['linewidth'],
label=r'$Y$')
min_y = min(min_y, np.min([f[1] for f in self._recorded_data['force']]))
max_y = max(max_y, np.max([f[1] for f in self._recorded_data['force']]))
ax_tm[0].plot(self._time, [f[2] for f in self._recorded_data['force']],
linewidth=self._plot_configs['auv_command']['linewidth'],
label=r'$Z$')
min_y = min(min_y, np.min([f[2] for f in self._recorded_data['force']]))
max_y = max(max_y, np.max([f[2] for f in self._recorded_data['force']]))

ax_tm[0].set_xlabel(r'Time [s]',
fontsize=self._plot_configs['auv_command']['label_fontsize'])
ax_tm[0].set_ylabel(r'Forces [N]',
fontsize=self._plot_configs['auv_command']['label_fontsize'])
ax_tm[0].tick_params(axis='both',
labelsize=self._plot_configs['auv_command']['tick_labelsize'])
ax_tm[0].grid(True)
ax_tm[0].set_xlim(np.min(self._time), np.max(self._time))
ax_tm[0].set_ylim(min_y, max_y)

ax_tm[0].legend(fancybox=True, framealpha=1,
loc=self._plot_configs['auv_command']['legend']['loc'],
fontsize=self._plot_configs['auv_command']['legend']['fontsize'])

min_y = 0
max_y = 0

ax_tm[1].plot(self._time, [x[0] for x in self._recorded_data['torque']],
linewidth=self._plot_configs['auv_command']['linewidth'],
label=r'$K$')
min_y = min(min_y, np.min([x[0] for x in self._recorded_data['torque']]))
max_y = max(max_y, np.max([x[0] for x in self._recorded_data['torque']]))
ax_tm[1].plot(self._time, [x[1] for x in self._recorded_data['torque']],
linewidth=self._plot_configs['auv_command']['linewidth'],
label=r'$M$')
min_y = min(min_y, np.min([x[1] for x in self._recorded_data['torque']]))
max_y = max(max_y, np.max([x[1] for x in self._recorded_data['torque']]))
ax_tm[1].plot(self._time, [x[2] for x in self._recorded_data['torque']],
linewidth=self._plot_configs['auv_command']['linewidth'],
label=r'$N$')
min_y = min(min_y, np.min([x[2] for x in self._recorded_data['torque']]))
max_y = max(max_y, np.max([x[2] for x in self._recorded_data['torque']]))

ax_tm[1].set_xlabel(r'Time [s]',
fontsize=self._plot_configs['auv_command']['label_fontsize'])
ax_tm[1].set_ylabel(r'Torques [Nm]',
fontsize=self._plot_configs['auv_command']['label_fontsize'])
ax_tm[1].tick_params(axis='both',
labelsize=self._plot_configs['auv_command']['tick_labelsize'])
ax_tm[1].grid(True)
ax_tm[1].set_xlim(np.min(self._time), np.max(self._time))
ax_tm[1].set_ylim(min_y, max_y)

ax_tm[1].legend(fancybox=True, framealpha=1,
loc=self._plot_configs['auv_command']['legend']['loc'],
fontsize=self._plot_configs['auv_command']['legend']['fontsize'])

min_y = 0
max_y = 0

ax_tm[2].plot(self._time, self._recorded_data['surge_speed'],
linewidth=self._plot_configs['auv_command']['linewidth'],
label=r'$U$')
min_y = np.min(self._recorded_data['surge_speed'])
max_y = np.max(self._recorded_data['surge_speed'])

ax_tm[2].set_xlabel(r'Time [s]',
fontsize=self._plot_configs['auv_command']['label_fontsize'])
ax_tm[2].set_ylabel(r'Surge speed [m/s]',
fontsize=self._plot_configs['auv_command']['label_fontsize'])
ax_tm[2].tick_params(axis='both',
labelsize=self._plot_configs['auv_command']['tick_labelsize'])
ax_tm[2].grid(True)
ax_tm[2].set_xlim(np.min(self._time), np.max(self._time))
ax_tm[2].set_ylim(min_y, max_y)

ax_tm[2].legend(fancybox=True, framealpha=1,
loc=self._plot_configs['auv_command']['legend']['loc'],
fontsize=self._plot_configs['auv_command']['legend']['fontsize'])

fig_tm.tight_layout()
output_path = (self._output_dir if output_dir is None else output_dir)
filename = os.path.join(output_path, 'auv_control_input.pdf')
fig_tm.savefig(filename)
plt.close(fig_tm)
del fig_tm
fig_tm = None
if len(self._recorded_data['force']):
##############################################################################
# Plot AUV input command
##############################################################################
fig_tm, ax_tm = plt.subplots(3, 1,
figsize=(self._plot_configs['figsize'][0],
3 * self._plot_configs['figsize'][1]))

self._logger.info('Plotting the input wrench of the thruster manager')

min_y = 0
max_y = 0

ax_tm[0].plot(
self._time,
[f[0] for f in self._recorded_data['force']],
color=COLOR_RED,
linewidth=self._plot_configs['linewidth'],
label=r'$X$')

min_y = min(min_y, np.min([f[0] for f in self._recorded_data['force']]))
max_y = max(max_y, np.max([f[0] for f in self._recorded_data['force']]))

ax_tm[0].plot(
self._time,
[f[1] for f in self._recorded_data['force']],
color=COLOR_GREEN,
linewidth=self._plot_configs['linewidth'],
label=r'$Y$')

min_y = min(min_y, np.min([f[1] for f in self._recorded_data['force']]))
max_y = max(max_y, np.max([f[1] for f in self._recorded_data['force']]))

ax_tm[0].plot(
self._time,
[f[2] for f in self._recorded_data['force']],
color=COLOR_BLUE,
linewidth=self._plot_configs['linewidth'],
label=r'$Z$')

min_y = min(min_y, np.min([f[2] for f in self._recorded_data['force']]))
max_y = max(max_y, np.max([f[2] for f in self._recorded_data['force']]))

ax_tm[0].set_xlim(np.min(self._time), np.max(self._time))
ax_tm[0].set_ylim(min_y, max_y)

self.config_2dplot(
ax=ax_tm[0],
title='',
xlabel=r'Time [s]',
ylabel=r'Forces [N]',
legend_on=True)

min_y = 0
max_y = 0

ax_tm[1].plot(
self._time,
[x[0] for x in self._recorded_data['torque']],
color=COLOR_RED,
linewidth=self._plot_configs['linewidth'],
label=r'$K$')

min_y = min(min_y, np.min([x[0] for x in self._recorded_data['torque']]))
max_y = max(max_y, np.max([x[0] for x in self._recorded_data['torque']]))

ax_tm[1].plot(
self._time,
[x[1] for x in self._recorded_data['torque']],
color=COLOR_GREEN,
linewidth=self._plot_configs['linewidth'],
label=r'$M$')

min_y = min(min_y, np.min([x[1] for x in self._recorded_data['torque']]))
max_y = max(max_y, np.max([x[1] for x in self._recorded_data['torque']]))

ax_tm[1].plot(
self._time,
[x[2] for x in self._recorded_data['torque']],
color=COLOR_BLUE,
linewidth=self._plot_configs['linewidth'],
label=r'$N$')

min_y = min(min_y, np.min([x[2] for x in self._recorded_data['torque']]))
max_y = max(max_y, np.max([x[2] for x in self._recorded_data['torque']]))

ax_tm[1].set_xlim(np.min(self._time), np.max(self._time))
ax_tm[1].set_ylim(min_y, max_y)

self.config_2dplot(
ax=ax_tm[1],
title='',
xlabel=r'Time [s]',
ylabel=r'Torques [Nm]',
legend_on=True)

min_y = 0
max_y = 0

ax_tm[2].plot(self._time, self._recorded_data['surge_speed'],
linewidth=self._plot_configs['linewidth'],
label=r'$U$')
min_y = np.min(self._recorded_data['surge_speed'])
max_y = np.max(self._recorded_data['surge_speed'])

ax_tm[2].set_xlim(np.min(self._time), np.max(self._time))
ax_tm[2].set_ylim(min_y, max_y)

self.config_2dplot(
ax=ax_tm[2],
title='',
xlabel=r'Time [s]',
ylabel=r'Surge speed [m/s]',
legend_on=True)

plt.tight_layout()
output_path = (self._output_dir if output_dir is None else output_dir)
filename = os.path.join(output_path, 'auv_control_input.pdf')
fig_tm.savefig(filename)
plt.close(fig_tm)
del fig_tm
except Exception as e:
self._logger.error('Error plotting AUV command input command wrench, message=' + str(e))
plt.close(fig_tm)
del fig_tm
if fig_tm is not None:
plt.close(fig_tm)
del fig_tm
Loading

0 comments on commit 9e58bf3

Please sign in to comment.