def plot_invariants(V, P, nlp):
[nlp_outputs, nlp_output_fun] = nlp.output_components
outputs_opt = nlp_outputs(nlp_output_fun(V, P))
first_tether_indeces = np.array(outputs_opt.f['outputs', 0, 0,
'tether_length'])
number_of_constraints = first_tether_indeces.shape[0]
number_tethers = int(np.ceil(np.float(number_of_constraints) / 2))
plt.figure(3).clear()
fig, axes = plt.subplots(nrows=number_of_constraints,
ncols=1,
sharex='all',
num=3)
for idx in range(number_of_constraints):
cstr_name = outputs_opt.getCanonicalIndex(
first_tether_indeces[idx])[-2]
cstr_vec = np.abs(
np.array(
struct_op.coll_slice_to_vec(outputs_opt['outputs', :, :,
'tether_length',
cstr_name])))
# axes[idx].semilogy(tgrid_xa, cstr_vec)
axes[idx].plot(cstr_vec)
axes[idx].set_ylabel(cstr_name)
if idx == 0:
axes[idx].set_title('tether length constraints')
return None
def merge_output_values(output_vals, output_type, output_name, dim, plot_dict, cosmetics):
# read in inputs
discretization = plot_dict['discretization']
if discretization == 'direct_collocation':
scheme = plot_dict['options']['nlp']['collocation']['scheme']
tgrid_coll = plot_dict['time_grids']['coll']
# total time points
tgrid_u_coll = plot_dict['time_grids']['x_coll'][:-1]
# interval time points
tgrid_u = plot_dict['time_grids']['u']
if discretization == 'multiple_shooting':
# take interval values
output_values = np.array(cas.vertcat(*output_vals['outputs',:,output_type,output_name,dim]).full())
tgrid = tgrid_u
ndim = output_vals['outputs',0,output_type,output_name].shape[0]
elif discretization == 'direct_collocation':
if scheme != 'radau':
output_values = []
# merge interval and node values
for k in range(plot_dict['n_k']):
# add interval values
output_values = cas.vertcat(output_values, output_vals['outputs',k, output_type, output_name,dim])
if cosmetics['plot_coll']:
# add node values
output_values = cas.vertcat(output_values, cas.vertcat(*output_vals['coll_outputs',k, :, output_type, output_name,dim]))
if cosmetics['plot_coll']:
tgrid = tgrid_u_coll
else:
tgrid = tgrid_u
ndim = output_vals['outputs',0,output_type,output_name].shape[0]
else:
if cosmetics['plot_coll']:
# add only node values for radau case
output_values = np.array(struct_op.coll_slice_to_vec(output_vals['coll_outputs',:,:,output_type,output_name,dim]))
tgrid = tgrid_coll
ndim = output_vals['coll_outputs',0,0,output_type,output_name].shape[0]
else:
output_values = []
tgrid = []
ndim = 1
# make list of time grid and values
tgrid = list(chain.from_iterable(tgrid.full().tolist()))
output_values = list(chain.from_iterable(output_values))
return output_values, tgrid, ndim
def merge_xd_values(V, name, dim, plot_dict, cosmetics):
# read in inputs
discretization = plot_dict['discretization']
if discretization == 'direct_collocation':
scheme = plot_dict['options']['nlp']['collocation']['scheme']
tgrid_coll = plot_dict['time_grids']['coll']
# total time points
tgrid_x_coll = plot_dict['time_grids']['x_coll']
# interval time points
tgrid_x = plot_dict['time_grids']['x']
if discretization == 'multiple_shooting':
# take interval values
xd_values = np.array(cas.vertcat(*V['xd', :, name, dim]).full())
tgrid = tgrid_x
elif discretization == 'direct_collocation':
if scheme != 'radau':
xd_values = []
# merge interval and node values
for k in range(plot_dict['n_k'] + 1):
# add interval values
xd_values = cas.vertcat(xd_values, V['xd', k, name, dim])
if (cosmetics['plot_coll'] and k < plot_dict['n_k']):
# add node values
xd_values = cas.vertcat(
xd_values,
cas.vertcat(*V['coll_var', k, :, 'xd', name,
dim]).full())
if cosmetics['plot_coll']:
tgrid = tgrid_x_coll
else:
tgrid = tgrid_x
elif scheme == 'radau':
if cosmetics['plot_coll']:
# add node values
xd_values = np.array(
struct_op.coll_slice_to_vec(V['coll_var', :, :, 'xd', name,
dim]))
tgrid = tgrid_coll
else:
xd_values = []
tgrid = []
# make list of time grid and values
tgrid = list(chain.from_iterable(tgrid.full().tolist()))
xd_values = list(chain.from_iterable(xd_values))
return xd_values, tgrid
def plot_trajectory(V):
xvals = struct_op.coll_slice_to_vec(V['xd', :, :, 'q21',
0]).full().flatten()
yvals = struct_op.coll_slice_to_vec(V['xd', :, :, 'q21',
1]).full().flatten()
zvals = struct_op.coll_slice_to_vec(V['xd', :, :, 'q21',
2]).full().flatten()
fig = plt.figure(1)
ax = fig.add_subplot(111, projection='3d')
ax.plot(xvals, yvals, zs=zvals)
xvals = struct_op.coll_slice_to_vec(V['xd', :, :, 'q31',
0]).full().flatten()
yvals = struct_op.coll_slice_to_vec(V['xd', :, :, 'q31',
1]).full().flatten()
zvals = struct_op.coll_slice_to_vec(V['xd', :, :, 'q31',
2]).full().flatten()
ax.plot(xvals, yvals, zs=zvals)
return None