def get_step_trajectories(x0_, p_, ground_heights_=None):
if ground_heights_ is None:
total_leg_length = p_['spring_resting_length']
total_leg_length += p_['actuator_resting_length']
ground_heights_ = np.linspace(0, -0.5*total_leg_length, 10)
x0_ = sys.reset_leg(x0_, p_)
trajectories_ = list()
for height in ground_heights_:
x0_[-1] = height
trajectories_.append(sys.step(x0_, p_))
x0_[-1] = 0.0 # reset x0 back to 0
return trajectories_
def get_step_trajectories(x0, p, ground_heights=None):
'''
helper function to apply a battery of ground-height perturbations.
returns a list of trajectories.
'''
if ground_heights is None:
total_leg_length = p['resting_length']
total_leg_length += p['actuator_resting_length']
ground_heights = np.linspace(0, -0.5 * total_leg_length, 10)
x0 = model.reset_leg(x0, p)
trajectories = list()
for height in ground_heights:
x0[-1] = height
trajectories.append(model.step(x0, p))
x0[-1] = 0.0 # reset x0 back to 0
return trajectories
def get_step_trajectories(x0, p, ground_heights=None):
'''
helper function to apply a battery of ground-height perturbations.
returns a list of trajectories.
'''
if ground_heights is None:
total_leg_length = p['resting_length']
total_leg_length += p['actuator_resting_length']
ground_heights = np.linspace(0, -0.5 * total_leg_length, 10)
x0 = model.reset_leg(x0, p)
# start_idx = np.argwhere(~np.isclose(p['actuator_force'][1], 0))[0]
# time_to_activation = p['actuator_force'][0, start_idx]
trajectories = list()
for height in ground_heights:
x0[-1] = height
# time_to_touchdown = np.sqrt(2*(x0[5] - x0[-1])/p['gravity'])
# p['activation_delay'] = time_to_touchdown - time_to_activation
trajectories.append(model.step(x0, p))
x0[-1] = 0.0 # reset x0 back to 0
return trajectories
'actuator_resting_length': 0.1, # m
'actuator_force': [], # * 2 x M matrix of time and force
'actuator_force_period': 10, # * s
'activation_amplification': 1,
'activation_delay': 0.0, # * a delay for when to start activation
'constant_normalized_damping': 0.75, # * s : D/K : [N/m/s]/[N/m]
'linear_normalized_damping_coefficient': 3.5, # * A: s/m : D/F : [N/m/s]/N : 0.0035 N/mm/s -> 3.5 1/m/s from Kirch et al. Fig 12
'linear_minimum_normalized_damping': 0.05, # * 1/A*(kg*N/kg) :
'swing_leg_norm_angular_velocity': 0, # [1/s]/[m/s] (omega/(vx/lr))
'swing_velocity': 0, # rad/s (set by calculation)
'angle_of_attack_offset': 0} # rad (set by calculation)
# * linear_normalized_damping_coefficient:
# * A: s/m : D/F : [N/m/s]/N : 0.0035 N/mm/s -> 3.5 1/m/s (Kirch et al. Fig 12)
x0 = np.array([0, 1.00, 5.5, 0, 0, 0, p['actuator_resting_length'], 0, 0, 0])
x0 = model.reset_leg(x0, p)
p['total_energy'] = model.compute_total_energy(x0, p)
x0, p = model.create_open_loop_trajectories(x0, p)
p['x0'] = x0
p['activation_amplification'] = 1.5
# 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.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)
def plot_ground_perturbations(ax,
trajectories,
S_M,
grids,
p,
v_threshold=0.1,
col_offset=0.65,
col_norm=1.0,
draw_ground=True,
draw_LC=False,
Q_M=None,
colormap=None,
norm=1):
'''
Plot a series of trajectories, centered around level-ground as nominal
inputs:
trajectories: list of traj objects (sol to scipy.integrate.solve_ivp)
v_threshold: minimum safety, otherwise don't plot it
'''
# TODO redo this with trajectories colored by measure
print(" ")
# * plot step-ups
up_cmap = plt.get_cmap("Blues")
down_cmap = plt.get_cmap("Reds")
idx0 = -1
pert_min = 0
pert_max = 0
lc_viab = 0
for idx in range(len(trajectories)):
traj = trajectories[idx]
x = traj.y[:, -1] # ground
s = sys.xp2s_y_xdot(x, p)
sbin = vibly.digitize_s(s, grids['states'])
s_m = interp_measure(sbin, S_M, grids)
if s_m >= v_threshold:
if np.isclose(x[-1], 0): # limit cycle
# print afterwards, so it's on top of other circles
idx0 = idx
# keep track of max viability at LC
lc_viab = s_m
continue
elif x[-1] < 0:
col = down_cmap(col_offset - np.abs(x[-1]) / col_norm)
# col = down_cmap(np.abs(x[-1])/col_norm)
zod = 2
# keep track of min and max perturbations
if x[-1] < pert_min:
pert_min = x[-1]
elif x[-1] > 0:
col = up_cmap(col_offset - np.abs(x[-1]) / col_norm)
# col = up_cmap(np.abs(x[-1])/col_norm)
zod = 1
if x[-1] > pert_max:
pert_max = x[-1]
ax.plot(traj.y[0], traj.y[1], color=col, linewidth=2, zorder=zod)
# and plot ground
td_index = np.abs(traj.t - traj.t_events[1]).argmin()
to_index = np.abs(traj.t - traj.t_events[3]).argmin()
ax.plot(traj.y[0, td_index:to_index],
traj.y[-1, td_index:to_index],
color=col,
linewidth=2)
if idx0 > 0:
zod = 3
traj = trajectories[idx0]
ax.plot(traj.y[0],
traj.y[1],
linewidth=2.5,
color='black',
zorder=zod)
ax.plot(traj.y[0], traj.y[-1], color='black', zorder=zod)
print("Min perturbation: " + str(pert_min))
print("Max perturbation: " + str(pert_max))
print("Viability Measure at Limit Cycle: " + str(lc_viab))
if draw_LC:
if idx0 <= 0:
print("WARNING: no LC fond.")
x_next = trajectories[idx0].y[:, -1]
# plot pointmass
ax.scatter(x_next[0], x_next[1], s=300, color='black', zorder=3)
# plot leg
ax.plot([x_next[0], x_next[4]], [x_next[1], x_next[5]],
linewidth=3,
color='black',
zorder=3)
if Q_M is not None:
# pick out the correct slice
s_next = sys.xp2s_y_xdot(x_next, p)
sbin = vibly.digitize_s(s_next, grids['states'])
A_slice = np.copy(Q_M[tuple(sbin) + (slice(None), )])
viable_actions = grids['actions'][0][np.nonzero(A_slice)]
viable_action_M = A_slice[np.nonzero(A_slice)]
# color = plt.cm.hsv(viable_action_M)
# create an array of foot-points
foot_x = np.zeros_like(viable_actions)
foot_y = np.zeros_like(viable_actions)
for i, a in np.ndenumerate(viable_actions):
p['angle_of_attack'] = a
xtemp = sys.reset_leg(x_next, p)
foot_x[i] = xtemp[4]
foot_y[i] = xtemp[5]
cmap = plt.get_cmap("viridis")
color = cmap(norm(interp_measure(sbin, S_M, grids)))
ax.plot(foot_x, foot_y, zorder=2, color=color)
ax.plot([x_next[0], foot_x[0]], [x_next[1], foot_y[0]],
zorder=2,
color=color)
ax.plot([x_next[0], foot_x[-1]], [x_next[1], foot_y[-1]],
zorder=2,
color=color)
ax.grid = True
add_title(p)
