# 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)