equilibrium_dict = []
for element in equilibrium_points:
equilibrium_dict.append(dict(zip(coordinates + speeds, element)))
print("equilibrium_dict done")
# Jacobian fo forcing vector w.r.t. states and inputs
tor_dict = dict(zip([l_ankle_torque], [0]))
F_A = forcing_vector.jacobian(coordinates + speeds)
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)
mass_matrix = mass_matrix.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(array(F_A.subs(element).tolist(), dtype=float))
print("forcing a done")
for element in equilibrium_dict:
forcing_b.append(array(F_B.subs(element).tolist(), dtype=float))
print("forcing b done")
forcing_a = []
forcing_b = []
M = []
#Create the vectors storing jacobians about every equilibrium point
for element in equilibrium_dict:
forcing_a.append(array(F_A.subs(element).tolist(), dtype = float))
print("forcing a done")
for element in equilibrium_dict:
forcing_b.append(array(F_B.subs(element).tolist(), dtype = float))
print("forcing b done")
for element in equilibrium_dict:
M.append(mass_matrix.subs(element).subs(parameter_dict))
print("mass matrix subs done")
for i in range(len(M)):
M[i] = M[i].subs(parameter_dict)
M[i] = array(M[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))
print("fa done")
forcing_a = []
forcing_b = []
M = []
#Create the vectors storing jacobians about every equilibrium point
for element in equilibrium_dict:
forcing_a.append(array(F_A.subs(element).tolist(), dtype = float))
print("forcing a done")
for element in equilibrium_dict:
forcing_b.append(array(F_B.subs(element).tolist(), dtype = float))
print("forcing b done")
for element in equilibrium_dict:
M.append(mass_matrix.subs(element))
print("mass matrix subs done")
for i in range(len(M)):
M[i] = M[i].subs(parameter_dict)
M[i] = array(M[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))
print("fa done")
