# --------------
T = 0.1 # sampling time interval
step_time = 0.8 # step duration
no_steps_per_T = int(round(step_time/T))
# walking parameters:
# ------------------
Step_length = 0.21 # fixed step length in the xz-plane
no_steps = 20 # number of desired walking steps
walking_time = no_steps * no_steps_per_T # number of desired walking intervals
# compute CoP reference trajectory:
# --------------------------------
foot_step_0 = np.array([0.0, -0.09]) # initial foot step position in x-y
Foot_steps = reference_trajectories.manual_foot_placement(foot_step_0, Step_length, no_steps)
Z_ref = reference_trajectories.create_CoP_trajectory(no_steps, Foot_steps, \
walking_time, no_steps_per_T)
# used in case you want to have terminal constraints
# -------------------------------------------------
x_terminal = np.array([Z_ref[walking_time-1, 0], 0.0, 0.0]) # CoM final constraint in x : [x, xdot, x_ddot].T
y_terminal = np.array([Z_ref[walking_time-1, 1], 0.0, 0.0]) # CoM final constraint in y : [y, ydot, y_ddot].T
no_terminal_constraints = 6
terminal_index = walking_time-1
# construct your preview system: 'Go pokemon !'
# --------------------------------------------
[P_ps, P_vs, P_as, P_zs, P_pu, P_vu, P_au, P_zu] = motionModel.compute_recursive_matrices(walking_time, T, h, g)
[Q, p_k] = cost_function.compute_objective_terms(alpha, gamma, walking_time, \
P_zs, P_zu, x_hat_0, y_hat_0, Z_ref)
[A_zmp, b_zmp] = constraints.add_ZMP_constraints(walking_time, foot_length, foot_width,\
# [y, ydot, y_ddot].T
# --------------------------------------
x_hat_0 = np.array([0.0, 0.0, 0.0])
y_hat_0 = np.array([-0.09, 0.0, 0.0])
# Adding gaussian white noise to the control input:
# use to emulate closed-loop behavior in simulation
U_noise = 0*np.random.normal(0, 1, 2*N) # multiply by zero to get open-loop MPC
# compute CoP reference trajectory:
# --------------------------------
foot_step_0 = np.array([0.0, -0.09]) # initial foot step position in x-y
desiredFoot_steps = reference_trajectories.manual_foot_placement(foot_step_0, \
step_length, no_desired_steps)
desired_Z_ref = reference_trajectories.create_CoP_trajectory(no_desired_steps, \
desiredFoot_steps, desired_walking_time, no_steps_per_T)
#plannedFoot_steps = reference_trajectories.manual_foot_placement(foot_step_0,\
# step_length, no_planned_steps)
#planned_Z_ref = reference_trajectories.create_CoP_trajectory(no_planned_steps,\
# plannedFoot_steps, planned_walking_time, no_steps_per_T)
# plan the last 2 steps in the future to be the same as last step
planned_Z_ref = np.zeros((planned_walking_time, 2))
planned_Z_ref[0:desired_walking_time,:] = desired_Z_ref
planned_Z_ref[desired_walking_time:planned_walking_time,:] = desired_Z_ref[desired_walking_time-1,:]
x_hat_k = x_hat_0
y_hat_k = y_hat_0
Z_ref_k = planned_Z_ref[0:N,:]