def plot_q_or_v_or_u(robot_output,
key,
x_names,
x_slice,
time_slice,
ylabel=None,
title=None):
ps = plot_styler.PlotStyler()
if ylabel is None:
ylabel = key
if title is None:
title = key
plotting_utils.make_plot(
robot_output, # data dict
't_x', # time channel
time_slice,
[key], # key to plot
{key: x_slice}, # slice of key to plot
{key: x_names}, # legend entries
{
'xlabel': 'Time',
'ylabel': ylabel,
'title': title
},
ps)
return ps
def plot_qp_solve_time(osc_debug, time_slice):
ps = plot_styler.PlotStyler()
plotting_utils.make_plot(osc_debug, 't_osc', time_slice, ['qp_solve_time'],
{}, {}, {
'xlabel': 'Timestamp',
'ylabel': 'Solve Time ',
'title': 'OSC QP Solve Time'
}, ps)
return ps
def plot_qp_costs(osc_debug, time_slice):
cost_keys = ['input_cost', 'acceleration_cost', 'soft_constraint_cost']
ps = plot_styler.PlotStyler()
plotting_utils.make_plot(osc_debug, 't_osc', time_slice, cost_keys, {},
{key: [key]
for key in cost_keys}, {
'xlabel': 'Time',
'ylabel': 'Cost',
'title': 'OSC QP Costs'
}, ps)
return ps
def plot_general_osc_tracking_data(traj_name, deriv, dim, data, time_slice):
ps = plot_styler.PlotStyler()
keys = [key for key in data.keys() if key != 't']
plotting_utils.make_plot(data, 't', time_slice, keys, {},
{key: [key]
for key in keys},
{
'xlabel': 'Time',
'ylabel': '',
'title': f'{traj_name} {deriv} tracking {dim}'
}, ps)
return ps
def plot_epsilon_sol(osc_debug, time_slice, epsilon_slice):
ps = plot_styler.PlotStyler()
plotting_utils.make_plot(
osc_debug, 't_osc', time_slice, ['epsilon_sol'],
{'epsilon_sol': epsilon_slice}, {
'epsilon_sol': [
'epsilon_sol' + i
for i in plotting_utils.slice_to_string_list(epsilon_slice)
]
}, {
'xlabel': 'time',
'ylabel': 'epsilon',
'title': 'OSC soft constraint epsilon sol'
}, ps)
return ps
def plot_lambda_c_sol(osc_debug, time_slice, lambda_slice):
ps = plot_styler.PlotStyler()
plotting_utils.make_plot(
osc_debug, 't_osc', time_slice, ['lambda_c_sol'],
{'lambda_c_sol': lambda_slice}, {
'lambda_c_sol': [
'lambda_c_' + i
for i in plotting_utils.slice_to_string_list(lambda_slice)
]
}, {
'xlabel': 'time',
'ylabel': 'lambda',
'title': 'OSC contact force solution'
}, ps)
return ps
def plot_tracking_costs(osc_debug, time_slice):
ps = plot_styler.PlotStyler()
data_dict = \
{key: val for key, val in osc_debug['tracking_cost'].items()}
data_dict['t_osc'] = osc_debug['t_osc']
plotting_utils.make_plot(
data_dict, 't_osc', time_slice, osc_debug['tracking_cost'].keys(), {},
{key: [key]
for key in osc_debug['tracking_cost'].keys()}, {
'xlabel': 'Time',
'ylabel': 'Cost',
'title': 'tracking_costs'
}, ps)
return ps
def plot_points_positions(robot_output, time_slice, plant, context,
frame_names, pts, dims):
dim_map = ['_x', '_y', '_z']
data_dict = {'t': robot_output['t_x']}
legend_entries = {}
for name in frame_names:
frame = plant.GetBodyByName(name).body_frame()
pt = pts[name]
data_dict[name] = make_point_positions_from_q(robot_output['q'], plant,
context, frame, pt)
legend_entries[name] = [name + dim_map[dim] for dim in dims[name]]
ps = plot_styler.PlotStyler()
plotting_utils.make_plot(data_dict, 't', time_slice, frame_names, dims,
legend_entries, {
'title': 'Point Positions',
'xlabel': 'time (s)',
'ylabel': 'pos (m)'
}, ps)
return ps