from slippy.slip import *
from slippy.viability import compute_Q_map
p = {
'mass': 80.0,
'stiffness': 8200.0,
'resting_length': 1.0,
'gravity': 9.81,
'angle_of_attack': 1 / 5 * np.pi
}
x0 = np.array([0, 0.85, 5.5, 0, 0, 0])
x0 = reset_leg(x0, p)
p['total_energy'] = compute_total_energy(x0, p)
poincare_map.p = p
poincare_map.x = x0
poincare_map.sa2xp = mapSA2xp_height_angle
poincare_map.xp2s = map2s
s_grid = np.linspace(0.1, 1, 50)
s_grid = s_grid[:-1]
a_grid = np.linspace(-10 / 180 * np.pi, 90 / 180 * np.pi, 51)
grids = {'states': (s_grid, ), 'actions': (a_grid, )}
Q_map, Q_F = compute_Q_map(grids, poincare_map)
# save file
# data2save = {"s_grid": s_grid, "a_grid": a_grid, "Q_map": Q_map, "Q_F": Q_F,
# "poincare_map":poincare_map, "p":p}
# np.savez('test.npz', **data2save)
plt.imshow(Q_map, origin='lower')
plt.show()
p_daslip['actuator_resting_length'], 0, 0, 0])
for idx, val in enumerate(x0_slip): # copy over default values from SLIP
x0_daslip[idx] = val
x0_daslip = reset_leg(x0_daslip, p_daslip)
poincare_map.p = p_daslip
poincare_map.x = x0_daslip
poincare_map.sa2xp = mapSA2xp_energy_normalizedheight_aoa
poincare_map.xp2s = map2s_energy_normalizedheight_aoa
s_grid_energy = np.linspace(1500, 2500, 10)
s_grid_normalizedheight = np.linspace(0.4, 1, 10)
s_grid = (s_grid_energy, s_grid_normalizedheight)
a_grid = (np.linspace(0/180*np.pi, 70/180*np.pi, 15), )
grids = {'states':s_grid, 'actions':a_grid}
Q_map, Q_F = vibly.compute_Q_map(grids, poincare_map, verbose = 2)
Q_V, S_V = vibly.compute_QV(Q_map, grids)
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
import time
filename = 'Q_map_daslip' + time.strftime("%Y_%m_%H_%M_%S") + '.pickle'
data2save = {"grids": grids, "Q_map": Q_map, "Q_F": Q_F, "Q_V":Q_V,
"p" : p_daslip, "x0":x0_daslip, "P_map" : poincare_map}
outfile = open(filename, 'wb')
pickle.dump(data2save, outfile)
outfile.close()
data2save = {
"s_grid": s_grid,
"a_grid": a_grid,
"Q_map": Q_map,
"Q_F": Q_F,
"Q_V": Q_V,
"S_V": S_V
}
np.savez('test_computeQ2D.npz', **data2save)
plt.imshow(Q_map)
plt.show()
# Do the same thing with the ND version
s_grid = np.linspace(0.1, 1, 5)
a_grid = np.linspace(-10 / 180 * np.pi, 90 / 180 * np.pi, 5)
Q_map, Q_F = compute_Q_map((s_grid, ), (a_grid, ), poincare_map)
grids = {'states': s_grid, 'actions': a_grid}
Q_V, S_V = compute_QV_2D(Q_map, grids)
# save file
data2save = {
"s_grid": s_grid,
"a_grid": a_grid,
"Q_map": Q_map,
"Q_F": Q_F,
"Q_V": Q_V,
"S_V": S_V
}
np.savez('test_computeQ.npz', **data2save)
sol_slip = step(x0_slip, p_lc)
# create the open_loop_force
actuator_time_force = create_actuator_open_loop_time_series(sol_slip, p_lc)
p_daslip = p_lc.copy()
p_daslip['model_type'] = 1
p_daslip['actuator_force'] = actuator_time_force
p_daslip['actuator_force_period'] = np.max(actuator_time_force[0, :])
# initialize default x0_daslip
x0_daslip = np.array(
[0, 0.85, 5.5, 0, 0, 0, p_daslip['actuator_resting_length'], 0, 0])
for idx, val in enumerate(x0_slip): # copy over default values from SLIP
x0_daslip[idx] = val
x0_daslip = reset_leg(x0_daslip, p_daslip)
poincare_map.p = p_daslip
poincare_map.x = x0_daslip
poincare_map.sa2xp = mapSA2xp_aoa
poincare_map.xp2s = map2s
s_grid_y = np.linspace(0.5, 2, 10)
s_grid_xdot = np.linspace(3, 10, 10)
s_grid = (s_grid_y, s_grid_xdot)
a_grid = (np.linspace(0 / 180 * np.pi, 70 / 180 * np.pi, 15), )
Q_map, Q_F = compute_Q_map(s_grid, a_grid, poincare_map)
grids = {'states': s_grid, 'actions': a_grid}
# save file
data2save = {"grids": grids, "Q_map": Q_map, "Q_F": Q_F}
np.savez('daslip_Q.npz', **data2save)