answer_vector.append([x,y,z]) print("answer_vector done") #Linearization equilibrium_points = [] for element in answer_vector: equilibrium_points.append(concatenate((element, zeros(len(speeds))), axis=1)) print("equilibrium_points done") 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) 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))
'specified': array([0.0,0.0,0.0])} # Simulate # ======== frames_per_sec = 60 final_time = 10.0 t = linspace(0.0, final_time, final_time * frames_per_sec) #Create dictionaries for the values for equilibrium point equilibrium_point = zeros(len(coordinates + speeds)) 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 = 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)