F_B = forcing_vector.subs(tor_dict).jacobian(specified)
print("jacobian done")
#Substitute in values for the variables in the forcing vector
F_A = F_A.subs(parameter_dict)
F_B = F_B.subs(parameter_dict)

print("subs done")
forcing_a = []
forcing_b = []
M = []

#Create the vectors storing jacobians about every equilibrium point
for element in equilibrium_dict:
  forcing_a.append(F_A.subs(element))
  forcing_b.append(F_B.subs(element))
  M.append(mass_matrix.subs(element))
print("forcing done")

for i in range(len(M)):
  M[i] = M[i].subs(parameter_dict)
  M[i] = array(M[i].tolist(), dtype = float)
  forcing_b[i] = array(forcing_b[i].tolist(), dtype = float)
  forcing_a[i] = array(forcing_a[i].tolist(), dtype = float)
print("m done")

#state A and input B values for linearized functions
A = []
B = []

for m, fa in zip(M, forcing_a):
  A.append(dot(inv(m), fa))
#Jacobian of the forcing vector w.r.t. states and inputs
tordict = dict(zip([l_ankle_torque], [0]))
F_A = forcing_vector.jacobian(coordinates + speeds)
F_B = forcing_vector.subs(tordict).jacobian(specified)

#Substitute in the values for the variables in the forcing vector
F_A = F_A.subs(equilibrium_dict)
F_A = F_A.subs(parameter_dict)
F_B = F_B.subs(equilibrium_dict).subs(parameter_dict)

#Convert into a floating point numpy array
F_A = array(F_A.tolist(), dtype = float)
F_B = array(F_B.tolist(), dtype = float)

M = mass_matrix.subs(equilibrium_dict)

M = M.subs(parameter_dict)
M = array(M.tolist(), dtype = float)

#Compute the state A and input B values for linearized function
A = dot(inv(M), F_A)
B = dot(inv(M), F_B)

Q = ((1/0.6)**2)*eye(4)
R = eye(2)

S = solve_continuous_are(A, B, Q, R)
gainK = dot(dot(inv(R), B.T), S)

inputK = open('double_block_LQR_K.pkl', 'rb')