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)
