p_map.sa2xp = model.sa2xp_y_xdot_timedaoa
# p_map.sa2xp = model.sa2xp_y_xdot_aoa
p_map.xp2s = model.xp2s_y_xdot
s_grid_height = np.linspace(0.5, 1.5, 7)
s_grid_velocity = np.linspace(3, 8, 7)
s_grid = (s_grid_height, s_grid_velocity)
a_grid_aoa = np.linspace(00/180*np.pi, 70/180*np.pi, 21)
# a_grid = (a_grid_aoa, )
a_grid_amp = np.linspace(0.9, 1.2, 11)
a_grid = (a_grid_aoa, a_grid_amp)
grids = {'states': s_grid, 'actions': a_grid}
t = TicToc()
t.tic()
Q_map, Q_F, Q_reach = vibly.parcompute_Q_map(grids, p_map, keep_coords=True,
verbose=2)
t.toc()
print("time elapsed: " + str(t.elapsed/60))
Q_V, S_V = vibly.compute_QV(Q_map, grids)
S_M = vibly.project_Q2S(Q_V, grids, proj_opt=np.mean)
Q_M = vibly.map_S2Q(Q_map, S_M, s_grid, Q_V=Q_V)
# plt.scatter(Q_map[1], Q_map[0])
print("non-failing portion of Q: " + str(np.sum(~Q_F)/Q_F.size))
print("viable portion of Q: " + str(np.sum(Q_V)/Q_V.size))
import itertools as it
# Q0 = np.zeros((len(grids['states']), total_gridpoints))
# def create_x0(grids):
# for idx, state_action in enumerate(np.array(list(
# it.product(*grids['states'], *grids['actions'])))):
def compute_viability(x0, p, name, visualise=False):
# * Solve for nominal open-loop limit-cycle
# * Set-up P maps for computations
p_map = model.poincare_map
p_map.p = p
p_map.x = x0.copy()
# * choose high-level represenation
p_map.xp2s = model.xp2s_y_xdot
# * this maps the full simulated state to the high-level representation
# * in this case the relevant states at apex:
# * (y, xdot), in other words the (height, velocity)
p_map.sa2xp = model.sa2xp_y_xdot_timedaoa
# * this maps the high-level representation of state and actions back
# * to the full state and parameters used for the simulation
# p_map.sa2xp = model.sa2xp_amp
# * this representation includes an amplification coefficient `a' for
# * the muscle activation a*f(t). It adds a dimension to the grids,
# * which substantially increases computation time, and also makes
# * visualization much less straightforward (due to the 4-dimensional
# * state-action space).
# * We have tried this out, and a from a preliminary look, there does
# * not seem to be much qualitative difference in the results for the
# * nominal limit-cycle used in the paper. For other conditions, it may
# * be important to include this (or other additional control inputs)
# * in the model.
# * set up grids for computing the viable set and measure
# * a denser grid will yield more precision, but require more compute
s_grid_height = np.linspace(0.05, 0.5, 91)
s_grid_velocity = np.linspace(0, 10.0, 101)
s_grid = (s_grid_height, s_grid_velocity)
a_grid_aoa = np.linspace(0 / 180 * np.pi, 90 / 180 * np.pi, 91)
a_grid = (a_grid_aoa, )
# * if you use the representation `sa2xp_amp` (see above), the
# * action grid also includes an extra dimension
# a_grid_amp = np.linspace(0.75, 1.25, 11)
# a_grid = (a_grid_aoa, a_grid_amp)
grids = {'states': s_grid, 'actions': a_grid}
# * compute transition matrix and boolean matrix of failures
Q_map, Q_F = vibly.parcompute_Q_map(grids, p_map, verbose=1)
# * compute viable sets
Q_V, S_V = vibly.compute_QV(Q_map, grids)
# * compute the measure in state-space
S_M = vibly.project_Q2S(Q_V, grids, proj_opt=np.mean)
# * map the measure to Q-space
Q_M = vibly.map_S2Q(Q_map, S_M, s_grid, Q_V=Q_V)
print("non-failing portion of Q: " + str(np.sum(~Q_F) / Q_F.size))
print("viable portion of Q: " + str(np.sum(Q_V) / Q_V.size))
# * save data
if not os.path.exists(name):
os.makedirs(name)
filename = name + '/' + name + '_' + '{:.4f}'.format(damping)
data2save = {
"grids": grids,
"Q_map": Q_map,
"Q_F": Q_F,
"Q_V": Q_V,
"Q_M": Q_M,
"S_M": S_M,
"p": p,
"x0": x0
}
outfile = open(filename + '.pickle', 'wb')
pickle.dump(data2save, outfile)
outfile.close()
if visualise:
print("SAVING FIGURE")
print(" ")
plt.figure()
plt.imshow(S_M, origin='lower', vmin=0, vmax=1, cmap='viridis')
plt.title('bird ' + name)
plt.savefig(filename + '.pdf', format='pdf')
# plt.show() # to just see it on the fly
plt.close()
# * note, the s_grid is a tuple of grids, such that each dimension can have
# * different resolution, and we do not need to initialize the entire array
s_grid = (np.linspace(-0.0, p['ceiling'], 201), )
# * same thing for the actions
a_grid = (np.linspace(0.0, 0.5, 151), )
# * for convenience, both grids are placed in a dictionary
grids = {'states': s_grid, 'actions': a_grid}
# * compute_Q_map computes a gridded transition map, `Q_map`, which is used as
# * a look-up table for computing viable sets
# * Q_F is a grid marking all failing state-action pairs
# * Q_on_grid is a helper grid, which marks if a state has not moved
# * this is used to catch corner cases, and is not important for most systems
# * setting `check_grid` to False will omit Q_on_grid
Q_map, Q_F, Q_on_grid = vibly.parcompute_Q_map(grids, p_map, check_grid=True)
# * compute_QV computes the viable set and viability kernel
Q_V, S_V = vibly.compute_QV(Q_map, grids, ~Q_F, Q_on_grid=Q_on_grid)
# * project_Q2S takens a projection of the viable set onto state-space
# * for the computing the measure, you can use either `np.mean` or `np.sum`
# * as the projection operator
S_M = vibly.project_Q2S(Q_V, grids, proj_opt=np.mean)
# * map_S2Q maps the measure back into state-action space using the gridded
# * transition map
Q_M = vibly.map_S2Q(Q_map, S_M, s_grid, Q_V=Q_V)
###############################################################################
# * save data as pickle
###############################################################################
p['x0'] = x0
# initialize default x0_daslip
p_map = model.poincare_map
p_map.p = p
p_map.x = x0
p_map.sa2xp = model.sa2xp_y_xdot_timedaoa
p_map.xp2s = model.xp2s_y_xdot
s_grid_height = np.linspace(0.5, 1.5, 26)
s_grid_velocity = np.linspace(1, 6, 26)
s_grid = (s_grid_height, s_grid_velocity)
a_grid = (np.linspace(20/180*np.pi, 60/180*np.pi, 25), )
grids = {'states': s_grid, 'actions': a_grid}
Q_map, Q_F = vibly.parcompute_Q_map(grids, p_map, verbose=2)
Q_V, S_V = vibly.compute_QV(Q_map, grids)
S_M = vibly.project_Q2S(Q_V, grids, proj_opt=np.mean)
Q_M = vibly.map_S2Q(Q_map, S_M, s_grid=s_grid, Q_V=Q_V)
print("non-failing portion of Q: " + str(np.sum(~Q_F)/Q_F.size))
print("viable portion of Q: " + str(np.sum(Q_V)/Q_V.size))
###############################################################################
# save data as pickle
###############################################################################
# import pickle
# filename = 'daslip.pickle'
# data2save = {"grids": grids, "Q_map": Q_map, "Q_F": Q_F, "Q_V": Q_V,
# "Q_M": Q_M, "S_M": S_M, "p": p, "x0": x0}
# outfile = open(filename, 'wb')
# pickle.dump(data2save, outfile)
x0 = np.array([0, 0.85, 5.5, 0, 0, 0, 0])
x0 = slip.reset_leg(x0, p)
p['x0'] = x0
p['total_energy'] = slip.compute_total_energy(x0, p)
p_map = slip.p_map
p_map.p = p
p_map.x = x0
p_map.sa2xp = slip.sa2xp
p_map.xp2s = slip.xp2s
s_grid = np.linspace(0.1, 1, 181)
s_grid = (s_grid[:-1], )
a_grid = (np.linspace(-10 / 180 * np.pi, 70 / 180 * np.pi, 161), )
grids = {'states': s_grid, 'actions': a_grid}
# Q_map, Q_F = vibly.compute_Q_map(grids, p_map)
Q_map, Q_F = vibly.parcompute_Q_map(grids, p_map)
Q_V, S_V = vibly.compute_QV(Q_map, grids)
S_M = vibly.project_Q2S(Q_V, grids, proj_opt=np.mean)
Q_M = vibly.map_S2Q(Q_map, S_M, s_grid, Q_V=Q_V)
###########################################################################
# * save data as pickle
###########################################################################
import pickle
import os
filename = 'slip_map.pickle'
if os.path.exists('data'): # if we are in the vibly root folder:
path_to_file = 'data/dynamics/'
else: # else we assume this is being run from the /demos folder.