# Run the kinematic / time forward
qrb.run_quadrotor(dt_sim, fb, taub)
# Time has increased now
t = t + dt_sim
# Visualization frequency
if abs(t / dt_vis - round(t / dt_vis)) < 0.000001:
panda3D_app.taskMgr.step()
panda3D_app.screenText_pos(qrb.pos, qrb.rpy)
# Logging frequency
if abs(t / dt_log - round(t / dt_log)) < 0.000001:
logger.log_attstab(
t, np.array([rpy_ref[0], rpy_ref[1], rpy_ref[2]]),
np.array([omegab_ref[0], omegab_ref[1], omegab_ref[2]]),
np.array([tau_ref[0], tau_ref[1], tau_ref[2]]))
logger.log_posctrl(t, np.array([pos_ref[0], pos_ref[1], pos_ref[2]]))
logger.log_rigidbody(t, qrb)
fe = qrb.rotmb2e @ fb + qrb.mass * np.array([0, 0, -envir.g])
taue = qrb.rotmb2e @ taub
logger.log_ftau(t, fe, taue, fb, taub)
logger.log_cmd(t, cmd)
# End of program, wrap it up with logger and plotter
#########################################################
logger.log2file_rigidbody()
logger.log2file_cmd()
logger.log2file_ftau()
logger.log2file_attstab()
logger.log2file_posctrl()
# Run the kinematic / time forward
qrb.run_quadrotor(dt_sim, fb, taub)
# Time has increased now
t = t + dt_sim
# Visualization frequency
if abs(t / dt_vis - round(t / dt_vis)) < 0.000001:
panda3D_app.taskMgr.step()
panda3D_app.screenText_pos(qrb.pos, qrb.rpy)
# Logging frequency
if abs(t / dt_log - round(t / dt_log)) < 0.000001:
logger.log_attstab(
t, np.zeros(3),
np.array([omegab_ref[0], omegab_ref[1], omegab_ref[2]]),
np.array([tau_ref[0], tau_ref[1], tau_ref[2]]))
logger.log_rigidbody(t, qrb)
fe = qrb.rotmb2e @ fb + qrb.mass * np.array([0, 0, -envir.g])
taue = qrb.rotmb2e @ taub
logger.log_ftau(t, fe, taue, fb, taub)
logger.log_cmd(t, cmd)
# End of program, wrap it up with logger and plotter
#########################################################
logger.log2file_rigidbody()
logger.log2file_cmd()
logger.log2file_ftau()
logger.log2file_attstab()
plotter.plot_rigidbody(logger)
plotter.plot_cmd(logger)