print("Equilibrium Ponts done")
equilibrium_dict = []
for element in equilibrium_points:
equilibrium_dict.append(dict(zip(speeds + coordinates, element)))
print("Equilibrium dict done")
#Jacobian of forcing vector w.r.t. states and inputs
F_A = forcing_vector.jacobian(coordinates + speeds)
F_B = forcing_vector.subs(tor_dict).jacobian(specified)
print("Jacobian done")
#substitute in values for the evariables 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 = []
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("Equilibrium Done")
for i in range(len(M)):
M[i] = array(M[i].tolist(), dtype = float)
forcing_b[i] = array(forcing_b[i].tolist(), dtype = float)
equilibrium_dict = dict(zip(coordinates + speeds, equilibrium_point)) #Jacobian of the forcing vector w.r.t. states and inputs F_A = forcing_vector.jacobian(coordinates + speeds) F_B = forcing_vector.jacobian(specified) #Substitute in the values for the variables in the forcing vector F_A = simplify(F_A.subs(equilibrium_dict)) F_A = F_A.subs(parameter_dict) F_B = simplify(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 = simplify(M) M = M.subs(parameter_dict) M = array(M.tolist(), dtype = float) #Compute the state A and input B values for our linearized function A = dot(inv(M), F_A) B = dot(inv(M), F_B) #Makes sure our function is controllable #assert controllable(A,B) Q = eye(4) Q[0][0] = ((1.0/20.0)**2) Q[1][1] = ((1.0/20.0)**2) Q[2][2] = ((1.0/20.0)**2)
