Exemplo n.º 1
0
            )

            #### Go to the next way point and loop #####################
            wp_counter = wp_counter + 1 if wp_counter < (NUM_WP - 1) else 0

        #### Log the simulation ####################################
        logger.log(drone=0,
                   timestamp=i / env.SIM_FREQ,
                   state=obs["0"]["state"],
                   control=np.hstack([TARGET_POS[wp_counter, :],
                                      np.zeros(9)]))

        #### Printout ##############################################
        if i % env.SIM_FREQ == 0:
            env.render()

        #### Sync the simulation ###################################
        if ARGS.gui:
            sync(i, START, env.TIMESTEP)

    #### Close the environment #################################
    env.close()

    #### Save the simulation results ###########################
    logger.save()
    logger.save_as_csv("gnd")  # Optional CSV save

    #### Plot the simulation results ###########################
    if ARGS.plot:
        logger.plot()
Exemplo n.º 2
0
        LOGGER.log(drone=1, timestamp=i / ENV.SIM_FREQ, state=STATE)

        #### Compute control for drone 2 ###########################
        STATE = OBS["2"]["state"]
        ACTION["2"] = CTRL_2.compute_control(
            current_position=STATE[0:3],
            current_velocity=STATE[10:13],
            current_rpy=STATE[7:10],
            target_position=TARGET_POSITION[i, :] + np.array([.3, .0, .0]),
            target_velocity=TARGET_VELOCITY[i, :],
            target_acceleration=TARGET_ACCELERATION[i, :])
        #### Log drone 2 ###########################################
        LOGGER.log(drone=2, timestamp=i / ENV.SIM_FREQ, state=STATE)

        #### Printout ##############################################
        if i % ENV.SIM_FREQ == 0:
            ENV.render()

        #### Sync the simulation ###################################
        if GUI:
            sync(i, START, ENV.TIMESTEP)

    #### Close the ENVironment #################################
    ENV.close()

    #### Save the simulation results ###########################
    LOGGER.save()

    #### Plot the simulation results ###########################
    LOGGER.plot()