def create_open_loop_trajectories(x0, p, options):
'''
Create a nominal trajectory based on a SLIP model
'''
p_slip = p.copy()
x0_slip = np.concatenate([x0[0:6], x0[-1:]])
x0_slip = slip.reset_leg(x0_slip, p_slip)
p_slip['total_energy'] = slip.compute_total_energy(x0_slip, p_slip)
#aoa, success = slip.find_limit_cycle(x0_slip, p_slip, limit_cycle_options)
val, success = slip.find_limit_cycle(x0_slip, p_slip, options)
if not success:
print("WARNING: no limit-cycles found")
return (x0, p)
searchP = options['search_parameter']
if (searchP):
p_slip[options['parameter_name']] = val
p[options['parameter_name']] = val
else:
x0_slip[options['state_index']] = val
x0[options['state_index']] = val
p_slip['total_energy'] = slip.compute_total_energy(x0_slip, p_slip)
p['total_energy'] = compute_total_energy(x0, p)
sol_slip = slip.step(x0_slip, p_slip)
# compute open-loop force trajectory from nominal slip traj
actuator_time_force = create_force_trajectory(sol_slip, p)
p['actuator_force'] = actuator_time_force
p['actuator_force_period'] = np.max(actuator_time_force[0, :])
t_contact = sol_slip.t_events[1][0]
p['angle_of_attack_offset'] = -t_contact * p['swing_velocity']
p['swing_leg_length_offset'] = -t_contact * p['swing_extension_velocity']
# Update the model.step solution
x0 = reset_leg(x0, p)
p['total_energy'] = compute_total_energy(x0, p)
return (x0, p)
'resting_length': 0.222,
'gravity': 9.81,
'angle_of_attack': 0.5452,
'actuator_resting_length': 0.0
} # offset of CoG to leg-start. Unimoprtant
x0 = np.array([
0, # x position: forwards
0.2, # y position: upwards
2.6, # x velocity
0, # y velocity
0, # foot position x
0, # foot position y
0
]) # ground position y
x0 = slip.reset_leg(x0, p)
p['total_energy'] = slip.compute_total_energy(x0, p)
sol = slip.step(x0, p)
import plotting.animation
plotting.animation.animation_visualisation(sol)
# * The arguments you can pass to the function are:
# slow = 10 : scales the animation relative to real time, default 10
# interval = 30 : time (in ms) between frames in the animation, default 30ms
# com_radius = 60 : size of the CoM dot
# see = True : calls up show() within the animation_visualisation function; I don't know why but if you call show here, you don't see the animation
# save = False : saves the animation, for this you need to have a matplotlib animation writer installed
# writer_name = 'html' : this default writer can only write html files, 'pillow' writes gif, ffmpeg writes mp4
# filename = 'test.html': name of the animation file that will be saved
def get_slip_trajectory(x0, p):
x0_slip = np.concatenate([x0[0:6], x0[-1:]])
x0_slip = slip.reset_leg(x0_slip, p)
p['total_energy'] = slip.compute_total_energy(x0_slip, p)
sol = slip.step(x0_slip, p)
return sol
