def plot(self,
ax,
save_location,
step=-1,
parameters=None,
c='b',
linestyle=None,
label=None,
title=None):
'''
Plots the parameters from save_location on the ax object
Returns the ax object and the current max x, y and z limits
PARAMETERS
----------
ax: ax object for plotting
save_location: string
points to the location in the hdf5 database to read from
parameters: None
This is not used in this function, but has been keep consistent
with the class
step: int, Optional (Default: -1)
the position in the data list to plot to, when -1 is used the
entire dataset will be plotted
c: string, Optional (Default: None)
matplotlib compatible color to be used when plotting data
linestyle: string, Optional (Default: None)
matplotlib compatible linestyle to be used when plotting data
label: string, Optional (Default: None)
the legend label for the data
title: string, Optional (Default: None)
the title of the ax object
'''
if save_location not in self.data:
# create a dictionary with our test data interpolated to the same
# number of steps
self.data[save_location] = proc.load_and_process(
db_name=self.db_name,
save_location=save_location,
interpolated_samples=self.interpolated_samples,
parameters=['time', 'q'])
# get our joint and link positions
[joints, links, ee_xyz
] = proc.calc_cartesian_points(robot_config=self.robot_config,
q=self.data[save_location]['q'])
self.data[save_location]['joints_xyz'] = joints
self.data[save_location]['links_xyz'] = links
self.data[save_location]['ee_xyz'] = ee_xyz
data = self.data[save_location]
# plot our arm figure
vis.plot_arm(ax=ax,
joints_xyz=data['joints_xyz'][step],
links_xyz=data['links_xyz'][step],
ee_xyz=data['ee_xyz'][step],
title=title)
return ax, [self.xlimit, self.ylimit, self.zlimit]
def load_and_process(interpolated_samples, parameters):
dat = DataHandler("tests")
loc = "fake_trajectory"
steps = 147
fake_traj_data = random_trajectories.generate(steps=steps, plot=False)
dat.save(data=fake_traj_data, save_location="fake_trajectory", overwrite=True)
data = proc.load_and_process(
db_name="tests",
save_location=loc,
parameters=parameters,
interpolated_samples=interpolated_samples,
)
if interpolated_samples is None:
interpolated_samples = steps
for key in parameters:
if key == "time":
key = "cumulative_time"
assert len(data[key]) == interpolated_samples
def plot(self,
ax,
save_location,
parameters,
step=-1,
c=None,
linestyle='--',
label=None,
title=None):
'''
Plots the parameters from save_location on the ax object
Returns the ax object and the current max x and y limits
PARAMETERS
----------
ax: ax object for plotting
save_location: string
points to the location in the hdf5 database to read from
parameters: string or list of strings
The parameters to load from the save location, can be a single
parameter, or a list of parameters
step: int, Optional (Default: -1)
the position in the data list to plot to, when -1 is used the
entire dataset will be plotted
c: string, Optional (Default: None)
matplotlib compatible color to be used when plotting data
linestyle: string, Optional (Default: None)
matplotlib compatible linestyle to be used when plotting data
label: string, Optional (Default: None)
the legend label for the data
title: string, Optional (Default: None)
the title of the ax object
'''
# convert our parameters to lists if they are not
parameters = self.make_list(parameters)
ax = self.make_list(ax)
# check if the specific save location and parameter set have been
# processed already to avoid reprocessing if plotting the same data at
# a different step or onto a different axis
save_name = '%s-%s' % (save_location, parameters)
if save_name not in self.data:
self.data[save_name] = proc.load_and_process(
db_name=self.db_name,
save_location=save_location,
parameters=parameters,
interpolated_samples=self.interpolated_samples)
for param in parameters:
# remove single dimensions
self.data[save_name][param] = np.squeeze(
self.data[save_name][param])
# avoid passing time in for finding y limits
if param is not 'time' and param is not 'cumulative_time':
# update our x and y limits with every test we add
self.check_plot_limits(x=np.cumsum(
self.data[save_name]['cumulative_time']),
y=self.data[save_name][param])
ax = vis.plot_2d_data(
ax=ax,
x=self.data[save_name]['cumulative_time'][:step],
y=self.data[save_name][param][:step],
c=c,
linestyle=linestyle,
label=label,
title=title)
elif len(parameters) == 1 and parameters[0] == 'time':
ax = vis.plot_2d_data(
ax=ax,
y=self.data[save_name]['time'][:step],
c=c,
linestyle=linestyle,
title=title,
label='%s: %.2fms' %
(label, 1000 * np.mean(self.data[save_name]['time'])))
# ax.set_xlim(self.xlimit[0], self.xlimit[1])
# ax.set_ylim(self.ylimit[0], self.ylimit[1])
return [ax, [self.xlimit, self.ylimit]]
def calculate_error(self, save_location, ideal=None):
'''
loads the ee_xyz data from save_location and compares it to ideal. If
ideal is not passed in, it is assuemed that a filtered path planner is
saved in save_location under the key 'ideal_trajectory' and will be
used as the reference for the error calculation. The data is loaded,
interpolated, and differentiated. The two norm error is returned.
the following dict is returned
data = {
'ee_xyz': list of end-effector positions (n_timesteps, xyz),
'ideal_trajectory': list of path planner positions
shape of (n_timesteps, xyz)
'time': list of timesteps (n_timesteps),
'time_derivative': int, the order of differentiation applied,
'read_location': string, the location the raw data was loaded from,
'error': the two-norm error between the end-effector trajectory and
the path planner followed that run
PARAMETERS
----------
save_location: string
location of data in database
ideal: string, Optional (Default: None)
This tells the function what trajectory to calculate the error
relative to
None: use the saved filter data at save_location
if string: key of Nx3 data in database to use
'''
if ideal is None:
ideal = 'ideal_trajectory'
parameters = ['ee_xyz', 'time', ideal]
# load and interpolate data
data = proc.load_and_process(
db_name=self.db_name,
save_location=save_location,
parameters=parameters,
interpolated_samples=self.interpolated_samples)
if ideal == 'ideal_trajectory':
data['ideal_trajectory'] = data['ideal_trajectory'][:, :3]
dt = np.sum(data['time']) / len(data['time'])
# integrate data
if self.time_derivative > 0:
# set our keys that are able to be differentiated to avoid errors
differentiable_keys = ['ee_xyz', 'ideal_trajectory']
if ideal is not None:
differentiable_keys.append(ideal)
for key in data:
if key in differentiable_keys:
# differentiate the number of times specified by
# time_derivative
for _ in range(0, self.time_derivative):
data[key][:, 0] = np.gradient(data[key][:, 0], dt)
data[key][:, 1] = np.gradient(data[key][:, 1], dt)
data[key][:, 2] = np.gradient(data[key][:, 2], dt)
data['time_derivative'] = self.time_derivative
data['read_location'] = save_location
data['error'] = np.linalg.norm((data['ee_xyz'] - data[ideal]), axis=1)
return data
def plot(
self,
ax,
save_location,
parameters,
step=-1,
c="tab:purple",
linestyle="--",
label=None,
title=None,
):
"""
Plots the parameters from save_location on the ax object
Returns the ax object and the current max x, y and z limits
PARAMETERS
----------
ax: ax object for plotting
save_location: string
points to the location in the hdf5 database to read from
parameters: string or list of strings
The parameters to load from the save location, can be a single
parameter, or a list of parameters
step: int, Optional (Default: -1)
the position in the data list to plot to, when -1 is used the
entire dataset will be plotted
c: string, Optional (Default: None)
matplotlib compatible color to be used when plotting data
linestyle: string, Optional (Default: None)
matplotlib compatible linestyle to be used when plotting data
label: string, Optional (Default: None)
the legend label for the data
title: string, Optional (Default: None)
the title of the ax object
"""
# convert our parameters to lists if they are not
parameters = self.make_list(parameters)
ax = self.make_list(ax)
save_name = "%s-%s" % (save_location, parameters)
if save_name not in self.data:
self.data[save_name] = proc.load_and_process(
db_name=self.db_name,
save_location=save_location,
parameters=parameters,
interpolated_samples=self.interpolated_samples,
)
for param in parameters:
# remove single dimensions
self.data[save_name][param] = np.squeeze(
self.data[save_name][param])
# avoid passing time in for finding y limits
if param != "time" or param != "cumulative_time":
# update our xyz limit with every test we add
self.check_plot_limits(
x=self.data[save_name][param][:, 0],
y=self.data[save_name][param][:, 1],
z=self.data[save_name][param][:, 2],
)
ax = vis.plot_3d_data(
ax=ax,
data=self.data[save_name][param][:step],
c=c,
linestyle=linestyle,
label=label,
title=title,
)
# ax.set_xlim(self.xlimit[0], self.xlimit[1])
# ax.set_ylim(self.ylimit[0], self.ylimit[1])
# ax.set_zlim(self.zlimit[0], self.zlimit[1])
return [ax, [self.xlimit, self.ylimit, self.zlimit]]
