#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(6)
R = eye(3)
S = solve_continuous_are(A, B, Q, R)
gainK = dot(dot(inv(R), B.T), S)
inputK = open('triple_pen_LQR_K_useful.pkl','rb')
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))
