q[7]: 0.0, q[8]: 0.0, q[9]: 0.0, q[10]: 0.14, q[11]: 0.0, u[0]: 0.0, u[1]: 0.0, u[2]: 0.0, u[3]: 0.0, u[4]: 0.0, u[5]: 0.0, u[6]: 0.0, u[7]: 0.0, u[8]: 0.0, u[9]: 0.0, u[10]: 0.0, u[11]: 0.0 } sys.times = np.linspace(0, 10.0, 1000000) sys.generate_ode_function(generator='cython') x = sys.integrate() plt.plot(sys.times, x) plt.plot(sys.times, x[:, 0:3]) plt.plot(sys.times, x[:, 3:6]) plt.plot(sys.times, x[:, 6:9]) plt.plot(sys.times, x[:, 9:12])
blade_omega[6]: 0., blade_omega[7]: 0., } bb_sys.specifieds = { motor_voltage[0]: v_1, motor_voltage[1]: v_2, motor_voltage[2]: v_3, motor_voltage[3]: v_4, } bb_sys.specifieds.update(dict(zip(thrust_func,get_blade_thrust))) bb_sys.specifieds.update(dict(zip(torque_func,get_blade_torque))) bb_sys.generate_ode_function() dyn = bb_sys.evaluate_ode_function x0 = bb_sys._initial_conditions_padded_with_defaults() x0 = [x0[k] for k in bb_sys.states] print("derivations finished") def quat_norm(quat): qr = quat[0] qi = quat[1] qj = quat[2] qk = quat[3] norm = sqrt(qr**2+qi**2+qj**2+qk**2)