diff --git a/.github/workflows/push.yml b/.github/workflows/push.yml new file mode 100644 index 000000000..0eeffd25b --- /dev/null +++ b/.github/workflows/push.yml @@ -0,0 +1,35 @@ +# This workflow will upload a Python Package using Twine when a release is created +# For more information see: https://help.github.com/en/actions/language-and-framework-guides/using-python-with-github-actions#publishing-to-package-registries + +# This workflow uses actions that are not certified by GitHub. +# They are provided by a third-party and are governed by +# separate terms of service, privacy policy, and support +# documentation. + +name: Test package quality + +on: push + +permissions: + contents: read + +jobs: + run_tests: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - name: Set up Python + uses: actions/setup-python@v3 + with: + python-version: '3.8' + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install pytest + - name: Install package + run: | + pip install -e . + - name: Unit tests + run: | + python -m pytest tests/ + rm -rf tmp/ diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml new file mode 100644 index 000000000..bb2923cfa --- /dev/null +++ b/.github/workflows/release.yml @@ -0,0 +1,70 @@ +# This workflow will upload a Python Package using Twine when a release is created +# For more information see: https://help.github.com/en/actions/language-and-framework-guides/using-python-with-github-actions#publishing-to-package-registries + +# This workflow uses actions that are not certified by GitHub. +# They are provided by a third-party and are governed by +# separate terms of service, privacy policy, and support +# documentation. + +name: Publish Python package to PyPi + +on: + release: + types: [published] + +permissions: + contents: read + +jobs: + run_tests: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - name: Set up Python + uses: actions/setup-python@v3 + with: + python-version: '3.8' + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install pytest + - name: Install package + run: | + pip install -e . + - name: Unit tests + run: | + python -m pytest tests/ + rm -rf tmp/ + + deploy: + needs: run_tests + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - name: Set up Python + uses: actions/setup-python@v3 + with: + python-version: '3.8' + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install poetry + - name: Build package + run: | + rm -rf dist/ + poetry build + - name: Publish distribution 📦 to Test PyPI + uses: pypa/gh-action-pypi-publish@master + with: + password: ${{ secrets.TEST_PYPI_API_TOKEN }} + repository_url: https://test.pypi.org/legacy/ + # - name: pypi-publish + # # You may pin to the exact commit or the version. + # uses: pypa/gh-action-pypi-publish@release/v1 + # with: + # # PyPI user + # user: __token__ + # # Password for your PyPI user or an access token + # password: ${{ secrets.PYPI_API_TOKEN }} + # # The repository URL to use + # repository_url: https://github.com/utiasDSL/gym-pybullet-drones diff --git a/.gitignore b/.gitignore index 58a222e8d..d2316598d 100644 --- a/.gitignore +++ b/.gitignore @@ -6,6 +6,8 @@ examples/test.py # NumPy saves and videos files/logs/*.npy files/videos/*.mp4 +results/ +tmp/ # Learning results experiments/learning/results/save-* @@ -41,6 +43,7 @@ share/python-wheels/ .installed.cfg *.egg MANIFEST +.vscode/ # PyInstaller # Usually these files are written by a python script from a template diff --git a/assignments/aer1216_fall2020_hw2_sim.py b/assignments/aer1216_fall2020_hw2_sim.py index f1fd59d2d..e28ada76b 100644 --- a/assignments/aer1216_fall2020_hw2_sim.py +++ b/assignments/aer1216_fall2020_hw2_sim.py @@ -21,7 +21,7 @@ from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary from gym_pybullet_drones.utils.Logger import Logger from gym_pybullet_drones.utils.utils import sync -from gym_pybullet_drones.envs.BaseAviary import DroneModel +from gym_pybullet_drones.utils.enums import DroneModel from aer1216_fall2020_hw2_ctrl import HW2Control DURATION = 10 diff --git a/build_project.sh b/build_project.sh new file mode 100755 index 000000000..f6acda8c6 --- /dev/null +++ b/build_project.sh @@ -0,0 +1,8 @@ +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 +rm -rf results +cd .. \ No newline at end of file diff --git a/experiments/learning/multiagent.py b/experiments/learning/multiagent.py index 66b606089..2ae74f620 100644 --- a/experiments/learning/multiagent.py +++ b/experiments/learning/multiagent.py @@ -48,7 +48,7 @@ from ray.rllib.policy.sample_batch import SampleBatch from ray.rllib.env.multi_agent_env import ENV_STATE -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics +from gym_pybullet_drones.utils.enums import DroneModel, Physics from gym_pybullet_drones.envs.multi_agent_rl.FlockAviary import FlockAviary from gym_pybullet_drones.envs.multi_agent_rl.LeaderFollowerAviary import LeaderFollowerAviary from gym_pybullet_drones.envs.multi_agent_rl.MeetupAviary import MeetupAviary diff --git a/experiments/learning/singleagent.py b/experiments/learning/singleagent.py index 9860ce4bd..15ea0c9a3 100644 --- a/experiments/learning/singleagent.py +++ b/experiments/learning/singleagent.py @@ -56,72 +56,79 @@ EPISODE_REWARD_THRESHOLD = -0 # Upperbound: rewards are always negative, but non-zero """float: Reward threshold to halt the script.""" -if __name__ == "__main__": +DEFAULT_ENV = 'hover' +DEFAULT_ALGO = 'ppo' +DEFAULT_OBS = ObservationType('kin') +DEFAULT_ACT = ActionType('one_d_rpm') +DEFAULT_CPU = 1 +DEFAULT_STEPS = 35000 +DEFAULT_OUTPUT_FOLDER = 'results' - #### Define and parse (optional) arguments for the script ## - parser = argparse.ArgumentParser(description='Single agent reinforcement learning experiments script') - parser.add_argument('--env', default='hover', type=str, choices=['takeoff', 'hover', 'flythrugate', 'tune'], help='Task (default: hover)', metavar='') - parser.add_argument('--algo', default='ppo', type=str, choices=['a2c', 'ppo', 'sac', 'td3', 'ddpg'], help='RL agent (default: ppo)', metavar='') - parser.add_argument('--obs', default='kin', type=ObservationType, help='Observation space (default: kin)', metavar='') - parser.add_argument('--act', default='one_d_rpm', type=ActionType, help='Action space (default: one_d_rpm)', metavar='') - parser.add_argument('--cpu', default='1', type=int, help='Number of training environments (default: 1)', metavar='') - ARGS = parser.parse_args() +def run( + env=DEFAULT_ENV, + algo=DEFAULT_ALGO, + obs=DEFAULT_OBS, + act=DEFAULT_ACT, + cpu=DEFAULT_CPU, + steps=DEFAULT_STEPS, + output_folder=DEFAULT_OUTPUT_FOLDER +): #### Save directory ######################################## - filename = os.path.dirname(os.path.abspath(__file__))+'/results/save-'+ARGS.env+'-'+ARGS.algo+'-'+ARGS.obs.value+'-'+ARGS.act.value+'-'+datetime.now().strftime("%m.%d.%Y_%H.%M.%S") + filename = os.path.join(output_folder, 'save-'+env+'-'+algo+'-'+obs.value+'-'+act.value+'-'+datetime.now().strftime("%m.%d.%Y_%H.%M.%S")) if not os.path.exists(filename): os.makedirs(filename+'/') #### Print out current git commit hash ##################### - if platform == "linux" or platform == "darwin": + if (platform == "linux" or platform == "darwin") and ('GITHUB_ACTIONS' not in os.environ.keys()): git_commit = subprocess.check_output(["git", "describe", "--tags"]).strip() with open(filename+'/git_commit.txt', 'w+') as f: f.write(str(git_commit)) #### Warning ############################################### - if ARGS.env == 'tune' and ARGS.act != ActionType.TUN: + if env == 'tune' and act != ActionType.TUN: print("\n\n\n[WARNING] TuneAviary is intended for use with ActionType.TUN\n\n\n") - if ARGS.act == ActionType.ONE_D_RPM or ARGS.act == ActionType.ONE_D_DYN or ARGS.act == ActionType.ONE_D_PID: + if act == ActionType.ONE_D_RPM or act == ActionType.ONE_D_DYN or act == ActionType.ONE_D_PID: print("\n\n\n[WARNING] Simplified 1D problem for debugging purposes\n\n\n") #### Errors ################################################ - if not ARGS.env in ['takeoff', 'hover']: + if not env in ['takeoff', 'hover']: print("[ERROR] 1D action space is only compatible with Takeoff and HoverAviary") exit() - if ARGS.act == ActionType.TUN and ARGS.env != 'tune' : + if act == ActionType.TUN and env != 'tune' : print("[ERROR] ActionType.TUN is only compatible with TuneAviary") exit() - if ARGS.algo in ['sac', 'td3', 'ddpg'] and ARGS.cpu!=1: + if algo in ['sac', 'td3', 'ddpg'] and cpu!=1: print("[ERROR] The selected algorithm does not support multiple environments") exit() #### Uncomment to debug slurm scripts ###################### # exit() - env_name = ARGS.env+"-aviary-v0" - sa_env_kwargs = dict(aggregate_phy_steps=shared_constants.AGGR_PHY_STEPS, obs=ARGS.obs, act=ARGS.act) - # train_env = gym.make(env_name, aggregate_phy_steps=shared_constants.AGGR_PHY_STEPS, obs=ARGS.obs, act=ARGS.act) # single environment instead of a vectorized one + env_name = env+"-aviary-v0" + sa_env_kwargs = dict(aggregate_phy_steps=shared_constants.AGGR_PHY_STEPS, obs=obs, act=act) + # train_env = gym.make(env_name, aggregate_phy_steps=shared_constants.AGGR_PHY_STEPS, obs=obs, act=act) # single environment instead of a vectorized one if env_name == "takeoff-aviary-v0": train_env = make_vec_env(TakeoffAviary, env_kwargs=sa_env_kwargs, - n_envs=ARGS.cpu, + n_envs=cpu, seed=0 ) if env_name == "hover-aviary-v0": train_env = make_vec_env(HoverAviary, env_kwargs=sa_env_kwargs, - n_envs=ARGS.cpu, + n_envs=cpu, seed=0 ) if env_name == "flythrugate-aviary-v0": train_env = make_vec_env(FlyThruGateAviary, env_kwargs=sa_env_kwargs, - n_envs=ARGS.cpu, + n_envs=cpu, seed=0 ) if env_name == "tune-aviary-v0": train_env = make_vec_env(TuneAviary, env_kwargs=sa_env_kwargs, - n_envs=ARGS.cpu, + n_envs=cpu, seed=0 ) print("[INFO] Action space:", train_env.action_space) @@ -132,25 +139,25 @@ onpolicy_kwargs = dict(activation_fn=torch.nn.ReLU, net_arch=[512, 512, dict(vf=[256, 128], pi=[256, 128])] ) # or None - if ARGS.algo == 'a2c': + if algo == 'a2c': model = A2C(a2cppoMlpPolicy, train_env, policy_kwargs=onpolicy_kwargs, tensorboard_log=filename+'/tb/', verbose=1 - ) if ARGS.obs == ObservationType.KIN else A2C(a2cppoCnnPolicy, + ) if obs == ObservationType.KIN else A2C(a2cppoCnnPolicy, train_env, policy_kwargs=onpolicy_kwargs, tensorboard_log=filename+'/tb/', verbose=1 ) - if ARGS.algo == 'ppo': + if algo == 'ppo': model = PPO(a2cppoMlpPolicy, train_env, policy_kwargs=onpolicy_kwargs, tensorboard_log=filename+'/tb/', verbose=1 - ) if ARGS.obs == ObservationType.KIN else PPO(a2cppoCnnPolicy, + ) if obs == ObservationType.KIN else PPO(a2cppoCnnPolicy, train_env, policy_kwargs=onpolicy_kwargs, tensorboard_log=filename+'/tb/', @@ -161,37 +168,37 @@ offpolicy_kwargs = dict(activation_fn=torch.nn.ReLU, net_arch=[512, 512, 256, 128] ) # or None # or dict(net_arch=dict(qf=[256, 128, 64, 32], pi=[256, 128, 64, 32])) - if ARGS.algo == 'sac': + if algo == 'sac': model = SAC(sacMlpPolicy, train_env, policy_kwargs=offpolicy_kwargs, tensorboard_log=filename+'/tb/', verbose=1 - ) if ARGS.obs==ObservationType.KIN else SAC(sacCnnPolicy, + ) if obs==ObservationType.KIN else SAC(sacCnnPolicy, train_env, policy_kwargs=offpolicy_kwargs, tensorboard_log=filename+'/tb/', verbose=1 ) - if ARGS.algo == 'td3': + if algo == 'td3': model = TD3(td3ddpgMlpPolicy, train_env, policy_kwargs=offpolicy_kwargs, tensorboard_log=filename+'/tb/', verbose=1 - ) if ARGS.obs==ObservationType.KIN else TD3(td3ddpgCnnPolicy, + ) if obs==ObservationType.KIN else TD3(td3ddpgCnnPolicy, train_env, policy_kwargs=offpolicy_kwargs, tensorboard_log=filename+'/tb/', verbose=1 ) - if ARGS.algo == 'ddpg': + if algo == 'ddpg': model = DDPG(td3ddpgMlpPolicy, train_env, policy_kwargs=offpolicy_kwargs, tensorboard_log=filename+'/tb/', verbose=1 - ) if ARGS.obs==ObservationType.KIN else DDPG(td3ddpgCnnPolicy, + ) if obs==ObservationType.KIN else DDPG(td3ddpgCnnPolicy, train_env, policy_kwargs=offpolicy_kwargs, tensorboard_log=filename+'/tb/', @@ -199,13 +206,13 @@ ) #### Create eveluation environment ######################### - if ARGS.obs == ObservationType.KIN: + if obs == ObservationType.KIN: eval_env = gym.make(env_name, aggregate_phy_steps=shared_constants.AGGR_PHY_STEPS, - obs=ARGS.obs, - act=ARGS.act + obs=obs, + act=act ) - elif ARGS.obs == ObservationType.RGB: + elif obs == ObservationType.RGB: if env_name == "takeoff-aviary-v0": eval_env = make_vec_env(TakeoffAviary, env_kwargs=sa_env_kwargs, @@ -242,11 +249,11 @@ verbose=1, best_model_save_path=filename+'/', log_path=filename+'/', - eval_freq=int(2000/ARGS.cpu), + eval_freq=int(2000/cpu), deterministic=True, render=False ) - model.learn(total_timesteps=35000, #int(1e12), + model.learn(total_timesteps=steps, #int(1e12), callback=eval_callback, log_interval=100, ) @@ -258,4 +265,19 @@ #### Print training progression ############################ with np.load(filename+'/evaluations.npz') as data: for j in range(data['timesteps'].shape[0]): - print(str(data['timesteps'][j])+","+str(data['results'][j][0][0])) + print(str(data['timesteps'][j])+","+str(data['results'][j][0])) + + +if __name__ == "__main__": + #### Define and parse (optional) arguments for the script ## + parser = argparse.ArgumentParser(description='Single agent reinforcement learning experiments script') + parser.add_argument('--env', default=DEFAULT_ENV, type=str, choices=['takeoff', 'hover', 'flythrugate', 'tune'], help='Task (default: hover)', metavar='') + parser.add_argument('--algo', default=DEFAULT_ALGO, type=str, choices=['a2c', 'ppo', 'sac', 'td3', 'ddpg'], help='RL agent (default: ppo)', metavar='') + parser.add_argument('--obs', default=DEFAULT_OBS, type=ObservationType, help='Observation space (default: kin)', metavar='') + parser.add_argument('--act', default=DEFAULT_ACT, type=ActionType, help='Action space (default: one_d_rpm)', metavar='') + parser.add_argument('--cpu', default=DEFAULT_CPU, type=int, help='Number of training environments (default: 1)', metavar='') + parser.add_argument('--steps', default=DEFAULT_STEPS, type=int, help='Number of training time steps (default: 35000)', 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/experiments/learning/test_multiagent.py b/experiments/learning/test_multiagent.py index 0e9a438d6..781ea9152 100644 --- a/experiments/learning/test_multiagent.py +++ b/experiments/learning/test_multiagent.py @@ -38,7 +38,7 @@ from ray.rllib.models import ModelCatalog from ray.rllib.policy.sample_batch import SampleBatch -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics +from gym_pybullet_drones.utils.enums import DroneModel, Physics from gym_pybullet_drones.envs.multi_agent_rl.FlockAviary import FlockAviary from gym_pybullet_drones.envs.multi_agent_rl.LeaderFollowerAviary import LeaderFollowerAviary from gym_pybullet_drones.envs.multi_agent_rl.MeetupAviary import MeetupAviary diff --git a/experiments/learning/test_singleagent.py b/experiments/learning/test_singleagent.py index c53f2ad72..59470afd2 100644 --- a/experiments/learning/test_singleagent.py +++ b/experiments/learning/test_singleagent.py @@ -38,25 +38,24 @@ from gym_pybullet_drones.envs.single_agent_rl.FlyThruGateAviary import FlyThruGateAviary from gym_pybullet_drones.envs.single_agent_rl.TuneAviary import TuneAviary from gym_pybullet_drones.envs.single_agent_rl.BaseSingleAgentAviary import ActionType, ObservationType +from gym_pybullet_drones.utils.utils import sync, str2bool import shared_constants -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('--exp', type=str, help='The experiment folder written as ./results/save-----', metavar='') - ARGS = parser.parse_args() +DEFAULT_GUI = True +DEFAULT_PLOT = True +DEFAULT_OUTPUT_FOLDER = 'results' +def run(exp, gui=DEFAULT_GUI, plot=DEFAULT_PLOT, output_folder=DEFAULT_OUTPUT_FOLDER): #### Load the model from file ############################## - algo = ARGS.exp.split("-")[2] + algo = exp.split("-")[2] - if os.path.isfile(ARGS.exp+'/success_model.zip'): - path = ARGS.exp+'/success_model.zip' - elif os.path.isfile(ARGS.exp+'/best_model.zip'): - path = ARGS.exp+'/best_model.zip' + if os.path.isfile(exp+'/success_model.zip'): + path = exp+'/success_model.zip' + elif os.path.isfile(exp+'/best_model.zip'): + path = exp+'/best_model.zip' else: - print("[ERROR]: no model under the specified path", ARGS.exp) + print("[ERROR]: no model under the specified path", exp) if algo == 'a2c': model = A2C.load(path) if algo == 'ppo': @@ -69,23 +68,23 @@ model = DDPG.load(path) #### Parameters to recreate the environment ################ - env_name = ARGS.exp.split("-")[1]+"-aviary-v0" - OBS = ObservationType.KIN if ARGS.exp.split("-")[3] == 'kin' else ObservationType.RGB - if ARGS.exp.split("-")[4] == 'rpm': + env_name = exp.split("-")[1]+"-aviary-v0" + OBS = ObservationType.KIN if exp.split("-")[3] == 'kin' else ObservationType.RGB + if exp.split("-")[4] == 'rpm': ACT = ActionType.RPM - elif ARGS.exp.split("-")[4] == 'dyn': + elif exp.split("-")[4] == 'dyn': ACT = ActionType.DYN - elif ARGS.exp.split("-")[4] == 'pid': + elif exp.split("-")[4] == 'pid': ACT = ActionType.PID - elif ARGS.exp.split("-")[4] == 'vel': + elif exp.split("-")[4] == 'vel': ACT = ActionType.VEL - elif ARGS.exp.split("-")[4] == 'tun': + elif exp.split("-")[4] == 'tun': ACT = ActionType.TUN - elif ARGS.exp.split("-")[4] == 'one_d_rpm': + elif exp.split("-")[4] == 'one_d_rpm': ACT = ActionType.ONE_D_RPM - elif ARGS.exp.split("-")[4] == 'one_d_dyn': + elif exp.split("-")[4] == 'one_d_dyn': ACT = ActionType.ONE_D_DYN - elif ARGS.exp.split("-")[4] == 'one_d_pid': + elif exp.split("-")[4] == 'one_d_pid': ACT = ActionType.ONE_D_PID #### Evaluate the model #################################### @@ -102,14 +101,15 @@ #### Show, record a video, and log the model's performance # test_env = gym.make(env_name, - gui=True, + gui=gui, record=False, aggregate_phy_steps=shared_constants.AGGR_PHY_STEPS, obs=OBS, act=ACT ) logger = Logger(logging_freq_hz=int(test_env.SIM_FREQ/test_env.AGGR_PHY_STEPS), - num_drones=1 + num_drones=1, + output_folder=output_folder ) obs = test_env.reset() start = time.time() @@ -129,10 +129,22 @@ # if done: obs = test_env.reset() # OPTIONAL EPISODE HALT test_env.close() logger.save_as_csv("sa") # Optional CSV save - logger.plot() + if plot: + logger.plot() - # with np.load(ARGS.exp+'/evaluations.npz') as data: + # with np.load(exp+'/evaluations.npz') as data: # print(data.files) # print(data['timesteps']) # print(data['results']) # print(data['ep_lengths']) + +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('--exp', type=str, help='The experiment folder written as ./results/save-----', metavar='') + parser.add_argument('--gui', default=DEFAULT_GUI, type=str2bool, help='Whether to use PyBullet GUI (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('--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/experiments/performance/script.py b/experiments/performance/script.py index f78ba1e13..ade75fd3f 100644 --- a/experiments/performance/script.py +++ b/experiments/performance/script.py @@ -14,7 +14,7 @@ import pybullet as p import matplotlib.pyplot as plt -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics +from gym_pybullet_drones.utils.enums import DroneModel, Physics from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary from gym_pybullet_drones.envs.VisionAviary import VisionAviary from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl 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/gym_pybullet_drones/control/BaseControl.py b/gym_pybullet_drones/control/BaseControl.py index e571d54e3..24b43163e 100644 --- a/gym_pybullet_drones/control/BaseControl.py +++ b/gym_pybullet_drones/control/BaseControl.py @@ -1,12 +1,9 @@ import os -import math import numpy as np -import pybullet as p -from enum import Enum import xml.etree.ElementTree as etxml -from scipy.spatial.transform import Rotation +import pkg_resources -from gym_pybullet_drones.envs.BaseAviary import DroneModel, BaseAviary +from gym_pybullet_drones.utils.enums import DroneModel class BaseControl(object): """Base class for control. @@ -202,7 +199,8 @@ def _getURDFParameter(self, """ #### Get the XML tree of the drone model to control ######## URDF = self.DRONE_MODEL.value + ".urdf" - URDF_TREE = etxml.parse(os.path.dirname(os.path.abspath(__file__))+"/../assets/"+URDF).getroot() + path = pkg_resources.resource_filename('gym_pybullet_drones', 'assets/'+URDF) + URDF_TREE = etxml.parse(path).getroot() #### Find and return the desired parameter ################# if parameter_name == 'm': return float(URDF_TREE[1][0][1].attrib['value']) diff --git a/gym_pybullet_drones/control/DSLPIDControl.py b/gym_pybullet_drones/control/DSLPIDControl.py index 64a291c2a..e8265f918 100644 --- a/gym_pybullet_drones/control/DSLPIDControl.py +++ b/gym_pybullet_drones/control/DSLPIDControl.py @@ -4,7 +4,7 @@ from scipy.spatial.transform import Rotation from gym_pybullet_drones.control.BaseControl import BaseControl -from gym_pybullet_drones.envs.BaseAviary import DroneModel, BaseAviary +from gym_pybullet_drones.utils.enums import DroneModel class DSLPIDControl(BaseControl): """PID control class for Crazyflies. diff --git a/gym_pybullet_drones/control/SimplePIDControl.py b/gym_pybullet_drones/control/SimplePIDControl.py index 3dfcc125a..776ab137e 100644 --- a/gym_pybullet_drones/control/SimplePIDControl.py +++ b/gym_pybullet_drones/control/SimplePIDControl.py @@ -1,9 +1,8 @@ -import math import numpy as np import pybullet as p from gym_pybullet_drones.control.BaseControl import BaseControl -from gym_pybullet_drones.envs.BaseAviary import DroneModel, BaseAviary +from gym_pybullet_drones.utils.enums import DroneModel from gym_pybullet_drones.utils.utils import nnlsRPM class SimplePIDControl(BaseControl): diff --git a/gym_pybullet_drones/envs/BaseAviary.py b/gym_pybullet_drones/envs/BaseAviary.py index 045ce3b14..30bdfc4c6 100644 --- a/gym_pybullet_drones/envs/BaseAviary.py +++ b/gym_pybullet_drones/envs/BaseAviary.py @@ -3,8 +3,8 @@ import time import collections from datetime import datetime -from enum import Enum import xml.etree.ElementTree as etxml +import pkg_resources from PIL import Image # import pkgutil # egl = pkgutil.get_loader('eglRenderer') @@ -12,37 +12,9 @@ import pybullet as p import pybullet_data import gym +from gym_pybullet_drones.utils.enums import DroneModel, Physics, ImageType -class DroneModel(Enum): - """Drone models enumeration class.""" - CF2X = "cf2x" # Bitcraze Craziflie 2.0 in the X configuration - CF2P = "cf2p" # Bitcraze Craziflie 2.0 in the + configuration - HB = "hb" # Generic quadrotor (with AscTec Hummingbird inertial properties) - -################################################################################ - -class Physics(Enum): - """Physics implementations enumeration class.""" - - PYB = "pyb" # Base PyBullet physics update - DYN = "dyn" # Update with an explicit model of the dynamics - PYB_GND = "pyb_gnd" # PyBullet physics update with ground effect - PYB_DRAG = "pyb_drag" # PyBullet physics update with drag - PYB_DW = "pyb_dw" # PyBullet physics update with downwash - PYB_GND_DRAG_DW = "pyb_gnd_drag_dw" # PyBullet physics update with ground effect, drag, and downwash - -################################################################################ - -class ImageType(Enum): - """Camera capture image type enumeration class.""" - - RGB = 0 # Red, green, blue (and alpha) - DEP = 1 # Depth - SEG = 2 # Segmentation by object id - BW = 3 # Black and white - -################################################################################ class BaseAviary(gym.Env): """Base class for "drone aviary" Gym environments.""" @@ -65,7 +37,8 @@ def __init__(self, obstacles=False, user_debug_gui=True, vision_attributes=False, - dynamics_attributes=False + dynamics_attributes=False, + output_folder='results' ): """Initialization of a generic aviary environment. @@ -119,6 +92,7 @@ def __init__(self, self.OBSTACLES = obstacles self.USER_DEBUG = user_debug_gui self.URDF = self.DRONE_MODEL.value + ".urdf" + self.OUTPUT_FOLDER = output_folder #### Load the drone properties from the .urdf file ######### self.M, \ self.L, \ @@ -163,7 +137,8 @@ def __init__(self, print("[ERROR] in BaseAviary.__init__(), aggregate_phy_steps incompatible with the desired video capture frame rate ({:f}Hz)".format(self.IMG_FRAME_PER_SEC)) exit() if self.RECORD: - self.ONBOARD_IMG_PATH = os.path.dirname(os.path.abspath(__file__))+"/../../files/videos/onboard-"+datetime.now().strftime("%m.%d.%Y_%H.%M.%S")+"/" + # TODO: This doesn't appear to work in general + self.ONBOARD_IMG_PATH = os.path.join(self.OUTPUT_FOLDER, "recording_" + datetime.now().strftime("%m.%d.%Y_%H.%M.%S")) os.makedirs(os.path.dirname(self.ONBOARD_IMG_PATH), exist_ok=True) #### Create attributes for dynamics control inputs ######### self.DYNAMICS_ATTR = dynamics_attributes @@ -307,7 +282,7 @@ def step(self, flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX, physicsClientId=self.CLIENT ) - (Image.fromarray(np.reshape(rgb, (h, w, 4)), 'RGBA')).save(self.IMG_PATH+"frame_"+str(self.FRAME_NUM)+".png") + (Image.fromarray(np.reshape(rgb, (h, w, 4)), 'RGBA')).save(os.path.join(self.IMG_PATH, "frame_"+str(self.FRAME_NUM)+".png")) #### Save the depth or segmentation view instead ####### # dep = ((dep-np.min(dep)) * 255 / (np.max(dep)-np.min(dep))).astype('uint8') # (Image.fromarray(np.reshape(dep, (h, w)))).save(self.IMG_PATH+"frame_"+str(self.FRAME_NUM)+".png") @@ -482,7 +457,8 @@ def _housekeeping(self): p.setAdditionalSearchPath(pybullet_data.getDataPath(), physicsClientId=self.CLIENT) #### Load ground plane, drone and obstacles models ######### self.PLANE_ID = p.loadURDF("plane.urdf", physicsClientId=self.CLIENT) - self.DRONE_IDS = np.array([p.loadURDF(os.path.dirname(os.path.abspath(__file__))+"/../assets/"+self.URDF, + + self.DRONE_IDS = np.array([p.loadURDF(pkg_resources.resource_filename('gym_pybullet_drones', 'assets/'+self.URDF), self.INIT_XYZS[i,:], p.getQuaternionFromEuler(self.INIT_RPYS[i,:]), flags = p.URDF_USE_INERTIA_FROM_FILE, @@ -524,12 +500,12 @@ def _startVideoRecording(self): """ if self.RECORD and self.GUI: self.VIDEO_ID = p.startStateLogging(loggingType=p.STATE_LOGGING_VIDEO_MP4, - fileName=os.path.dirname(os.path.abspath(__file__))+"/../../files/videos/video-"+datetime.now().strftime("%m.%d.%Y_%H.%M.%S")+".mp4", + fileName=os.path.join(self.OUTPUT_FOLDER, "recording_" + datetime.now().strftime("%m.%d.%Y_%H.%M.%S"), "output.mp4"), physicsClientId=self.CLIENT ) if self.RECORD and not self.GUI: self.FRAME_NUM = 0 - self.IMG_PATH = os.path.dirname(os.path.abspath(__file__))+"/../../files/videos/video-"+datetime.now().strftime("%m.%d.%Y_%H.%M.%S")+"/" + self.IMG_PATH = os.path.join(self.OUTPUT_FOLDER, "recording_" + datetime.now().strftime("%m.%d.%Y_%H.%M.%S"), '') os.makedirs(os.path.dirname(self.IMG_PATH), exist_ok=True) ################################################################################ @@ -636,7 +612,7 @@ def _exportImage(self, """ if img_type == ImageType.RGB: - (Image.fromarray(img_input.astype('uint8'), 'RGBA')).save(path+"frame_"+str(frame_num)+".png") + (Image.fromarray(img_input.astype('uint8'), 'RGBA')).save(os.path.join(path,"frame_"+str(frame_num)+".png")) elif img_type == ImageType.DEP: temp = ((img_input-np.min(img_input)) * 255 / (np.max(img_input)-np.min(img_input))).astype('uint8') elif img_type == ImageType.SEG: @@ -647,7 +623,7 @@ def _exportImage(self, print("[ERROR] in BaseAviary._exportImage(), unknown ImageType") exit() if img_type != ImageType.RGB: - (Image.fromarray(temp)).save(path+"frame_"+str(frame_num)+".png") + (Image.fromarray(temp)).save(os.path.join(path,"frame_"+str(frame_num)+".png")) ################################################################################ @@ -991,7 +967,7 @@ def _parseURDFParameters(self): files in folder `assets/`. """ - URDF_TREE = etxml.parse(os.path.dirname(os.path.abspath(__file__))+"/../assets/"+self.URDF).getroot() + URDF_TREE = etxml.parse(pkg_resources.resource_filename('gym_pybullet_drones', 'assets/'+self.URDF)).getroot() M = float(URDF_TREE[1][0][1].attrib['value']) L = float(URDF_TREE[0].attrib['arm']) THRUST2WEIGHT_RATIO = float(URDF_TREE[0].attrib['thrust2weight']) diff --git a/gym_pybullet_drones/envs/CtrlAviary.py b/gym_pybullet_drones/envs/CtrlAviary.py index 0581e9982..be5d40972 100644 --- a/gym_pybullet_drones/envs/CtrlAviary.py +++ b/gym_pybullet_drones/envs/CtrlAviary.py @@ -1,7 +1,8 @@ import numpy as np from gym import spaces -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics, BaseAviary +from gym_pybullet_drones.envs.BaseAviary import BaseAviary +from gym_pybullet_drones.utils.enums import DroneModel, Physics class CtrlAviary(BaseAviary): """Multi-drone environment class for control applications.""" @@ -20,7 +21,8 @@ def __init__(self, gui=False, record=False, obstacles=False, - user_debug_gui=True + user_debug_gui=True, + output_folder='results' ): """Initialization of an aviary environment for control applications. @@ -63,7 +65,8 @@ def __init__(self, gui=gui, record=record, obstacles=obstacles, - user_debug_gui=user_debug_gui + user_debug_gui=user_debug_gui, + output_folder=output_folder ) ################################################################################ diff --git a/gym_pybullet_drones/envs/DynAviary.py b/gym_pybullet_drones/envs/DynAviary.py index c0f9552b9..744aa9c8e 100644 --- a/gym_pybullet_drones/envs/DynAviary.py +++ b/gym_pybullet_drones/envs/DynAviary.py @@ -1,7 +1,8 @@ import numpy as np from gym import spaces -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics, BaseAviary +from gym_pybullet_drones.envs.BaseAviary import BaseAviary +from gym_pybullet_drones.utils.enums import DroneModel, Physics from gym_pybullet_drones.utils.utils import nnlsRPM class DynAviary(BaseAviary): @@ -21,7 +22,8 @@ def __init__(self, gui=False, record=False, obstacles=False, - user_debug_gui=True + user_debug_gui=True, + output_folder='results' ): """Initialization of an aviary controlled by desired thrust and torques. @@ -68,7 +70,8 @@ def __init__(self, record=record, obstacles=obstacles, user_debug_gui=user_debug_gui, - dynamics_attributes=True + dynamics_attributes=True, + output_folder=output_folder ) diff --git a/gym_pybullet_drones/envs/VelocityAviary.py b/gym_pybullet_drones/envs/VelocityAviary.py index 36759a13b..cdcb9d949 100644 --- a/gym_pybullet_drones/envs/VelocityAviary.py +++ b/gym_pybullet_drones/envs/VelocityAviary.py @@ -2,7 +2,8 @@ import numpy as np from gym import spaces -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics, BaseAviary +from gym_pybullet_drones.envs.BaseAviary import BaseAviary +from gym_pybullet_drones.utils.enums import DroneModel, Physics from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl from gym_pybullet_drones.control.SimplePIDControl import SimplePIDControl @@ -23,7 +24,8 @@ def __init__(self, gui=False, record=False, obstacles=False, - user_debug_gui=True + user_debug_gui=True, + output_folder='results' ): """Initialization of an aviary environment for or high-level planning. @@ -72,7 +74,8 @@ def __init__(self, gui=gui, record=record, obstacles=obstacles, - user_debug_gui=user_debug_gui + user_debug_gui=user_debug_gui, + output_folder=output_folder ) #### Set a limit on the maximum target speed ############### self.SPEED_LIMIT = 0.03 * self.MAX_SPEED_KMH * (1000/3600) diff --git a/gym_pybullet_drones/envs/VisionAviary.py b/gym_pybullet_drones/envs/VisionAviary.py index f6cf9da45..f072f4e20 100644 --- a/gym_pybullet_drones/envs/VisionAviary.py +++ b/gym_pybullet_drones/envs/VisionAviary.py @@ -2,7 +2,8 @@ import numpy as np from gym import spaces -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics, ImageType, BaseAviary +from gym_pybullet_drones.envs.BaseAviary import BaseAviary +from gym_pybullet_drones.utils.enums import DroneModel, Physics, ImageType class VisionAviary(BaseAviary): """Multi-drone environment class for control applications using vision.""" @@ -21,7 +22,8 @@ def __init__(self, gui=False, record=False, obstacles=False, - user_debug_gui=True + user_debug_gui=True, + output_folder='results' ): """Initialization of an aviary environment for control applications using vision. @@ -68,7 +70,8 @@ def __init__(self, record=record, obstacles=obstacles, user_debug_gui=user_debug_gui, - vision_attributes=True + vision_attributes=True, + output_folder=output_folder ) diff --git a/gym_pybullet_drones/envs/multi_agent_rl/BaseMultiagentAviary.py b/gym_pybullet_drones/envs/multi_agent_rl/BaseMultiagentAviary.py index 44c087774..719da5421 100644 --- a/gym_pybullet_drones/envs/multi_agent_rl/BaseMultiagentAviary.py +++ b/gym_pybullet_drones/envs/multi_agent_rl/BaseMultiagentAviary.py @@ -1,11 +1,11 @@ import os -from datetime import datetime import numpy as np import pybullet as p from gym import spaces -from ray.rllib.env.multi_agent_env import MultiAgentEnv, ENV_STATE +from ray.rllib.env.multi_agent_env import MultiAgentEnv -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics, BaseAviary +from gym_pybullet_drones.envs.BaseAviary import BaseAviary +from gym_pybullet_drones.utils.enums import DroneModel, Physics from gym_pybullet_drones.envs.single_agent_rl.BaseSingleAgentAviary import ActionType, ObservationType from gym_pybullet_drones.utils.utils import nnlsRPM from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl diff --git a/gym_pybullet_drones/envs/multi_agent_rl/FlockAviary.py b/gym_pybullet_drones/envs/multi_agent_rl/FlockAviary.py index f3b63218b..c83b20093 100644 --- a/gym_pybullet_drones/envs/multi_agent_rl/FlockAviary.py +++ b/gym_pybullet_drones/envs/multi_agent_rl/FlockAviary.py @@ -1,11 +1,7 @@ import math import numpy as np -from gym import spaces -from ray.rllib.env.multi_agent_env import MultiAgentEnv, ENV_STATE -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics, BaseAviary - -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics, BaseAviary +from gym_pybullet_drones.utils.enums import DroneModel, Physics from gym_pybullet_drones.envs.single_agent_rl.BaseSingleAgentAviary import ActionType, ObservationType from gym_pybullet_drones.envs.multi_agent_rl.BaseMultiagentAviary import BaseMultiagentAviary diff --git a/gym_pybullet_drones/envs/multi_agent_rl/LeaderFollowerAviary.py b/gym_pybullet_drones/envs/multi_agent_rl/LeaderFollowerAviary.py index 3943b6b46..cd5b73938 100644 --- a/gym_pybullet_drones/envs/multi_agent_rl/LeaderFollowerAviary.py +++ b/gym_pybullet_drones/envs/multi_agent_rl/LeaderFollowerAviary.py @@ -1,11 +1,6 @@ -import math import numpy as np -from gym import spaces -from ray.rllib.env.multi_agent_env import MultiAgentEnv, ENV_STATE -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics, BaseAviary - -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics, BaseAviary +from gym_pybullet_drones.utils.enums import DroneModel, Physics from gym_pybullet_drones.envs.single_agent_rl.BaseSingleAgentAviary import ActionType, ObservationType from gym_pybullet_drones.envs.multi_agent_rl.BaseMultiagentAviary import BaseMultiagentAviary diff --git a/gym_pybullet_drones/envs/multi_agent_rl/MeetupAviary.py b/gym_pybullet_drones/envs/multi_agent_rl/MeetupAviary.py index 01f9f92b7..5535f4756 100644 --- a/gym_pybullet_drones/envs/multi_agent_rl/MeetupAviary.py +++ b/gym_pybullet_drones/envs/multi_agent_rl/MeetupAviary.py @@ -1,11 +1,6 @@ -import math import numpy as np -from gym import spaces -from ray.rllib.env.multi_agent_env import MultiAgentEnv, ENV_STATE -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics, BaseAviary - -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics, BaseAviary +from gym_pybullet_drones.utils.enums import DroneModel, Physics from gym_pybullet_drones.envs.single_agent_rl.BaseSingleAgentAviary import ActionType, ObservationType from gym_pybullet_drones.envs.multi_agent_rl.BaseMultiagentAviary import BaseMultiagentAviary diff --git a/gym_pybullet_drones/envs/single_agent_rl/BaseSingleAgentAviary.py b/gym_pybullet_drones/envs/single_agent_rl/BaseSingleAgentAviary.py index c008e5d7e..3f1b9534d 100644 --- a/gym_pybullet_drones/envs/single_agent_rl/BaseSingleAgentAviary.py +++ b/gym_pybullet_drones/envs/single_agent_rl/BaseSingleAgentAviary.py @@ -1,12 +1,11 @@ import os -from datetime import datetime from enum import Enum import numpy as np from gym import spaces import pybullet as p -import pybullet_data -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics, ImageType, BaseAviary +from gym_pybullet_drones.envs.BaseAviary import BaseAviary +from gym_pybullet_drones.utils.enums import DroneModel, Physics, ImageType from gym_pybullet_drones.utils.utils import nnlsRPM from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl from gym_pybullet_drones.control.SimplePIDControl import SimplePIDControl diff --git a/gym_pybullet_drones/envs/single_agent_rl/FlyThruGateAviary.py b/gym_pybullet_drones/envs/single_agent_rl/FlyThruGateAviary.py index 939ed78d4..ed37f4302 100644 --- a/gym_pybullet_drones/envs/single_agent_rl/FlyThruGateAviary.py +++ b/gym_pybullet_drones/envs/single_agent_rl/FlyThruGateAviary.py @@ -1,9 +1,9 @@ import os import numpy as np -from gym import spaces import pybullet as p +import pkg_resources -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics, BaseAviary +from gym_pybullet_drones.utils.enums import DroneModel, Physics from gym_pybullet_drones.envs.single_agent_rl.BaseSingleAgentAviary import ActionType, ObservationType, BaseSingleAgentAviary @@ -73,7 +73,7 @@ def _addObstacles(self): """ super()._addObstacles() - p.loadURDF(os.path.dirname(os.path.abspath(__file__))+"/../../assets/architrave.urdf", + p.loadURDF(pkg_resources.resource_filename('gym_pybullet_drones', 'assets/architrave.urdf'), [0, -1, .55], p.getQuaternionFromEuler([0, 0, 0]), physicsClientId=self.CLIENT diff --git a/gym_pybullet_drones/envs/single_agent_rl/HoverAviary.py b/gym_pybullet_drones/envs/single_agent_rl/HoverAviary.py index cbd71b26d..ccc2fd0c8 100644 --- a/gym_pybullet_drones/envs/single_agent_rl/HoverAviary.py +++ b/gym_pybullet_drones/envs/single_agent_rl/HoverAviary.py @@ -1,7 +1,6 @@ import numpy as np -from gym import spaces -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics, BaseAviary +from gym_pybullet_drones.utils.enums import DroneModel, Physics from gym_pybullet_drones.envs.single_agent_rl.BaseSingleAgentAviary import ActionType, ObservationType, BaseSingleAgentAviary class HoverAviary(BaseSingleAgentAviary): diff --git a/gym_pybullet_drones/envs/single_agent_rl/TakeoffAviary.py b/gym_pybullet_drones/envs/single_agent_rl/TakeoffAviary.py index 4ed0da2a0..64ea99401 100644 --- a/gym_pybullet_drones/envs/single_agent_rl/TakeoffAviary.py +++ b/gym_pybullet_drones/envs/single_agent_rl/TakeoffAviary.py @@ -1,7 +1,6 @@ import numpy as np -from gym import spaces -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics, BaseAviary +from gym_pybullet_drones.utils.enums import DroneModel, Physics from gym_pybullet_drones.envs.single_agent_rl.BaseSingleAgentAviary import ActionType, ObservationType, BaseSingleAgentAviary class TakeoffAviary(BaseSingleAgentAviary): diff --git a/gym_pybullet_drones/envs/single_agent_rl/TuneAviary.py b/gym_pybullet_drones/envs/single_agent_rl/TuneAviary.py index 30173f781..5a45a7983 100644 --- a/gym_pybullet_drones/envs/single_agent_rl/TuneAviary.py +++ b/gym_pybullet_drones/envs/single_agent_rl/TuneAviary.py @@ -1,7 +1,6 @@ import numpy as np -from gym import spaces -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics, BaseAviary +from gym_pybullet_drones.utils.enums import DroneModel, Physics from gym_pybullet_drones.envs.single_agent_rl.BaseSingleAgentAviary import ActionType, ObservationType, BaseSingleAgentAviary class TuneAviary(BaseSingleAgentAviary): diff --git a/gym_pybullet_drones/examples/__init__.py b/gym_pybullet_drones/examples/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/examples/compare.py b/gym_pybullet_drones/examples/compare.py similarity index 67% rename from examples/compare.py rename to gym_pybullet_drones/examples/compare.py index d335697bb..96c0e5be9 100644 --- a/examples/compare.py +++ b/gym_pybullet_drones/examples/compare.py @@ -13,30 +13,36 @@ The comparison is along a 2D trajectory in the X-Z plane, between x == +1 and -1. """ -import os import time import argparse import pickle import numpy as np +import pkg_resources from gym_pybullet_drones.utils.utils import sync, str2bool -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics +from gym_pybullet_drones.utils.enums import DroneModel, Physics from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary 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 = pkg_resources.resource_filename('gym_pybullet_drones', 'assets/example_trace.pkl') +DEFAULT_OUTPUT_FOLDER = 'results' +DEFAULT_COLAB = False + +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, + colab=DEFAULT_COLAB + ): #### 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 +53,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 +69,9 @@ #### 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, + colab=colab ) #### Initialize the controller ############################# @@ -105,7 +113,7 @@ env.render() #### Sync the simulation ################################### - if ARGS.gui: + if gui: sync(i, START, env.TIMESTEP) #### Close the environment ################################# @@ -115,4 +123,18 @@ 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='') + parser.add_argument('--colab', default=DEFAULT_COLAB, type=bool, help='Whether example is being run by a notebook (default: "False")', 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 54% rename from examples/downwash.py rename to gym_pybullet_drones/examples/downwash.py index 012ef0b0c..f9354d051 100644 --- a/examples/downwash.py +++ b/gym_pybullet_drones/examples/downwash.py @@ -16,61 +16,72 @@ import numpy as np from gym_pybullet_drones.utils.utils import sync, str2bool -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics +from gym_pybullet_drones.utils.enums import DroneModel, Physics from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary 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' +DEFAULT_COLAB = False + +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, + colab=DEFAULT_COLAB + ): #### 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, + colab=colab ) #### 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 +113,7 @@ env.render() #### Sync the simulation ################################### - if ARGS.gui: + if gui: sync(i, START, env.TIMESTEP) #### Close the environment ################################# @@ -113,4 +124,22 @@ 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='') + parser.add_argument('--colab', default=DEFAULT_COLAB, type=bool, help='Whether example is being run by a notebook (default: "False")', 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 53% rename from examples/fly.py rename to gym_pybullet_drones/examples/fly.py index c415e6899..643f5050e 100644 --- a/examples/fly.py +++ b/gym_pybullet_drones/examples/fly.py @@ -26,7 +26,7 @@ import pybullet as p import matplotlib.pyplot as plt -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics +from gym_pybullet_drones.utils.enums import DroneModel, Physics from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary from gym_pybullet_drones.envs.VisionAviary import VisionAviary from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl @@ -34,46 +34,60 @@ 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' +DEFAULT_COLAB = False + +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, + colab=DEFAULT_COLAB + ): #### 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 +102,58 @@ # 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, + colab=colab ) #### 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 +165,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 +174,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 +190,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 +209,27 @@ 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='') + parser.add_argument('--colab', default=DEFAULT_COLAB, type=bool, help='Whether example is being run by a notebook (default: "False")', 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 55% rename from examples/groundeffect.py rename to gym_pybullet_drones/examples/groundeffect.py index 57143dcd5..52fe3f2f3 100644 --- a/examples/groundeffect.py +++ b/gym_pybullet_drones/examples/groundeffect.py @@ -24,7 +24,7 @@ import pybullet as p import matplotlib.pyplot as plt -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics +from gym_pybullet_drones.utils.enums import DroneModel, Physics from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary from gym_pybullet_drones.envs.VisionAviary import VisionAviary from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl @@ -32,28 +32,39 @@ 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' +DEFAULT_COLAB = False + +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, + colab=DEFAULT_COLAB + ): #### 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 +77,32 @@ 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, + colab=colab ) #### 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 +134,7 @@ env.render() #### Sync the simulation ################################### - if ARGS.gui: + if gui: sync(i, START, env.TIMESTEP) #### Close the environment ################################# @@ -132,5 +145,23 @@ 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='') + parser.add_argument('--colab', default=DEFAULT_COLAB, type=bool, help='Whether example is being run by a notebook (default: "False")', metavar='') + ARGS = parser.parse_args() + + run(**vars(ARGS)) diff --git a/examples/learn.py b/gym_pybullet_drones/examples/learn.py similarity index 72% rename from examples/learn.py rename to gym_pybullet_drones/examples/learn.py index e00c1fae3..4fb12f767 100644 --- a/examples/learn.py +++ b/gym_pybullet_drones/examples/learn.py @@ -31,12 +31,13 @@ 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_RECORD_VIDEO = False +DEFAULT_OUTPUT_FOLDER = 'results' +DEFAULT_COLAB = False - #### 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, colab=DEFAULT_COLAB, record_video=DEFAULT_RECORD_VIDEO): #### Check the environment's spaces ######################## env = gym.make("takeoff-aviary-v0") @@ -48,7 +49,7 @@ ) #### Train the model ####################################### - if not ARGS.rllib: + if not rllib: model = A2C(MlpPolicy, env, verbose=1 @@ -75,16 +76,18 @@ ray.shutdown() #### Show (and record a video of) the model's performance ## - env = TakeoffAviary(gui=True, - record=False + env = TakeoffAviary(gui=gui, + record=record_video ) logger = Logger(logging_freq_hz=int(env.SIM_FREQ/env.AGGR_PHY_STEPS), - num_drones=1 + num_drones=1, + output_folder=output_folder, + colab=colab ) 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 +106,18 @@ 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('--record_video', default=DEFAULT_RECORD_VIDEO, type=str2bool, help='Whether to record a video (default: False)', metavar='') + parser.add_argument('--output_folder', default=DEFAULT_OUTPUT_FOLDER, type=str, help='Folder where to save logs (default: "results")', metavar='') + parser.add_argument('--colab', default=DEFAULT_COLAB, type=bool, help='Whether example is being run by a notebook (default: "False")', metavar='') + ARGS = parser.parse_args() + + run(**vars(ARGS)) diff --git a/examples/velocity.py b/gym_pybullet_drones/examples/velocity.py similarity index 54% rename from examples/velocity.py rename to gym_pybullet_drones/examples/velocity.py index 40dae67e8..107ba137e 100644 --- a/examples/velocity.py +++ b/gym_pybullet_drones/examples/velocity.py @@ -24,7 +24,7 @@ import pybullet as p import matplotlib.pyplot as plt -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics +from gym_pybullet_drones.utils.enums import DroneModel, Physics from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary from gym_pybullet_drones.envs.VisionAviary import VisionAviary from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl @@ -34,23 +34,34 @@ 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' +DEFAULT_COLAB = False + +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, + colab=DEFAULT_COLAB + ): + #### Initialize the simulation ############################# INIT_XYZS = np.array([ [ 0, 0, .1], [.3, 0, .1], @@ -63,22 +74,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 +97,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 +110,17 @@ 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, + colab=colab ) #### 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 +152,7 @@ env.render() #### Sync the simulation ################################### - if ARGS.gui: + if gui: sync(i, START, env.TIMESTEP) #### Close the environment ################################# @@ -147,5 +160,25 @@ #### 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='') + parser.add_argument('--colab', default=DEFAULT_COLAB, type=bool, help='Whether example is being run by a notebook (default: "False")', 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 baeddfcc8..f1e6d0bb0 100644 --- a/gym_pybullet_drones/utils/Logger.py +++ b/gym_pybullet_drones/utils/Logger.py @@ -1,5 +1,4 @@ import os -import time from datetime import datetime from cycler import cycler import numpy as np @@ -19,8 +18,10 @@ class Logger(object): def __init__(self, logging_freq_hz: int, + output_folder: str="results", num_drones: int=1, - duration_sec: int=0 + duration_sec: int=0, + colab: bool=False, ): """Logger class __init__ method. @@ -37,6 +38,10 @@ def __init__(self, Used to preallocate the log arrays (improves performance). """ + self.COLAB = colab + 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 @@ -118,7 +123,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) ################################################################################ @@ -134,7 +139,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) @@ -368,4 +373,7 @@ def plot(self, pwm=False): wspace=0.15, hspace=0.0 ) - plt.show() + if self.COLAB: + plt.savefig(os.path.join('results', 'output_figure.png')) + else: + plt.show() diff --git a/gym_pybullet_drones/utils/enums.py b/gym_pybullet_drones/utils/enums.py new file mode 100644 index 000000000..4634835be --- /dev/null +++ b/gym_pybullet_drones/utils/enums.py @@ -0,0 +1,32 @@ +from enum import Enum + +class DroneModel(Enum): + """Drone models enumeration class.""" + + CF2X = "cf2x" # Bitcraze Craziflie 2.0 in the X configuration + CF2P = "cf2p" # Bitcraze Craziflie 2.0 in the + configuration + HB = "hb" # Generic quadrotor (with AscTec Hummingbird inertial properties) + +################################################################################ + +class Physics(Enum): + """Physics implementations enumeration class.""" + + PYB = "pyb" # Base PyBullet physics update + DYN = "dyn" # Update with an explicit model of the dynamics + PYB_GND = "pyb_gnd" # PyBullet physics update with ground effect + PYB_DRAG = "pyb_drag" # PyBullet physics update with drag + PYB_DW = "pyb_dw" # PyBullet physics update with downwash + PYB_GND_DRAG_DW = "pyb_gnd_drag_dw" # PyBullet physics update with ground effect, drag, and downwash + +################################################################################ + +class ImageType(Enum): + """Camera capture image type enumeration class.""" + + RGB = 0 # Red, green, blue (and alpha) + DEP = 1 # Depth + SEG = 2 # Segmentation by object id + BW = 3 # Black and white + +################################################################################ \ No newline at end of file diff --git a/pypi_description.md b/pypi_description.md new file mode 100644 index 000000000..31acb7dcc --- /dev/null +++ b/pypi_description.md @@ -0,0 +1,8 @@ +# gym-pybullet-drones + +[Simple](https://en.wikipedia.org/wiki/KISS_principle) OpenAI [Gym environment](https://gym.openai.com/envs/#classic_control) based on [PyBullet](https://github.com/bulletphysics/bullet3) for multi-agent reinforcement learning with quadrotors + +Find the project's published results at [Learning to Fly—a Gym Environment with PyBullet Physics for Reinforcement Learning of Multi-agent Quadcopter Control](https://ieeexplore.ieee.org/abstract/document/9635857). + +----- +> University of Toronto's [Dynamic Systems Lab](https://github.com/utiasDSL) / [Vector Institute](https://github.com/VectorInstitute) / University of Cambridge's [Prorok Lab](https://github.com/proroklab) / [Mitacs](https://www.mitacs.ca/en/projects/multi-agent-reinforcement-learning-decentralized-uavugv-cooperative-exploration) diff --git a/pyproject.toml b/pyproject.toml index 69495a745..f7f700642 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,9 +1,14 @@ [tool.poetry] name = "gym-pybullet-drones" -version = "1.0.0" +version = "0.0.3" description = "PyBullet Gym environments for single and multi-agent reinforcement learning of quadcopter control" authors = ["Jacopo Panerati "] license = "MIT" +readme = "pypi_description.md" +repository = "https://github.com/utiasDSL/gym-pybullet-drones" +packages = [ + { include = "gym_pybullet_drones" } +] [tool.poetry.dependencies] python = "^3.8" @@ -13,9 +18,10 @@ matplotlib = "^3.5" cycler = "^0.10" gym = "^0.21" pybullet = "^3.2" -ray = "1.9" +"ray[rllib]" = "1.9" stable-baselines3 = "^1.5" scipy = "^1.8" +tensorboard = "^2.9" [tool.poetry.dev-dependencies] diff --git a/ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/aviary_wrapper.py b/ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/aviary_wrapper.py index c01a8e7e4..a2aa6eb26 100644 --- a/ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/aviary_wrapper.py +++ b/ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/aviary_wrapper.py @@ -16,7 +16,7 @@ from rclpy.node import Node from std_msgs.msg import Float32MultiArray -from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics +from gym_pybullet_drones.utils.enums import DroneModel, Physics from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary class AviaryWrapper(Node): diff --git a/setup.py b/setup.py deleted file mode 100644 index aef2988ab..000000000 --- a/setup.py +++ /dev/null @@ -1,15 +0,0 @@ -from setuptools import setup - -setup(name='gym_pybullet_drones', - version='1.0.0', - install_requires=[ - 'numpy', - 'Pillow', - 'matplotlib', - 'cycler', - 'gym', - 'pybullet', - 'stable_baselines3', - 'ray[rllib]' - ] -) diff --git a/tests/test_build.py b/tests/test_build.py new file mode 100644 index 000000000..c304abac8 --- /dev/null +++ b/tests/test_build.py @@ -0,0 +1,6 @@ +def test_imports(): + import gym_pybullet_drones + import gym_pybullet_drones.control + import gym_pybullet_drones.envs + import gym_pybullet_drones.examples + import gym_pybullet_drones.utils diff --git a/tests/test_examples.py b/tests/test_examples.py new file mode 100644 index 000000000..8ded78358 --- /dev/null +++ b/tests/test_examples.py @@ -0,0 +1,23 @@ +def test_compare(): + from gym_pybullet_drones.examples.compare import run + run(gui=False,plot=False,output_folder='tmp') + +def test_downwash(): + from gym_pybullet_drones.examples.downwash import run + run(gui=False,plot=False,output_folder='tmp') + +def test_fly(): + from gym_pybullet_drones.examples.fly import run + run(gui=False,plot=False,output_folder='tmp') + +def test_groundeffect(): + from gym_pybullet_drones.examples.groundeffect import run + run(gui=False,plot=False,output_folder='tmp') + +def test_learn(): + from gym_pybullet_drones.examples.learn import run + run(gui=False,plot=False,output_folder='tmp') + +def test_velocity(): + from gym_pybullet_drones.examples.velocity import run + run(gui=False,plot=False,output_folder='tmp') diff --git a/tests/test_experiments.py b/tests/test_experiments.py new file mode 100644 index 000000000..18bf9a78d --- /dev/null +++ b/tests/test_experiments.py @@ -0,0 +1,28 @@ +import sys +import glob + +def test_single_agent(): + initial_models = set(glob.glob('**/best_model.zip', recursive=True)) + sys.path.append('experiments/learning/') + from experiments.learning.singleagent import run as run_train + run_train(steps=30, output_folder='tmp') + + from experiments.learning.test_singleagent import run as run_test + new_models = set(glob.glob('**/best_model.zip', recursive=True)) + test_model = new_models - initial_models + assert len(test_model) > 0, initial_models + path = '/'.join(next(iter(test_model)).split('/')[:-1]) + run_test(path, plot=False, gui=False, output_folder='tmp') + +# def test_multi_agent(self): +# initial_models = set(glob.glob('**/best_model.zip', recursive=True)) +# sys.path.append('experiments/learning') +# from experiments.learning.multiagent import run as run_train +# run_train(steps=30, output_folder='tmp') + +# from experiments.learning.test_multiagent import run as run_test +# new_models = set(glob.glob('**/best_model.zip', recursive=True)) +# test_model = new_models - initial_models +# assert len(test_model) > 0, initial_models +# path = '/'.join(next(iter(test_model)).split('/')[:-1]) +# run_test(path, plot=False, gui=False, output_folder='tmp')