From f747c9ac090fc7f126eecf41c7d32fc2a3e1f5ca Mon Sep 17 00:00:00 2001 From: Spencer Teetaert Date: Thu, 9 Jun 2022 16:33:18 -0400 Subject: [PATCH] Move examples to package. Added unit test script for build testing. Added bash file for building and testing package. Changed paths to be user controlled, results are not to be saved in package path. --- .gitignore | 1 + build_project.sh | 7 + .../assets}/example_trace.pkl | Bin .../examples}/compare.py | 53 ++++-- .../examples}/downwash.py | 77 ++++++--- .../examples}/fly.py | 154 +++++++++++------- .../examples}/groundeffect.py | 81 ++++++--- .../examples}/learn.py | 32 ++-- .../examples}/velocity.py | 93 +++++++---- gym_pybullet_drones/utils/Logger.py | 11 +- setup.py | 23 ++- tests/test_build.py | 32 ++++ 12 files changed, 385 insertions(+), 179 deletions(-) create mode 100755 build_project.sh rename {files => gym_pybullet_drones/assets}/example_trace.pkl (100%) rename {examples => gym_pybullet_drones/examples}/compare.py (71%) rename {examples => gym_pybullet_drones/examples}/downwash.py (57%) rename {examples => gym_pybullet_drones/examples}/fly.py (54%) rename {examples => gym_pybullet_drones/examples}/groundeffect.py (58%) rename {examples => gym_pybullet_drones/examples}/learn.py (81%) rename {examples => gym_pybullet_drones/examples}/velocity.py (56%) create mode 100644 tests/test_build.py diff --git a/.gitignore b/.gitignore index 58a222e8d..f687d5ca6 100644 --- a/.gitignore +++ b/.gitignore @@ -6,6 +6,7 @@ examples/test.py # NumPy saves and videos files/logs/*.npy files/videos/*.mp4 +results/ # Learning results experiments/learning/results/save-* diff --git a/build_project.sh b/build_project.sh new file mode 100755 index 000000000..56a08fafc --- /dev/null +++ b/build_project.sh @@ -0,0 +1,7 @@ +echo "Y" | pip uninstall gym_pybullet_drones +rm -rf dist/ +poetry build +pip install dist/gym_pybullet_drones-1.0.0-py3-none-any.whl +cd tests +python test_build.py +cd .. \ No newline at end of file diff --git a/files/example_trace.pkl b/gym_pybullet_drones/assets/example_trace.pkl similarity index 100% rename from files/example_trace.pkl rename to gym_pybullet_drones/assets/example_trace.pkl diff --git a/examples/compare.py b/gym_pybullet_drones/examples/compare.py similarity index 71% rename from examples/compare.py rename to gym_pybullet_drones/examples/compare.py index 58c680081..cf3625324 100644 --- a/examples/compare.py +++ b/gym_pybullet_drones/examples/compare.py @@ -13,6 +13,7 @@ The comparison is along a 2D trajectory in the X-Z plane, between x == +1 and -1. """ +from filecmp import DEFAULT_IGNORES import os import time import argparse @@ -25,18 +26,22 @@ from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl from gym_pybullet_drones.utils.Logger import Logger -if __name__ == "__main__": - - #### Define and parse (optional) arguments for the script ## - parser = argparse.ArgumentParser(description='Trace comparison script using CtrlAviary and DSLPIDControl') - parser.add_argument('--physics', default="pyb", type=Physics, help='Physics updates (default: PYB)', metavar='', choices=Physics) - parser.add_argument('--gui', default=False, type=str2bool, help='Whether to use PyBullet GUI (default: False)', metavar='') - parser.add_argument('--record_video', default=False, type=str2bool, help='Whether to record a video (default: False)', metavar='') - parser.add_argument('--trace_file', default="example_trace.pkl", type=str, help='Pickle file with the trace to compare to (default: "example_trace.pkl")', metavar='') - ARGS = parser.parse_args() - +DEFAULT_PHYICS = Physics('pyb') +DEFAULT_GUI = False +DEFAULT_RECORD_VIDEO = False +DEFAULT_TRACE_FILE = os.path.dirname(os.path.abspath(__file__))+"/../assets/example_trace.pkl" +DEFAULT_OUTPUT_FOLDER = 'results' + +def run( + physics=DEFAULT_PHYICS, + gui=DEFAULT_GUI, + record_video=DEFAULT_RECORD_VIDEO, + trace_file=DEFAULT_TRACE_FILE, + output_folder=DEFAULT_OUTPUT_FOLDER, + plot=True + ): #### Load a trace and control reference from a .pkl file ### - with open(os.path.dirname(os.path.abspath(__file__))+"/../files/"+ARGS.trace_file, 'rb') as in_file: + with open(trace_file, 'rb') as in_file: TRACE_TIMESTAMPS, TRACE_DATA, TRACE_CTRL_REFERENCE, _, _, _ = pickle.load(in_file) #### Compute trace's parameters ############################ @@ -47,10 +52,10 @@ env = CtrlAviary(drone_model=DroneModel.CF2X, num_drones=1, initial_xyzs=np.array([0, 0, .1]).reshape(1, 3), - physics=ARGS.physics, + physics=physics, freq=SIMULATION_FREQ_HZ, - gui=ARGS.gui, - record=ARGS.record_video, + gui=gui, + record=record_video, obstacles=False ) INITIAL_STATE = env.reset() @@ -63,7 +68,8 @@ #### Initialize the logger ################################# logger = Logger(logging_freq_hz=SIMULATION_FREQ_HZ, num_drones=2, - duration_sec=DURATION_SEC + duration_sec=DURATION_SEC, + output_folder=output_folder ) #### Initialize the controller ############################# @@ -105,7 +111,7 @@ env.render() #### Sync the simulation ################################### - if ARGS.gui: + if gui: sync(i, START, env.TIMESTEP) #### Close the environment ################################# @@ -115,4 +121,17 @@ logger.save() #### Plot the simulation results ########################### - logger.plot(pwm=True) + if plot: + logger.plot(pwm=True) + +if __name__ == "__main__": + #### Define and parse (optional) arguments for the script ## + parser = argparse.ArgumentParser(description='Trace comparison script using CtrlAviary and DSLPIDControl') + parser.add_argument('--physics', default=DEFAULT_PHYICS, type=Physics, help='Physics updates (default: PYB)', metavar='', choices=Physics) + parser.add_argument('--gui', default=DEFAULT_GUI, type=str2bool, help='Whether to use PyBullet GUI (default: False)', metavar='') + parser.add_argument('--record_video', default=DEFAULT_RECORD_VIDEO, type=str2bool, help='Whether to record a video (default: False)', metavar='') + parser.add_argument('--trace_file', default=DEFAULT_TRACE_FILE, type=str, help='Pickle file with the trace to compare to (default: "example_trace.pkl")', metavar='') + parser.add_argument('--output_folder', default=DEFAULT_OUTPUT_FOLDER, type=str, help='Folder where to save logs (default: "results")', metavar='') + ARGS = parser.parse_args() + + run(**vars(ARGS)) \ No newline at end of file diff --git a/examples/downwash.py b/gym_pybullet_drones/examples/downwash.py similarity index 57% rename from examples/downwash.py rename to gym_pybullet_drones/examples/downwash.py index deaefd421..3d51709f9 100644 --- a/examples/downwash.py +++ b/gym_pybullet_drones/examples/downwash.py @@ -21,56 +21,64 @@ from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl from gym_pybullet_drones.utils.Logger import Logger -if __name__ == "__main__": - - #### Define and parse (optional) arguments for the script ## - parser = argparse.ArgumentParser(description='Downwash example script using CtrlAviary and DSLPIDControl') - parser.add_argument('--drone', default="cf2x", type=DroneModel, help='Drone model (default: CF2X)', metavar='', choices=DroneModel) - parser.add_argument('--gui', default=True, type=str2bool, help='Whether to use PyBullet GUI (default: True)', metavar='') - parser.add_argument('--record_video', default=False, type=str2bool, help='Whether to record a video (default: False)', metavar='') - parser.add_argument('--simulation_freq_hz', default=240, type=int, help='Simulation frequency in Hz (default: 240)', metavar='') - parser.add_argument('--control_freq_hz', default=48, type=int, help='Control frequency in Hz (default: 48)', metavar='') - parser.add_argument('--aggregate', default=True, type=str2bool, help='Whether to aggregate physics steps (default: False)', metavar='') - parser.add_argument('--duration_sec', default=12, type=int, help='Duration of the simulation in seconds (default: 10)', metavar='') - ARGS = parser.parse_args() - +DEFAULT_DRONE = DroneModel('cf2x') +DEFAULT_GUI = True +DEFAULT_RECORD_VIDEO = False +DEFAULT_SIMULATION_FREQ_HZ = 240 +DEFAULT_CONTROL_FREQ_HZ = 48 +DEFAULT_AGGREGATE = True +DEFAULT_DURATION_SEC = 12 +DEFAULT_OUTPUT_FOLDER = 'results' + +def run( + drone=DEFAULT_DRONE, + gui=DEFAULT_GUI, + record_video=DEFAULT_RECORD_VIDEO, + simulation_freq_hz=DEFAULT_SIMULATION_FREQ_HZ, + control_freq_hz=DEFAULT_CONTROL_FREQ_HZ, + aggregate=DEFAULT_AGGREGATE, + duration_sec=DEFAULT_DURATION_SEC, + output_folder=DEFAULT_OUTPUT_FOLDER, + plot=True + ): #### Initialize the simulation ############################# INIT_XYZS = np.array([[.5, 0, 1],[-.5, 0, .5]]) - AGGR_PHY_STEPS = int(ARGS.simulation_freq_hz/ARGS.control_freq_hz) if ARGS.aggregate else 1 - env = CtrlAviary(drone_model=ARGS.drone, + AGGR_PHY_STEPS = int(simulation_freq_hz/control_freq_hz) if aggregate else 1 + env = CtrlAviary(drone_model=drone, num_drones=2, initial_xyzs=INIT_XYZS, physics=Physics.PYB_DW, neighbourhood_radius=10, - freq=ARGS.simulation_freq_hz, + freq=simulation_freq_hz, aggregate_phy_steps=AGGR_PHY_STEPS, - gui=ARGS.gui, - record=ARGS.record_video, + gui=gui, + record=record_video, obstacles=True ) #### Initialize the trajectories ########################### PERIOD = 5 - NUM_WP = ARGS.control_freq_hz*PERIOD + NUM_WP = control_freq_hz*PERIOD TARGET_POS = np.zeros((NUM_WP, 2)) for i in range(NUM_WP): TARGET_POS[i, :] = [0.5*np.cos(2*np.pi*(i/NUM_WP)), 0] wp_counters = np.array([0, int(NUM_WP/2)]) #### Initialize the logger ################################# - logger = Logger(logging_freq_hz=int(ARGS.simulation_freq_hz/AGGR_PHY_STEPS), + logger = Logger(logging_freq_hz=int(simulation_freq_hz/AGGR_PHY_STEPS), num_drones=2, - duration_sec=ARGS.duration_sec + duration_sec=duration_sec, + output_folder=output_folder ) #### Initialize the controllers ############################ - ctrl = [DSLPIDControl(drone_model=ARGS.drone) for i in range(2)] + ctrl = [DSLPIDControl(drone_model=drone) for i in range(2)] #### Run the simulation #################################### - CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ/ARGS.control_freq_hz)) + CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ/control_freq_hz)) action = {str(i): np.array([0, 0, 0, 0]) for i in range(2)} START = time.time() - for i in range(0, int(ARGS.duration_sec*env.SIM_FREQ), AGGR_PHY_STEPS): + for i in range(0, int(duration_sec*env.SIM_FREQ), AGGR_PHY_STEPS): #### Step the simulation ################################### obs, reward, done, info = env.step(action) @@ -102,7 +110,7 @@ env.render() #### Sync the simulation ################################### - if ARGS.gui: + if gui: sync(i, START, env.TIMESTEP) #### Close the environment ################################# @@ -113,4 +121,21 @@ logger.save_as_csv("dw") # Optional CSV save #### Plot the simulation results ########################### - logger.plot() + if plot: + logger.plot() + + +if __name__ == "__main__": + #### Define and parse (optional) arguments for the script ## + parser = argparse.ArgumentParser(description='Downwash example script using CtrlAviary and DSLPIDControl') + parser.add_argument('--drone', default=DEFAULT_DRONE, type=DroneModel, help='Drone model (default: CF2X)', metavar='', choices=DroneModel) + parser.add_argument('--gui', default=DEFAULT_GUI, type=str2bool, help='Whether to use PyBullet GUI (default: True)', metavar='') + parser.add_argument('--record_video', default=DEFAULT_RECORD_VIDEO, type=str2bool, help='Whether to record a video (default: False)', metavar='') + parser.add_argument('--simulation_freq_hz', default=DEFAULT_SIMULATION_FREQ_HZ, type=int, help='Simulation frequency in Hz (default: 240)', metavar='') + parser.add_argument('--control_freq_hz', default=DEFAULT_CONTROL_FREQ_HZ, type=int, help='Control frequency in Hz (default: 48)', metavar='') + parser.add_argument('--aggregate', default=DEFAULT_AGGREGATE, type=str2bool, help='Whether to aggregate physics steps (default: False)', metavar='') + parser.add_argument('--duration_sec', default=DEFAULT_DURATION_SEC, type=int, help='Duration of the simulation in seconds (default: 10)', metavar='') + parser.add_argument('--output_folder', default=DEFAULT_OUTPUT_FOLDER, type=str, help='Folder where to save logs (default: "results")', metavar='') + ARGS = parser.parse_args() + + run(**vars(ARGS)) \ No newline at end of file diff --git a/examples/fly.py b/gym_pybullet_drones/examples/fly.py similarity index 54% rename from examples/fly.py rename to gym_pybullet_drones/examples/fly.py index a56fcb181..323ca6d31 100644 --- a/examples/fly.py +++ b/gym_pybullet_drones/examples/fly.py @@ -34,46 +34,58 @@ from gym_pybullet_drones.utils.Logger import Logger from gym_pybullet_drones.utils.utils import sync, str2bool -if __name__ == "__main__": - - #### Define and parse (optional) arguments for the script ## - parser = argparse.ArgumentParser(description='Helix flight script using CtrlAviary or VisionAviary and DSLPIDControl') - parser.add_argument('--drone', default="cf2x", type=DroneModel, help='Drone model (default: CF2X)', metavar='', choices=DroneModel) - parser.add_argument('--num_drones', default=3, type=int, help='Number of drones (default: 3)', metavar='') - parser.add_argument('--physics', default="pyb", type=Physics, help='Physics updates (default: PYB)', metavar='', choices=Physics) - parser.add_argument('--vision', default=False, type=str2bool, help='Whether to use VisionAviary (default: False)', metavar='') - parser.add_argument('--gui', default=True, type=str2bool, help='Whether to use PyBullet GUI (default: True)', metavar='') - parser.add_argument('--record_video', default=False, type=str2bool, help='Whether to record a video (default: False)', metavar='') - parser.add_argument('--plot', default=True, type=str2bool, help='Whether to plot the simulation results (default: True)', metavar='') - parser.add_argument('--user_debug_gui', default=False, type=str2bool, help='Whether to add debug lines and parameters to the GUI (default: False)', metavar='') - parser.add_argument('--aggregate', default=True, type=str2bool, help='Whether to aggregate physics steps (default: True)', metavar='') - parser.add_argument('--obstacles', default=True, type=str2bool, help='Whether to add obstacles to the environment (default: True)', metavar='') - parser.add_argument('--simulation_freq_hz', default=240, type=int, help='Simulation frequency in Hz (default: 240)', metavar='') - parser.add_argument('--control_freq_hz', default=48, type=int, help='Control frequency in Hz (default: 48)', metavar='') - parser.add_argument('--duration_sec', default=12, type=int, help='Duration of the simulation in seconds (default: 5)', metavar='') - ARGS = parser.parse_args() - +DEFAULT_DRONES = DroneModel("cf2x") +DEFAULT_NUM_DRONES = 3 +DEFAULT_PHYSICS = Physics("pyb") +DEFAULT_VISION = False +DEFAULT_GUI = True +DEFAULT_RECORD_VISION = False +DEFAULT_PLOT = True +DEFAULT_USER_DEBUG_GUI = False +DEFAULT_AGGREGATE = True +DEFAULT_OBSTACLES = True +DEFAULT_SIMULATION_FREQ_HZ = 240 +DEFAULT_CONTROL_FREQ_HZ = 48 +DEFAULT_DURATION_SEC = 12 +DEFAULT_OUTPUT_FOLDER = 'results' + +def run( + drone=DEFAULT_DRONES, + num_drones=DEFAULT_NUM_DRONES, + physics=DEFAULT_PHYSICS, + vision=DEFAULT_VISION, + gui=DEFAULT_GUI, + record_video=DEFAULT_RECORD_VISION, + plot=DEFAULT_PLOT, + user_debug_gui=DEFAULT_USER_DEBUG_GUI, + aggregate=DEFAULT_AGGREGATE, + obstacles=DEFAULT_OBSTACLES, + simulation_freq_hz=DEFAULT_SIMULATION_FREQ_HZ, + control_freq_hz=DEFAULT_CONTROL_FREQ_HZ, + duration_sec=DEFAULT_DURATION_SEC, + output_folder=DEFAULT_OUTPUT_FOLDER + ): #### Initialize the simulation ############################# H = .1 H_STEP = .05 R = .3 - INIT_XYZS = np.array([[R*np.cos((i/6)*2*np.pi+np.pi/2), R*np.sin((i/6)*2*np.pi+np.pi/2)-R, H+i*H_STEP] for i in range(ARGS.num_drones)]) - INIT_RPYS = np.array([[0, 0, i * (np.pi/2)/ARGS.num_drones] for i in range(ARGS.num_drones)]) - AGGR_PHY_STEPS = int(ARGS.simulation_freq_hz/ARGS.control_freq_hz) if ARGS.aggregate else 1 + INIT_XYZS = np.array([[R*np.cos((i/6)*2*np.pi+np.pi/2), R*np.sin((i/6)*2*np.pi+np.pi/2)-R, H+i*H_STEP] for i in range(num_drones)]) + INIT_RPYS = np.array([[0, 0, i * (np.pi/2)/num_drones] for i in range(num_drones)]) + AGGR_PHY_STEPS = int(simulation_freq_hz/control_freq_hz) if aggregate else 1 #### Initialize a circular trajectory ###################### PERIOD = 10 - NUM_WP = ARGS.control_freq_hz*PERIOD + NUM_WP = control_freq_hz*PERIOD TARGET_POS = np.zeros((NUM_WP,3)) for i in range(NUM_WP): TARGET_POS[i, :] = R*np.cos((i/NUM_WP)*(2*np.pi)+np.pi/2)+INIT_XYZS[0, 0], R*np.sin((i/NUM_WP)*(2*np.pi)+np.pi/2)-R+INIT_XYZS[0, 1], 0 - wp_counters = np.array([int((i*NUM_WP/6)%NUM_WP) for i in range(ARGS.num_drones)]) + wp_counters = np.array([int((i*NUM_WP/6)%NUM_WP) for i in range(num_drones)]) #### Debug trajectory ###################################### #### Uncomment alt. target_pos in .computeControlFromState() - # INIT_XYZS = np.array([[.3 * i, 0, .1] for i in range(ARGS.num_drones)]) - # INIT_RPYS = np.array([[0, 0, i * (np.pi/3)/ARGS.num_drones] for i in range(ARGS.num_drones)]) - # NUM_WP = ARGS.control_freq_hz*15 + # INIT_XYZS = np.array([[.3 * i, 0, .1] for i in range(num_drones)]) + # INIT_RPYS = np.array([[0, 0, i * (np.pi/3)/num_drones] for i in range(num_drones)]) + # NUM_WP = control_freq_hz*15 # TARGET_POS = np.zeros((NUM_WP,3)) # for i in range(NUM_WP): # if i < NUM_WP/6: @@ -88,56 +100,57 @@ # TARGET_POS[i, :] = ((i-4*NUM_WP/6)*6)/NUM_WP, ((i-4*NUM_WP/6)*6)/NUM_WP, 0.5*((i-4*NUM_WP/6)*6)/NUM_WP # elif i < 6 * NUM_WP/6: # TARGET_POS[i, :] = 1 - ((i-5*NUM_WP/6)*6)/NUM_WP, 1 - ((i-5*NUM_WP/6)*6)/NUM_WP, 0.5 - 0.5*((i-5*NUM_WP/6)*6)/NUM_WP - # wp_counters = np.array([0 for i in range(ARGS.num_drones)]) + # wp_counters = np.array([0 for i in range(num_drones)]) #### Create the environment with or without video capture ## - if ARGS.vision: - env = VisionAviary(drone_model=ARGS.drone, - num_drones=ARGS.num_drones, + if vision: + env = VisionAviary(drone_model=drone, + num_drones=num_drones, initial_xyzs=INIT_XYZS, initial_rpys=INIT_RPYS, - physics=ARGS.physics, + physics=physics, neighbourhood_radius=10, - freq=ARGS.simulation_freq_hz, + freq=simulation_freq_hz, aggregate_phy_steps=AGGR_PHY_STEPS, - gui=ARGS.gui, - record=ARGS.record_video, - obstacles=ARGS.obstacles + gui=gui, + record=record_video, + obstacles=obstacles ) else: - env = CtrlAviary(drone_model=ARGS.drone, - num_drones=ARGS.num_drones, + env = CtrlAviary(drone_model=drone, + num_drones=num_drones, initial_xyzs=INIT_XYZS, initial_rpys=INIT_RPYS, - physics=ARGS.physics, + physics=physics, neighbourhood_radius=10, - freq=ARGS.simulation_freq_hz, + freq=simulation_freq_hz, aggregate_phy_steps=AGGR_PHY_STEPS, - gui=ARGS.gui, - record=ARGS.record_video, - obstacles=ARGS.obstacles, - user_debug_gui=ARGS.user_debug_gui + gui=gui, + record=record_video, + obstacles=obstacles, + user_debug_gui=user_debug_gui ) #### Obtain the PyBullet Client ID from the environment #### PYB_CLIENT = env.getPyBulletClient() #### Initialize the logger ################################# - logger = Logger(logging_freq_hz=int(ARGS.simulation_freq_hz/AGGR_PHY_STEPS), - num_drones=ARGS.num_drones + logger = Logger(logging_freq_hz=int(simulation_freq_hz/AGGR_PHY_STEPS), + num_drones=num_drones, + output_folder=output_folder ) #### Initialize the controllers ############################ - if ARGS.drone in [DroneModel.CF2X, DroneModel.CF2P]: - ctrl = [DSLPIDControl(drone_model=ARGS.drone) for i in range(ARGS.num_drones)] - elif ARGS.drone in [DroneModel.HB]: - ctrl = [SimplePIDControl(drone_model=ARGS.drone) for i in range(ARGS.num_drones)] + if drone in [DroneModel.CF2X, DroneModel.CF2P]: + ctrl = [DSLPIDControl(drone_model=drone) for i in range(num_drones)] + elif drone in [DroneModel.HB]: + ctrl = [SimplePIDControl(drone_model=drone) for i in range(num_drones)] #### Run the simulation #################################### - CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ/ARGS.control_freq_hz)) - action = {str(i): np.array([0,0,0,0]) for i in range(ARGS.num_drones)} + CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ/control_freq_hz)) + action = {str(i): np.array([0,0,0,0]) for i in range(num_drones)} START = time.time() - for i in range(0, int(ARGS.duration_sec*env.SIM_FREQ), AGGR_PHY_STEPS): + for i in range(0, int(duration_sec*env.SIM_FREQ), AGGR_PHY_STEPS): #### Make it rain rubber ducks ############################# # if i/env.SIM_FREQ>5 and i%10==0 and i/env.SIM_FREQ<10: p.loadURDF("duck_vhacd.urdf", [0+random.gauss(0, 0.3),-0.5+random.gauss(0, 0.3),3], p.getQuaternionFromEuler([random.randint(0,360),random.randint(0,360),random.randint(0,360)]), physicsClientId=PYB_CLIENT) @@ -149,7 +162,7 @@ if i%CTRL_EVERY_N_STEPS == 0: #### Compute control for the current way point ############# - for j in range(ARGS.num_drones): + for j in range(num_drones): action[str(j)], _, _ = ctrl[j].computeControlFromState(control_timestep=CTRL_EVERY_N_STEPS*env.TIMESTEP, state=obs[str(j)]["state"], target_pos=np.hstack([TARGET_POS[wp_counters[j], 0:2], INIT_XYZS[j, 2]]), @@ -158,11 +171,11 @@ ) #### Go to the next way point and loop ##################### - for j in range(ARGS.num_drones): + for j in range(num_drones): wp_counters[j] = wp_counters[j] + 1 if wp_counters[j] < (NUM_WP-1) else 0 #### Log the simulation #################################### - for j in range(ARGS.num_drones): + for j in range(num_drones): logger.log(drone=j, timestamp=i/env.SIM_FREQ, state= obs[str(j)]["state"], @@ -174,15 +187,15 @@ if i%env.SIM_FREQ == 0: env.render() #### Print matrices with the images captured by each drone # - if ARGS.vision: - for j in range(ARGS.num_drones): + if vision: + for j in range(num_drones): print(obs[str(j)]["rgb"].shape, np.average(obs[str(j)]["rgb"]), obs[str(j)]["dep"].shape, np.average(obs[str(j)]["dep"]), obs[str(j)]["seg"].shape, np.average(obs[str(j)]["seg"]) ) #### Sync the simulation ################################### - if ARGS.gui: + if gui: sync(i, START, env.TIMESTEP) #### Close the environment ################################# @@ -193,5 +206,26 @@ logger.save_as_csv("pid") # Optional CSV save #### Plot the simulation results ########################### - if ARGS.plot: + if plot: logger.plot() + +if __name__ == "__main__": + #### Define and parse (optional) arguments for the script ## + parser = argparse.ArgumentParser(description='Helix flight script using CtrlAviary or VisionAviary and DSLPIDControl') + parser.add_argument('--drone', default=DEFAULT_DRONES, type=DroneModel, help='Drone model (default: CF2X)', metavar='', choices=DroneModel) + parser.add_argument('--num_drones', default=DEFAULT_NUM_DRONES, type=int, help='Number of drones (default: 3)', metavar='') + parser.add_argument('--physics', default=DEFAULT_PHYSICS, type=Physics, help='Physics updates (default: PYB)', metavar='', choices=Physics) + parser.add_argument('--vision', default=DEFAULT_VISION, type=str2bool, help='Whether to use VisionAviary (default: False)', metavar='') + parser.add_argument('--gui', default=DEFAULT_GUI, type=str2bool, help='Whether to use PyBullet GUI (default: True)', metavar='') + parser.add_argument('--record_video', default=DEFAULT_RECORD_VISION, type=str2bool, help='Whether to record a video (default: False)', metavar='') + parser.add_argument('--plot', default=DEFAULT_PLOT, type=str2bool, help='Whether to plot the simulation results (default: True)', metavar='') + parser.add_argument('--user_debug_gui', default=DEFAULT_USER_DEBUG_GUI, type=str2bool, help='Whether to add debug lines and parameters to the GUI (default: False)', metavar='') + parser.add_argument('--aggregate', default=DEFAULT_AGGREGATE, type=str2bool, help='Whether to aggregate physics steps (default: True)', metavar='') + parser.add_argument('--obstacles', default=DEFAULT_OBSTACLES, type=str2bool, help='Whether to add obstacles to the environment (default: True)', metavar='') + parser.add_argument('--simulation_freq_hz', default=DEFAULT_SIMULATION_FREQ_HZ, type=int, help='Simulation frequency in Hz (default: 240)', metavar='') + parser.add_argument('--control_freq_hz', default=DEFAULT_CONTROL_FREQ_HZ, type=int, help='Control frequency in Hz (default: 48)', metavar='') + parser.add_argument('--duration_sec', default=DEFAULT_DURATION_SEC, type=int, help='Duration of the simulation in seconds (default: 5)', metavar='') + parser.add_argument('--output_folder', default=DEFAULT_OUTPUT_FOLDER, type=str, help='Folder where to save logs (default: "results")', metavar='') + ARGS = parser.parse_args() + + run(**vars(ARGS)) \ No newline at end of file diff --git a/examples/groundeffect.py b/gym_pybullet_drones/examples/groundeffect.py similarity index 58% rename from examples/groundeffect.py rename to gym_pybullet_drones/examples/groundeffect.py index f1cde0843..3c2288bf7 100644 --- a/examples/groundeffect.py +++ b/gym_pybullet_drones/examples/groundeffect.py @@ -32,28 +32,37 @@ from gym_pybullet_drones.utils.Logger import Logger from gym_pybullet_drones.utils.utils import sync, str2bool -if __name__ == "__main__": - - #### Define and parse (optional) arguments for the script ## - parser = argparse.ArgumentParser(description='Ground effect script using CtrlAviary and DSLPIDControl') - parser.add_argument('--gui', default=True, type=str2bool, help='Whether to use PyBullet GUI (default: True)', metavar='') - parser.add_argument('--record_video', default=False, type=str2bool, help='Whether to record a video (default: False)', metavar='') - parser.add_argument('--plot', default=True, type=str2bool, help='Whether to plot the simulation results (default: True)', metavar='') - parser.add_argument('--user_debug_gui', default=False, type=str2bool, help='Whether to add debug lines and parameters to the GUI (default: False)', metavar='') - parser.add_argument('--aggregate', default=True, type=str2bool, help='Whether to aggregate physics steps (default: False)', metavar='') - parser.add_argument('--obstacles', default=True, type=str2bool, help='Whether to add obstacles to the environment (default: True)', metavar='') - parser.add_argument('--simulation_freq_hz', default=240, type=int, help='Simulation frequency in Hz (default: 240)', metavar='') - parser.add_argument('--control_freq_hz', default=240, type=int, help='Control frequency in Hz (default: 48)', metavar='') - parser.add_argument('--duration_sec', default=4, type=int, help='Duration of the simulation in seconds (default: 5)', metavar='') - ARGS = parser.parse_args() +DEFAULT_GUI = True +DEFAULT_RECORD_VIDEO = False +DEFAULT_PLOT = True +DEFAULT_USER_DEBUG_GUI = False +DEFAULT_AGGREGATE = True +DEFAULT_OBSTACLES = True +DEFAULT_SIMULATION_FREQ_HZ = 240 +DEFAULT_CONTROL_FREQ_HZ = 240 +DEFAULT_DURATION_SEC = 4 +DEFAULT_OUTPUT_FOLDER = 'results' + +def run( + gui=DEFAULT_GUI, + record_video=DEFAULT_RECORD_VIDEO, + plot=DEFAULT_PLOT, + user_debug_gui=DEFAULT_USER_DEBUG_GUI, + aggregate=DEFAULT_AGGREGATE, + obstacles=DEFAULT_OBSTACLES, + simulation_freq_hz=DEFAULT_SIMULATION_FREQ_HZ, + control_freq_hz=DEFAULT_CONTROL_FREQ_HZ, + duration_sec=DEFAULT_DURATION_SEC, + output_folder=DEFAULT_OUTPUT_FOLDER + ): #### Initialize the simulation ############################# INIT_XYZ = np.array([0, 0, 0.014]).reshape(1,3) - AGGR_PHY_STEPS = int(ARGS.simulation_freq_hz/ARGS.control_freq_hz) if ARGS.aggregate else 1 + AGGR_PHY_STEPS = int(simulation_freq_hz/control_freq_hz) if aggregate else 1 #### Initialize a vertical trajectory ###################### PERIOD = 4 - NUM_WP = ARGS.control_freq_hz*PERIOD + NUM_WP = control_freq_hz*PERIOD TARGET_POS = np.zeros((NUM_WP,3)) for i in range(NUM_WP): TARGET_POS[i, :] = INIT_XYZ[0, 0], INIT_XYZ[0, 1], INIT_XYZ[0, 2] + 0.15 * (np.sin((i/NUM_WP)*(2*np.pi)) + 1) @@ -66,30 +75,31 @@ physics=Physics.PYB_GND, # physics=Physics.PYB, # For comparison neighbourhood_radius=10, - freq=ARGS.simulation_freq_hz, + freq=simulation_freq_hz, aggregate_phy_steps=AGGR_PHY_STEPS, - gui=ARGS.gui, - record=ARGS.record_video, - obstacles=ARGS.obstacles, - user_debug_gui=ARGS.user_debug_gui + gui=gui, + record=record_video, + obstacles=obstacles, + user_debug_gui=user_debug_gui ) #### Obtain the PyBullet Client ID from the environment #### PYB_CLIENT = env.getPyBulletClient() #### Initialize the logger ################################# - logger = Logger(logging_freq_hz=int(ARGS.simulation_freq_hz/AGGR_PHY_STEPS), - num_drones=1 + logger = Logger(logging_freq_hz=int(simulation_freq_hz/AGGR_PHY_STEPS), + num_drones=1, + output_folder=output_folder ) #### Initialize the controller ############################# ctrl = DSLPIDControl(drone_model=DroneModel.CF2X) #### Run the simulation #################################### - CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ/ARGS.control_freq_hz)) + CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ/control_freq_hz)) action = {"0": np.array([0,0,0,0])} START = time.time() - for i in range(0, int(ARGS.duration_sec*env.SIM_FREQ), AGGR_PHY_STEPS): + for i in range(0, int(duration_sec*env.SIM_FREQ), AGGR_PHY_STEPS): #### Make it rain rubber ducks ############################# # if i/env.SIM_FREQ>5 and i%10==0 and i/env.SIM_FREQ<10: p.loadURDF("duck_vhacd.urdf", [0+random.gauss(0, 0.3),-0.5+random.gauss(0, 0.3),3], p.getQuaternionFromEuler([random.randint(0,360),random.randint(0,360),random.randint(0,360)]), physicsClientId=PYB_CLIENT) @@ -121,7 +131,7 @@ env.render() #### Sync the simulation ################################### - if ARGS.gui: + if gui: sync(i, START, env.TIMESTEP) #### Close the environment ################################# @@ -132,5 +142,22 @@ logger.save_as_csv("gnd") # Optional CSV save #### Plot the simulation results ########################### - if ARGS.plot: + if plot: logger.plot() + +if __name__ == "__main__": + #### Define and parse (optional) arguments for the script ## + parser = argparse.ArgumentParser(description='Ground effect script using CtrlAviary and DSLPIDControl') + parser.add_argument('--gui', default=DEFAULT_GUI, type=str2bool, help='Whether to use PyBullet GUI (default: True)', metavar='') + parser.add_argument('--record_video', default=DEFAULT_RECORD_VIDEO, type=str2bool, help='Whether to record a video (default: False)', metavar='') + parser.add_argument('--plot', default=DEFAULT_PLOT, type=str2bool, help='Whether to plot the simulation results (default: True)', metavar='') + parser.add_argument('--user_debug_gui', default=DEFAULT_USER_DEBUG_GUI, type=str2bool, help='Whether to add debug lines and parameters to the GUI (default: False)', metavar='') + parser.add_argument('--aggregate', default=DEFAULT_AGGREGATE, type=str2bool, help='Whether to aggregate physics steps (default: False)', metavar='') + parser.add_argument('--obstacles', default=DEFAULT_OBSTACLES, type=str2bool, help='Whether to add obstacles to the environment (default: True)', metavar='') + parser.add_argument('--simulation_freq_hz', default=DEFAULT_SIMULATION_FREQ_HZ, type=int, help='Simulation frequency in Hz (default: 240)', metavar='') + parser.add_argument('--control_freq_hz', default=DEFAULT_CONTROL_FREQ_HZ, type=int, help='Control frequency in Hz (default: 48)', metavar='') + parser.add_argument('--duration_sec', default=DEFAULT_DURATION_SEC, type=int, help='Duration of the simulation in seconds (default: 5)', metavar='') + parser.add_argument('--output_folder', default=DEFAULT_OUTPUT_FOLDER, type=str, help='Folder where to save logs (default: "results")', metavar='') + ARGS = parser.parse_args() + + run(**vars(ARGS)) diff --git a/examples/learn.py b/gym_pybullet_drones/examples/learn.py similarity index 81% rename from examples/learn.py rename to gym_pybullet_drones/examples/learn.py index e00c1fae3..5210791c7 100644 --- a/examples/learn.py +++ b/gym_pybullet_drones/examples/learn.py @@ -31,12 +31,11 @@ from gym_pybullet_drones.envs.single_agent_rl.TakeoffAviary import TakeoffAviary from gym_pybullet_drones.utils.utils import sync, str2bool -if __name__ == "__main__": +DEFAULT_RLLIB = False +DEFAULT_GUI = True +DEFAULT_OUTPUT_FOLDER = 'results' - #### Define and parse (optional) arguments for the script ## - parser = argparse.ArgumentParser(description='Single agent reinforcement learning example script using TakeoffAviary') - parser.add_argument('--rllib', default=False, type=str2bool, help='Whether to use RLlib PPO in place of stable-baselines A2C (default: False)', metavar='') - ARGS = parser.parse_args() +def run(rllib=DEFAULT_RLLIB,output_folder=DEFAULT_OUTPUT_FOLDER, gui=DEFAULT_GUI, plot=True): #### Check the environment's spaces ######################## env = gym.make("takeoff-aviary-v0") @@ -48,7 +47,7 @@ ) #### Train the model ####################################### - if not ARGS.rllib: + if not rllib: model = A2C(MlpPolicy, env, verbose=1 @@ -75,16 +74,17 @@ ray.shutdown() #### Show (and record a video of) the model's performance ## - env = TakeoffAviary(gui=True, + env = TakeoffAviary(gui=gui, record=False ) logger = Logger(logging_freq_hz=int(env.SIM_FREQ/env.AGGR_PHY_STEPS), - num_drones=1 + num_drones=1, + output_folder=output_folder ) obs = env.reset() start = time.time() for i in range(3*env.SIM_FREQ): - if not ARGS.rllib: + if not rllib: action, _states = model.predict(obs, deterministic=True ) @@ -103,4 +103,16 @@ if done: obs = env.reset() env.close() - logger.plot() + + if plot: + logger.plot() + +if __name__ == "__main__": + #### Define and parse (optional) arguments for the script ## + parser = argparse.ArgumentParser(description='Single agent reinforcement learning example script using TakeoffAviary') + parser.add_argument('--rllib', default=DEFAULT_RLLIB, type=str2bool, help='Whether to use RLlib PPO in place of stable-baselines A2C (default: False)', metavar='') + parser.add_argument('--gui', default=DEFAULT_GUI, type=str2bool, help='Whether to use PyBullet GUI (default: True)', metavar='') + parser.add_argument('--output_folder', default=DEFAULT_OUTPUT_FOLDER, type=str, help='Folder where to save logs (default: "results")', metavar='') + ARGS = parser.parse_args() + + run(**vars(ARGS)) diff --git a/examples/velocity.py b/gym_pybullet_drones/examples/velocity.py similarity index 56% rename from examples/velocity.py rename to gym_pybullet_drones/examples/velocity.py index 17c957d8a..49e59a14f 100644 --- a/examples/velocity.py +++ b/gym_pybullet_drones/examples/velocity.py @@ -34,23 +34,32 @@ from gym_pybullet_drones.envs.VelocityAviary import VelocityAviary -if __name__ == "__main__": - - #### Define and parse (optional) arguments for the script ## - parser = argparse.ArgumentParser(description='Velocity control example using VelocityAviary') - parser.add_argument('--drone', default="cf2x", type=DroneModel, help='Drone model (default: CF2X)', metavar='', choices=DroneModel) - parser.add_argument('--gui', default=True, type=str2bool, help='Whether to use PyBullet GUI (default: True)', metavar='') - parser.add_argument('--record_video', default=False, type=str2bool, help='Whether to record a video (default: False)', metavar='') - parser.add_argument('--plot', default=True, type=str2bool, help='Whether to plot the simulation results (default: True)', metavar='') - parser.add_argument('--user_debug_gui', default=False, type=str2bool, help='Whether to add debug lines and parameters to the GUI (default: False)', metavar='') - parser.add_argument('--aggregate', default=True, type=str2bool, help='Whether to aggregate physics steps (default: False)', metavar='') - parser.add_argument('--obstacles', default=False, type=str2bool, help='Whether to add obstacles to the environment (default: True)', metavar='') - parser.add_argument('--simulation_freq_hz', default=240, type=int, help='Simulation frequency in Hz (default: 240)', metavar='') - parser.add_argument('--control_freq_hz', default=48, type=int, help='Control frequency in Hz (default: 48)', metavar='') - parser.add_argument('--duration_sec', default=5, type=int, help='Duration of the simulation in seconds (default: 5)', metavar='') - ARGS = parser.parse_args() - - #### Initialize the simulation ############################# +DEFAULT_DRONE = DroneModel("cf2x") +DEFAULT_GUI = True +DEFAULT_RECORD_VIDEO = False +DEFAULT_PLOT = True +DEFAULT_USER_DEBUG_GUI = False +DEFAULT_AGGREGATE = True +DEFAULT_OBSTACLES = False +DEFAULT_SIMULATION_FREQ_HZ = 240 +DEFAULT_CONTROL_FREQ_HZ = 48 +DEFAULT_DURATION_SEC = 5 +DEFAULT_OUTPUT_FOLDER = 'results' + +def run( + drone=DEFAULT_DRONE, + gui=DEFAULT_GUI, + record_video=DEFAULT_RECORD_VIDEO, + plot=DEFAULT_PLOT, + user_debug_gui=DEFAULT_USER_DEBUG_GUI, + aggregate=DEFAULT_AGGREGATE, + obstacles=DEFAULT_OBSTACLES, + simulation_freq_hz=DEFAULT_SIMULATION_FREQ_HZ, + control_freq_hz=DEFAULT_CONTROL_FREQ_HZ, + duration_sec=DEFAULT_DURATION_SEC, + output_folder=DEFAULT_OUTPUT_FOLDER + ): + #### Initialize the simulation ############################# INIT_XYZS = np.array([ [ 0, 0, .1], [.3, 0, .1], @@ -63,22 +72,22 @@ [0, 0, np.pi/4], [0, 0, np.pi/2] ]) - AGGR_PHY_STEPS = int(ARGS.simulation_freq_hz/ARGS.control_freq_hz) if ARGS.aggregate else 1 + AGGR_PHY_STEPS = int(simulation_freq_hz/control_freq_hz) if aggregate else 1 PHY = Physics.PYB #### Create the environment ################################ - env = VelocityAviary(drone_model=ARGS.drone, + env = VelocityAviary(drone_model=drone, num_drones=4, initial_xyzs=INIT_XYZS, initial_rpys=INIT_RPYS, physics=Physics.PYB, neighbourhood_radius=10, - freq=ARGS.simulation_freq_hz, + freq=simulation_freq_hz, aggregate_phy_steps=AGGR_PHY_STEPS, - gui=ARGS.gui, - record=ARGS.record_video, - obstacles=ARGS.obstacles, - user_debug_gui=ARGS.user_debug_gui + gui=gui, + record=record_video, + obstacles=obstacles, + user_debug_gui=user_debug_gui ) #### Obtain the PyBullet Client ID from the environment #### @@ -86,8 +95,8 @@ DRONE_IDS = env.getDroneIds() #### Compute number of control steps in the simlation ###### - PERIOD = ARGS.duration_sec - NUM_WP = ARGS.control_freq_hz*PERIOD + PERIOD = duration_sec + NUM_WP = control_freq_hz*PERIOD wp_counters = np.array([0 for i in range(4)]) #### Initialize the velocity target ######################## @@ -99,15 +108,16 @@ TARGET_VEL[3, i, :] = [0, 1, 0.5, 0.99] if i < (NUM_WP/8+3*NUM_WP/6) else [0, -1, -0.5, 0.99] #### Initialize the logger ################################# - logger = Logger(logging_freq_hz=int(ARGS.simulation_freq_hz/AGGR_PHY_STEPS), - num_drones=4 + logger = Logger(logging_freq_hz=int(simulation_freq_hz/AGGR_PHY_STEPS), + num_drones=4, + output_folder=output_folder ) #### Run the simulation #################################### - CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ/ARGS.control_freq_hz)) + CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ/control_freq_hz)) action = {str(i): np.array([0,0,0,0]) for i in range(4)} START = time.time() - for i in range(0, int(ARGS.duration_sec*env.SIM_FREQ), AGGR_PHY_STEPS): + for i in range(0, int(duration_sec*env.SIM_FREQ), AGGR_PHY_STEPS): ############################################################ # for j in range(3): env._showDroneLocalAxes(j) @@ -139,7 +149,7 @@ env.render() #### Sync the simulation ################################### - if ARGS.gui: + if gui: sync(i, START, env.TIMESTEP) #### Close the environment ################################# @@ -147,5 +157,24 @@ #### Plot the simulation results ########################### logger.save_as_csv("vel") # Optional CSV save - if ARGS.plot: + if plot: logger.plot() + +if __name__ == "__main__": + #### Define and parse (optional) arguments for the script ## + parser = argparse.ArgumentParser(description='Velocity control example using VelocityAviary') + parser.add_argument('--drone', default=DEFAULT_DRONE, type=DroneModel, help='Drone model (default: CF2X)', metavar='', choices=DroneModel) + parser.add_argument('--gui', default=DEFAULT_GUI, type=str2bool, help='Whether to use PyBullet GUI (default: True)', metavar='') + parser.add_argument('--record_video', default=DEFAULT_RECORD_VIDEO, type=str2bool, help='Whether to record a video (default: False)', metavar='') + parser.add_argument('--plot', default=DEFAULT_PLOT, type=str2bool, help='Whether to plot the simulation results (default: True)', metavar='') + parser.add_argument('--user_debug_gui', default=DEFAULT_USER_DEBUG_GUI, type=str2bool, help='Whether to add debug lines and parameters to the GUI (default: False)', metavar='') + parser.add_argument('--aggregate', default=DEFAULT_AGGREGATE, type=str2bool, help='Whether to aggregate physics steps (default: False)', metavar='') + parser.add_argument('--obstacles', default=DEFAULT_OBSTACLES, type=str2bool, help='Whether to add obstacles to the environment (default: True)', metavar='') + parser.add_argument('--simulation_freq_hz', default=DEFAULT_SIMULATION_FREQ_HZ, type=int, help='Simulation frequency in Hz (default: 240)', metavar='') + parser.add_argument('--control_freq_hz', default=DEFAULT_CONTROL_FREQ_HZ, type=int, help='Control frequency in Hz (default: 48)', metavar='') + parser.add_argument('--duration_sec', default=DEFAULT_DURATION_SEC, type=int, help='Duration of the simulation in seconds (default: 5)', metavar='') + parser.add_argument('--output_folder', default=DEFAULT_OUTPUT_FOLDER, type=str, help='Folder where to save logs (default: "results")', metavar='') + ARGS = parser.parse_args() + + run(**vars(ARGS)) + diff --git a/gym_pybullet_drones/utils/Logger.py b/gym_pybullet_drones/utils/Logger.py index 6b09ee7c1..6527fed0f 100644 --- a/gym_pybullet_drones/utils/Logger.py +++ b/gym_pybullet_drones/utils/Logger.py @@ -1,5 +1,6 @@ import os from datetime import datetime +from telnetlib import OUTMRK from cycler import cycler import numpy as np import matplotlib.pyplot as plt @@ -18,8 +19,9 @@ class Logger(object): def __init__(self, logging_freq_hz: int, + output_folder: str, num_drones: int=1, - duration_sec: int=0 + duration_sec: int=0, ): """Logger class __init__ method. @@ -36,6 +38,9 @@ def __init__(self, Used to preallocate the log arrays (improves performance). """ + self.OUTPUT_FOLDER = output_folder + if not os.path.exists(self.OUTPUT_FOLDER): + os.mkdir(self.OUTPUT_FOLDER) self.LOGGING_FREQ_HZ = logging_freq_hz self.NUM_DRONES = num_drones self.PREALLOCATED_ARRAYS = False if duration_sec == 0 else True @@ -117,7 +122,7 @@ def log(self, def save(self): """Save the logs to file. """ - with open(os.path.dirname(os.path.abspath(__file__))+"/../../files/logs/save-flight-"+datetime.now().strftime("%m.%d.%Y_%H.%M.%S")+".npy", 'wb') as out_file: + with open(os.path.join(self.OUTPUT_FOLDER, "save-flight-"+datetime.now().strftime("%m.%d.%Y_%H.%M.%S")+".npy"), 'wb') as out_file: np.savez(out_file, timestamps=self.timestamps, states=self.states, controls=self.controls) ################################################################################ @@ -133,7 +138,7 @@ def save_as_csv(self, Added to the foldername. """ - csv_dir = os.environ.get('HOME')+"/Desktop/save-flight-"+comment+"-"+datetime.now().strftime("%m.%d.%Y_%H.%M.%S") + csv_dir = os.path.join(self.OUTPUT_FOLDER, "save-flight-"+comment+"-"+datetime.now().strftime("%m.%d.%Y_%H.%M.%S")) if not os.path.exists(csv_dir): os.makedirs(csv_dir+'/') t = np.arange(0, self.timestamps.shape[1]/self.LOGGING_FREQ_HZ, 1/self.LOGGING_FREQ_HZ) diff --git a/setup.py b/setup.py index aef2988ab..f62fef191 100644 --- a/setup.py +++ b/setup.py @@ -1,6 +1,12 @@ -from setuptools import setup +import setuptools +from gym_pybullet_drones import * -setup(name='gym_pybullet_drones', +# read the contents of your README file +from pathlib import Path +this_directory = Path(__file__).parent +long_description = (this_directory / "README.md").read_text() + +setuptools.setup(name='gym_pybullet_drones', version='1.0.0', install_requires=[ 'numpy', @@ -11,5 +17,14 @@ 'pybullet', 'stable_baselines3', 'ray[rllib]' - ] -) + ], + packages=setuptools.find_packages(where="gym_pybullet_drones") + package_dir={'':'gym_pybullet_drones'}, + package_data={'gym_pybullet_drones': ['assets/*.*']}, + long_description=long_description, + long_description_content_type='test/markdown', + license='MIT License\nCopyright (c) 2020 Jacopo Panerati', + entry_points = { + 'console_scripts': ['downwash=examples.downwash:main'], + } +) \ No newline at end of file diff --git a/tests/test_build.py b/tests/test_build.py new file mode 100644 index 000000000..97c13a071 --- /dev/null +++ b/tests/test_build.py @@ -0,0 +1,32 @@ +import unittest + +class TestBuild(unittest.TestCase): + + def test_imports(self): + from gym_pybullet_drones.control.BaseControl import BaseControl + from gym_pybullet_drones.utils.enums import DroneModel + import gym_pybullet_drones.envs.multi_agent_rl.BaseMultiagentAviary + + def test_compare(self): + from gym_pybullet_drones.examples.compare import run + run(gui=False,plot=False) + def test_downwash(self): + from gym_pybullet_drones.examples.downwash import run + run(gui=False,plot=False) + def test_fly(self): + from gym_pybullet_drones.examples.fly import run + run(gui=False,plot=False) + def test_groundeffect(self): + from gym_pybullet_drones.examples.groundeffect import run + run(gui=False,plot=False) + def test_learn(self): + from gym_pybullet_drones.examples.learn import run + run(gui=False,plot=False) + def test_velocity(self): + from gym_pybullet_drones.examples.velocity import run + run(gui=False,plot=False) + + + +if __name__ == '__main__': + unittest.main() \ No newline at end of file