├── README.md ├── gym-pybullet-drones └── gym-pybullet-drones │ ├── .vscode │ └── launch.json │ ├── LICENSE │ ├── README.md │ ├── experiments │ └── learning │ │ ├── __pycache__ │ │ ├── shared_constants.cpython-310.pyc │ │ └── shared_constants.cpython-37.pyc │ │ ├── shared_constants.py │ │ ├── singleagent.py │ │ └── test_singleagent.py │ └── gym_pybullet_drones │ ├── __init__.py │ ├── __pycache__ │ ├── __init__.cpython-310.pyc │ └── __init__.cpython-37.pyc │ ├── assets │ ├── architrave.urdf │ ├── cf2.dae │ ├── cf2p.urdf │ ├── cf2x.urdf │ ├── example_trace.pkl │ ├── hb.urdf │ └── quad.obj │ ├── control │ ├── BaseControl.py │ ├── DSLPIDControl.py │ ├── SimplePIDControl.py │ ├── __init__.py │ └── __pycache__ │ │ ├── BaseControl.cpython-310.pyc │ │ ├── BaseControl.cpython-37.pyc │ │ ├── DSLPIDControl.cpython-310.pyc │ │ ├── DSLPIDControl.cpython-37.pyc │ │ ├── SimplePIDControl.cpython-310.pyc │ │ ├── SimplePIDControl.cpython-37.pyc │ │ ├── __init__.cpython-310.pyc │ │ └── __init__.cpython-37.pyc │ ├── envs │ ├── BaseAviary.py │ ├── ControlAviary.py │ ├── __init__.py │ ├── __pycache__ │ │ ├── BaseAviary.cpython-310.pyc │ │ ├── BaseAviary.cpython-37.pyc │ │ ├── ControlAviary.cpython-310.pyc │ │ ├── ControlAviary.cpython-37.pyc │ │ ├── ControlAviary_1.cpython-37.pyc │ │ ├── ControlAviary_1_1.cpython-37.pyc │ │ ├── CtrlAviary.cpython-310.pyc │ │ ├── CtrlAviary.cpython-37.pyc │ │ ├── DynAviary.cpython-310.pyc │ │ ├── DynAviary.cpython-37.pyc │ │ ├── VelocityAviary.cpython-310.pyc │ │ ├── VelocityAviary.cpython-37.pyc │ │ ├── VisionAviary.cpython-310.pyc │ │ ├── VisionAviary.cpython-37.pyc │ │ ├── __init__.cpython-310.pyc │ │ └── __init__.cpython-37.pyc │ └── single_agent_rl │ │ ├── BaseSingleAgentAviary.py │ │ ├── __init__.py │ │ └── __pycache__ │ │ ├── BaseSingleAgentAviary.cpython-310.pyc │ │ ├── BaseSingleAgentAviary.cpython-37.pyc │ │ ├── FlyThruGateAviary.cpython-310.pyc │ │ ├── FlyThruGateAviary.cpython-37.pyc │ │ ├── HoverAviary.cpython-310.pyc │ │ ├── HoverAviary.cpython-37.pyc │ │ ├── TakeoffAviary.cpython-310.pyc │ │ ├── TakeoffAviary.cpython-37.pyc │ │ ├── TuneAviary.cpython-310.pyc │ │ ├── TuneAviary.cpython-37.pyc │ │ ├── __init__.cpython-310.pyc │ │ └── __init__.cpython-37.pyc │ ├── examples │ ├── __init__.py │ └── fly_test.py │ └── utils │ ├── Logger.py │ ├── __init__.py │ ├── __pycache__ │ ├── Logger.cpython-310.pyc │ ├── Logger.cpython-37.pyc │ ├── __init__.cpython-310.pyc │ ├── __init__.cpython-37.pyc │ ├── adsb.cpython-37.pyc │ ├── adsbcenter.cpython-310.pyc │ ├── adsbcenter.cpython-37.pyc │ ├── enums.cpython-310.pyc │ ├── enums.cpython-37.pyc │ ├── utils.cpython-310.pyc │ └── utils.cpython-37.pyc │ ├── adsbcenter.py │ ├── enums.py │ └── utils.py └── results ├── save-ControlAviary-ddpg-kin-tun-48HZ-1M ├── success_model.zip └── tb │ └── DDPG_1 │ └── events.out.tfevents.1677372157.hecs-340817.8560.0 ├── save-ControlAviary-ppo-kin-tun-12HZ-1M ├── success_model.zip └── tb │ └── PPO_1 │ └── events.out.tfevents.1679256062.hecs-340817.5200.0 ├── save-ControlAviary-ppo-kin-tun-2HZ-1M ├── success_model.zip └── tb │ └── PPO_1 │ └── events.out.tfevents.1679076909.hecs-340817.7528.0 ├── save-ControlAviary-ppo-kin-tun-2Hz-2M ├── success_model.zip └── tb │ └── PPO_1 │ └── events.out.tfevents.1679153911.hecs-340817.3700.0 ├── save-ControlAviary-ppo-kin-tun-48HZ-1M-Ob ├── success_model.zip └── tb │ └── PPO_1 │ └── events.out.tfevents.1677942124.hecs-340817.8428.0 ├── save-ControlAviary-ppo-kin-tun-48HZ-1M ├── success_model.zip └── tb │ └── PPO_1 │ └── events.out.tfevents.1675884099.hecs-340817.6760.0 └── save-ControlAviary-ppo-kin-tun-8HZ-1M ├── success_model.zip └── tb └── PPO_1 └── events.out.tfevents.1679318052.hecs-340817.4352.0 /README.md: -------------------------------------------------------------------------------- 1 | # DRL4DAPTA 2 | UAV PATH TRACKING AND DYNAMIC AVOIDANCE BASED ON ADS-B AND DEEP REINFORCEMENT LEARNING for Univerisity of Bristol RP3 final 3 | 4 | # Backgorund 5 | The code is the final submission code for University of Bristol aerospace engineering--Research Project 3. And constructed based on https://github.com/utiasDSL/gym-pybullet-drones 6 | Contact cf20770@bristol.ac.uk if you have any question 7 | 8 | 9 | # Abstraction 10 | This paper presents a novel dynamic avoidance and path tracking algorithm for unmanned aerial vehicles (UAVs) called DRL4DAPTA. The algorithm utilizes deep reinforcement learning and is implemented using ADS-B as a signal transmission intermediary and PyBullet as a simulation and training platform. The algorithm is designed to adapt to real-life situations by considering the error of the ADS-B signal. The reward function is constructed by predicting collision time, and the path-following algorithm is based on interacting with a set path and designing the corresponding reward function. To ensure safety despite the maximum error, the ADS-B signal error is processed by designing a sufficient detection zone. Two comparison algorithms, Proximal Policy Optimization (PPO) and deep Deterministic Policy Gradient (DDPG), are evaluated using the same reward function. Simulation experiments demonstrate that DRL4DAPTA achieves stable path tracking and dynamic obstacle avoidance in the presence of errors and low ADS-B signal frequency in a relative stable manner, verifying the feasibility of the algorithm. 11 | 12 | 13 | # Contents 14 | The main contents includes four objects: ‘ControlAviary’, ‘adsbcentre’, ‘Singleagent’, and ‘test_singleagent’. Among them, ‘ControlAviary’ is derived from ‘gym.Env’ and is used to control the actions of the agent drone, which implementing the interface functions of OpenAI gym, such as step, reward, reset, etc. ‘ControlAviary’ is responsible for calculating the states s_t and actions a_t of each step of the agent drone and updating the information to the neural network using the 'step' function. ‘Adsbcentre’ is used to store the information of the agent drone at each step and add ADS-B errors to the information. Meanwhile, the transmission frequency of ‘ControlAviary’ to adsbcentre is set to simulate the ADS-B frequency. ‘Singleagent’ is responsible for training, while ‘test_singleagent’ is responsible for conducting 10 experiments with a successful model to obtain the results. 15 | 16 | In addition, it contains the successful results below: 17 | -PPO algorithm, 48Hz ADS-B signal, 1M trainning times. 18 | -DDPG algorithm, 48Hz ADS-B signal, 1M trainning times. 19 | -PPO algorithm, 12Hz ADS-B signal, 1M trainning times. 20 | -PPO algorithm, 8Hz ADS-B signal, 1M trainning times. 21 | -PPO algorithm, 2Hz ADS-B signal, 1M trainning times. 22 | -PPO algorithm, 2Hz ADS-B signal, 2M trainning times. 23 | -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | // 使用 IntelliSense 了解相关属性。 3 | // 悬停以查看现有属性的描述。 4 | // 欲了解更多信息，请访问: https://go.microsoft.com/fwlink/?linkid=830387 5 | "version": "0.2.0", 6 | "configurations": [ 7 | { 8 | "name": "Python: Current File", 9 | "type": "python", 10 | "request": "launch", 11 | "program": "${file}", 12 | "console": "integratedTerminal", 13 | "justMyCode": false 14 | } 15 | ] 16 | } -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Jacopo Panerati 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/README.md: -------------------------------------------------------------------------------- 1 | # Backgorund 2 | The code is the final submission code for University of Bristol aerospace engineering--Research Project 3. And constructed based on https://github.com/utiasDSL/gym-pybullet-drones 3 | Contact cf20770@bristol.ac.uk if you have any question 4 | 5 | 6 | # Abstraction 7 | This paper presents a novel dynamic avoidance and path tracking algorithm for unmanned aerial vehicles (UAVs) called DRL4DAPTA. The algorithm utilizes deep reinforcement learning and is implemented using ADS-B as a signal transmission intermediary and PyBullet as a simulation and training platform. The algorithm is designed to adapt to real-life situations by considering the error of the ADS-B signal. The reward function is constructed by predicting collision time, and the path-following algorithm is based on interacting with a set path and designing the corresponding reward function. To ensure safety despite the maximum error, the ADS-B signal error is processed by designing a sufficient detection zone. Two comparison algorithms, Proximal Policy Optimization (PPO) and deep Deterministic Policy Gradient (DDPG), are evaluated using the same reward function. Simulation experiments demonstrate that DRL4DAPTA achieves stable path tracking and dynamic obstacle avoidance in the presence of errors and low ADS-B signal frequency in a relative stable manner, verifying the feasibility of the algorithm. 8 | 9 | 10 | # Contents 11 | The main contents includes four objects: ‘ControlAviary’, ‘adsbcentre’, ‘Singleagent’, and ‘test_singleagent’. Among them, ‘ControlAviary’ is derived from ‘gym.Env’ and is used to control the actions of the agent drone, which implementing the interface functions of OpenAI gym, such as step, reward, reset, etc. ‘ControlAviary’ is responsible for calculating the states s_t and actions a_t of each step of the agent drone and updating the information to the neural network using the 'step' function. ‘Adsbcentre’ is used to store the information of the agent drone at each step and add ADS-B errors to the information. Meanwhile, the transmission frequency of ‘ControlAviary’ to adsbcentre is set to simulate the ADS-B frequency. ‘Singleagent’ is responsible for training, while ‘test_singleagent’ is responsible for conducting 10 experiments with a successful model to obtain the results. 12 | 13 | In addition, it contains the successful results below: 14 | -PPO algorithm, 48Hz ADS-B signal, 1M trainning times. 15 | -DDPG algorithm, 48Hz ADS-B signal, 1M trainning times. 16 | -PPO algorithm, 12Hz ADS-B signal, 1M trainning times. 17 | -PPO algorithm, 8Hz ADS-B signal, 1M trainning times. 18 | -PPO algorithm, 2Hz ADS-B signal, 1M trainning times. 19 | -PPO algorithm, 2Hz ADS-B signal, 2M trainning times. 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/experiments/learning/__pycache__/shared_constants.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/experiments/learning/__pycache__/shared_constants.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/experiments/learning/__pycache__/shared_constants.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/experiments/learning/__pycache__/shared_constants.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/experiments/learning/shared_constants.py: -------------------------------------------------------------------------------- 1 | AGGR_PHY_STEPS = 5 2 | """int: Aggregate PyBullet/physics steps within a single call to env.step().""" 3 | -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/experiments/learning/singleagent.py: -------------------------------------------------------------------------------- 1 | """Learning script for single agent problems. 2 | 3 | Agents are based on `stable_baselines3`'s implementation of A2C, PPO SAC, TD3, DDPG. 4 | 5 | Example 6 | ------- 7 | To run the script, type in a terminal: 8 | 9 | $ python singleagent.py --env --algo --obs --act --cpu 10 | 11 | Notes 12 | ----- 13 | Use: 14 | 15 | $ tensorboard --logdir ./results/save-----/tb/ 16 | 17 | To check the tensorboard results at: 18 | 19 | http://localhost:6006/ 20 | 21 | """ 22 | import os 23 | import time 24 | from datetime import datetime 25 | from sys import platform 26 | import argparse 27 | import subprocess 28 | import numpy as np 29 | import gym 30 | import torch 31 | import sys 32 | from stable_baselines3.common.env_checker import check_env 33 | from stable_baselines3.common.env_util import make_vec_env # Module cmd_util will be renamed to env_util https://github.com/DLR-RM/stable-baselines3/pull/197 34 | from stable_baselines3.common.vec_env import SubprocVecEnv, VecTransposeImage 35 | from stable_baselines3.common.utils import set_random_seed 36 | from stable_baselines3 import A2C 37 | from stable_baselines3 import PPO 38 | from stable_baselines3 import SAC 39 | from stable_baselines3 import TD3 40 | from stable_baselines3 import DDPG 41 | from stable_baselines3.common.policies import ActorCriticPolicy as a2cppoMlpPolicy 42 | from stable_baselines3.common.policies import ActorCriticCnnPolicy as a2cppoCnnPolicy 43 | from stable_baselines3.sac.policies import SACPolicy as sacMlpPolicy 44 | from stable_baselines3.sac import CnnPolicy as sacCnnPolicy 45 | from stable_baselines3.td3 import MlpPolicy as td3ddpgMlpPolicy 46 | from stable_baselines3.td3 import CnnPolicy as td3ddpgCnnPolicy 47 | from stable_baselines3.common.callbacks import CheckpointCallback, EvalCallback, StopTrainingOnRewardThreshold 48 | sys.path.append("C:/software/gym-pybullet-drones") 49 | 50 | from gym_pybullet_drones.envs.ControlAviary import ControlAviary 51 | from gym_pybullet_drones.envs.single_agent_rl.BaseSingleAgentAviary import ActionType, ObservationType 52 | 53 | import shared_constants 54 | 55 | EPISODE_REWARD_THRESHOLD = 10000 # Upperbound: rewards are always negative, but non-zero 56 | """float: Reward threshold to halt the script.""" 57 | 58 | DEFAULT_ENV = 'ControlAviary' 59 | DEFAULT_ALGO = 'ppo' 60 | DEFAULT_OBS = ObservationType('kin') 61 | DEFAULT_ACT = ActionType('tun') 62 | DEFAULT_CPU = 1 63 | DEFAULT_STEPS = 1000000 64 | DEFAULT_OUTPUT_FOLDER = 'results' 65 | 66 | def run( 67 | env=DEFAULT_ENV, 68 | algo=DEFAULT_ALGO, 69 | obs=DEFAULT_OBS, 70 | act=DEFAULT_ACT, 71 | cpu=DEFAULT_CPU, 72 | steps=DEFAULT_STEPS, 73 | output_folder=DEFAULT_OUTPUT_FOLDER 74 | ): 75 | 76 | #### Save directory ######################################## 77 | filename = os.path.join(output_folder, 'save-'+env+'-'+algo+'-'+obs.value+'-'+act.value+'-'+datetime.now().strftime("%m.%d.%Y_%H.%M.%S")) 78 | if not os.path.exists(filename): 79 | os.makedirs(filename+'/') 80 | 81 | #### Print out current git commit hash ##################### 82 | if (platform == "linux" or platform == "darwin") and ('GITHUB_ACTIONS' not in os.environ.keys()): 83 | git_commit = subprocess.check_output(["git", "describe", "--tags"]).strip() 84 | with open(filename+'/git_commit.txt', 'w+') as f: 85 | f.write(str(git_commit)) 86 | 87 | #### Warning ############################################### 88 | if env == 'tune' and act != ActionType.TUN: 89 | print("\n\n\n[WARNING] TuneAviary is intended for use with ActionType.TUN\n\n\n") 90 | if act == ActionType.ONE_D_RPM or act == ActionType.ONE_D_DYN or act == ActionType.ONE_D_PID: 91 | print("\n\n\n[WARNING] Simplified 1D problem for debugging purposes\n\n\n") 92 | #### Errors ################################################ 93 | if not env in ['takeoff', 'hover']: 94 | print("[ERROR] 1D action space is only compatible with Takeoff and HoverAviary") 95 | exit() 96 | # if act == ActionType.TUN and env != 'tune' : 97 | # print("[ERROR] ActionType.TUN is only compatible with TuneAviary") 98 | # exit() 99 | # if algo in ['sac', 'td3', 'ddpg'] and cpu!=1: 100 | # print("[ERROR] The selected algorithm does not support multiple environments") 101 | # exit() 102 | 103 | #### Uncomment to debug slurm scripts ###################### 104 | # exit() 105 | #env_name="Control-aviary-v0" 106 | env_name = env+"-aviary-v0" 107 | sa_env_kwargs = dict(aggregate_phy_steps=shared_constants.AGGR_PHY_STEPS, obs=obs,act=act) 108 | # 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 109 | if env_name == "ControlAviary-aviary-v0": 110 | 111 | train_env = make_vec_env(ControlAviary, 112 | env_kwargs=sa_env_kwargs, 113 | n_envs=cpu, 114 | seed=0 115 | ) 116 | 117 | print("[INFO] Action space:", train_env.action_space) 118 | print("[INFO] Observation space:", train_env.observation_space) 119 | # check_env(train_env, warn=True, skip_render_check=True) 120 | 121 | #### On-policy algorithms ################################## 122 | onpolicy_kwargs = dict(activation_fn=torch.nn.ReLU, 123 | net_arch=[512, 512, dict(vf=[256, 128], pi=[256, 128])] 124 | ) # or None 125 | if algo == 'a2c': 126 | model = A2C(a2cppoMlpPolicy, 127 | train_env, 128 | policy_kwargs=onpolicy_kwargs, 129 | tensorboard_log=filename+'/tb/', 130 | verbose=1 131 | ) if obs == ObservationType.KIN else A2C(a2cppoCnnPolicy, 132 | train_env, 133 | policy_kwargs=onpolicy_kwargs, 134 | tensorboard_log=filename+'/tb/', 135 | verbose=1 136 | ) 137 | if algo == 'ppo': 138 | model = PPO(a2cppoMlpPolicy, 139 | train_env, 140 | policy_kwargs=onpolicy_kwargs, 141 | tensorboard_log=filename+'/tb/', 142 | verbose=1, 143 | device="cpu" 144 | ) if obs == ObservationType.KIN else PPO(a2cppoCnnPolicy, 145 | train_env, 146 | policy_kwargs=onpolicy_kwargs, 147 | tensorboard_log=filename+'/tb/', 148 | verbose=1 149 | ) 150 | 151 | #### Off-policy algorithms ################################# 152 | offpolicy_kwargs = dict(activation_fn=torch.nn.ReLU, 153 | net_arch=[512, 512, 256, 128] 154 | ) # or None # or dict(net_arch=dict(qf=[256, 128, 64, 32], pi=[256, 128, 64, 32])) 155 | if algo == 'sac': 156 | model = SAC(sacMlpPolicy, 157 | train_env, 158 | policy_kwargs=offpolicy_kwargs, 159 | tensorboard_log=filename+'/tb/', 160 | verbose=1 161 | ) if obs==ObservationType.KIN else SAC(sacCnnPolicy, 162 | train_env, 163 | policy_kwargs=offpolicy_kwargs, 164 | tensorboard_log=filename+'/tb/', 165 | verbose=1 166 | ) 167 | if algo == 'td3': 168 | model = TD3(td3ddpgMlpPolicy, 169 | train_env, 170 | policy_kwargs=offpolicy_kwargs, 171 | tensorboard_log=filename+'/tb/', 172 | verbose=1 173 | ) if obs==ObservationType.KIN else TD3(td3ddpgCnnPolicy, 174 | train_env, 175 | policy_kwargs=offpolicy_kwargs, 176 | tensorboard_log=filename+'/tb/', 177 | verbose=1 178 | ) 179 | if algo == 'ddpg': 180 | model = DDPG(td3ddpgMlpPolicy, 181 | train_env, 182 | learning_rate=0.00005, 183 | policy_kwargs=offpolicy_kwargs, 184 | tensorboard_log=filename+'/tb/', 185 | verbose=1 186 | ) if obs==ObservationType.KIN else DDPG(td3ddpgCnnPolicy, 187 | train_env, 188 | policy_kwargs=offpolicy_kwargs, 189 | tensorboard_log=filename+'/tb/', 190 | verbose=1 191 | ) 192 | 193 | #### Create eveluation environment ######################### 194 | if obs == ObservationType.KIN: 195 | eval_env = gym.make(env_name, 196 | aggregate_phy_steps=shared_constants.AGGR_PHY_STEPS, 197 | obs=obs, 198 | act=act 199 | ) 200 | elif obs == ObservationType.RGB: 201 | 202 | eval_env = VecTransposeImage(eval_env) 203 | 204 | #### Train the model ####################################### 205 | # checkpoint_callback = CheckpointCallback(save_freq=1000, save_path=filename+'-logs/', name_prefix='rl_model') 206 | # callback_on_best = StopTrainingOnRewardThreshold(reward_threshold=EPISODE_REWARD_THRESHOLD, 207 | # verbose=1 208 | # ) 209 | # eval_callback = EvalCallback(eval_env, 210 | # callback_on_new_best=callback_on_best, 211 | # verbose=1, 212 | # best_model_save_path=filename+'/', 213 | # log_path=filename+'/', 214 | # eval_freq=int(2000/cpu), 215 | # deterministic=True, 216 | # render=False 217 | # ) 218 | model.learn(total_timesteps=steps, #int(1e12), 219 | # callback=eval_callback, 220 | # log_interval=100, 221 | # tb_log_name="DDPG" 222 | ) 223 | 224 | #### Save the model ######################################## 225 | model.save(filename+'/success_model.zip') 226 | print(filename) 227 | 228 | #### Print training progression ############################ 229 | # with np.load(filename+'/evaluations.npz') as data: 230 | # for j in range(data['timesteps'].shape[0]): 231 | # print(str(data['timesteps'][j])+","+str(data['results'][j][0])) 232 | 233 | 234 | if __name__ == "__main__": 235 | #### Define and parse (optional) arguments for the script ## 236 | parser = argparse.ArgumentParser(description='Single agent reinforcement learning experiments script') 237 | parser.add_argument('--env', default=DEFAULT_ENV, type=str, choices=['takeoff', 'hover', 'flythrugate', 'tune'], help='Task (default: hover)', metavar='') 238 | parser.add_argument('--algo', default=DEFAULT_ALGO, type=str, choices=['a2c', 'ppo', 'sac', 'td3', 'ddpg'], help='RL agent (default: ppo)', metavar='') 239 | parser.add_argument('--obs', default=DEFAULT_OBS, type=ObservationType, help='Observation space (default: kin)', metavar='') 240 | parser.add_argument('--act', default=DEFAULT_ACT, type=ActionType, help='Action space (default: one_d_rpm)', metavar='') 241 | parser.add_argument('--cpu', default=DEFAULT_CPU, type=int, help='Number of training environments (default: 1)', metavar='') 242 | parser.add_argument('--steps', default=DEFAULT_STEPS, type=int, help='Number of training time steps (default: 35000)', metavar='') 243 | parser.add_argument('--output_folder', default=DEFAULT_OUTPUT_FOLDER, type=str, help='Folder where to save logs (default: "results")', metavar='') 244 | ARGS = parser.parse_args() 245 | 246 | run(**vars(ARGS)) -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/experiments/learning/test_singleagent.py: -------------------------------------------------------------------------------- 1 | """Test script for single agent problems. 2 | 3 | This scripts runs the best model found by one of the executions of `singleagent.py` 4 | 5 | Example 6 | ------- 7 | To run the script, type in a terminal: 8 | 9 | $ python test_singleagent.py --exp ./results/save----- 10 | 11 | """ 12 | import os 13 | import time 14 | from datetime import datetime 15 | import argparse 16 | import re 17 | from turtle import pos 18 | import numpy as np 19 | import gym 20 | import torch 21 | from stable_baselines3.common.env_checker import check_env 22 | from stable_baselines3 import A2C 23 | from stable_baselines3 import PPO 24 | from stable_baselines3 import SAC 25 | from stable_baselines3 import TD3 26 | from stable_baselines3 import DDPG 27 | import matplotlib as mpl 28 | from mpl_toolkits.mplot3d import Axes3D 29 | import matplotlib.pyplot as plt 30 | from stable_baselines3.common.policies import ActorCriticPolicy as a2cppoMlpPolicy 31 | from stable_baselines3.common.policies import ActorCriticCnnPolicy as a2cppoCnnPolicy 32 | from stable_baselines3.sac.policies import SACPolicy as sacMlpPolicy 33 | from stable_baselines3.sac import CnnPolicy as sacCnnPolicy 34 | from stable_baselines3.td3 import MlpPolicy as td3ddpgMlpPolicy 35 | from stable_baselines3.td3 import CnnPolicy as td3ddpgCnnPolicy 36 | from stable_baselines3.common.evaluation import evaluate_policy 37 | import sys 38 | sys.path.append("C:/software/gym-pybullet-drones") 39 | 40 | from gym_pybullet_drones.envs.ControlAviary import ControlAviary 41 | from gym_pybullet_drones.utils.utils import sync 42 | from gym_pybullet_drones.utils.Logger import Logger 43 | from gym_pybullet_drones.envs.single_agent_rl.BaseSingleAgentAviary import ActionType, ObservationType 44 | from gym_pybullet_drones.utils.utils import sync, str2bool 45 | 46 | import shared_constants 47 | 48 | DEFAULT_GUI = False 49 | DEFAULT_PLOT = True 50 | DEFAULT_OUTPUT_FOLDER = 'results' 51 | class OnnxablePolicy(torch.nn.Module): 52 | def __init__(self, extractor, action_net, value_net): 53 | super(OnnxablePolicy, self).__init__() 54 | self.extractor = extractor 55 | self.action_net = action_net 56 | self.value_net = value_net 57 | 58 | def forward(self, observation): 59 | # NOTE: You may have to process (normalize) observation in the correct 60 | # way before using this. See `common.preprocessing.preprocess_obs` 61 | action_hidden, value_hidden = self.extractor(observation) 62 | return self.action_net(action_hidden), self.value_net(value_hidden) 63 | def run(exp, gui=DEFAULT_GUI, plot=DEFAULT_PLOT, output_folder=DEFAULT_OUTPUT_FOLDER): 64 | #### Load the model from file ############################## 65 | exp="./results/save-ControlAviary-ppo-kin-tun-48HZ-1M" #load the model 66 | ###Note: change the corresponding ADS-B signal frequency in ControlAviary### 67 | algo = exp.split("-")[2] 68 | 69 | if os.path.isfile(exp+'/success_model.zip'): 70 | path = exp+'/success_model.zip' 71 | elif os.path.isfile(exp+'/best_model.zip'): 72 | path = exp+'/best_model.zip' 73 | else: 74 | print("[ERROR]: no model under the specified path", exp) 75 | if algo == 'a2c': 76 | model = A2C.load(path) 77 | if algo == 'ppo': 78 | model = PPO.load(path) 79 | if algo == 'sac': 80 | model = SAC.load(path) 81 | if algo == 'td3': 82 | model = TD3.load(path) 83 | if algo == 'ddpg': 84 | model = DDPG.load(path) 85 | model.policy.to("cpu") 86 | #onnxable_model = OnnxablePolicy(model.policy.mlp_extractor, model.policy.action_net, model.policy.value_net) 87 | 88 | dummy_input = torch.randn(1, 15) 89 | #torch.onnx.export(onnxable_model, dummy_input, "my_ppo_model.onnx", opset_version=9) 90 | #### Parameters to recreate the environment ################ 91 | env_name = exp.split("-")[1]+"-aviary-v0" 92 | OBS = ObservationType.KIN if exp.split("-")[3] == 'kin' else ObservationType.RGB 93 | 94 | # Parse ActionType instance from file name 95 | action_name = exp.split("-")[4] 96 | ACT = [action for action in ActionType if action.value == action_name] 97 | if len(ACT) != 1: 98 | raise AssertionError("Result file could have gotten corrupted. Extracted action type does not match any of the existing ones.") 99 | ACT = ACT.pop() 100 | 101 | ### Evaluate the model #################################### 102 | eval_env = gym.make(env_name, 103 | aggregate_phy_steps=shared_constants.AGGR_PHY_STEPS, 104 | obs=OBS, 105 | act=ACT 106 | ) 107 | mean_reward, std_reward = evaluate_policy(model, 108 | eval_env, 109 | n_eval_episodes=10 110 | ) 111 | print("\n\n\nMean reward ", mean_reward, " +- ", std_reward, "\n\n") 112 | 113 | ### Show, record a video, and log the model's performance # 114 | test_env = gym.make(env_name, 115 | gui=gui, 116 | record=False, 117 | aggregate_phy_steps=shared_constants.AGGR_PHY_STEPS, 118 | obs=OBS, 119 | act=ACT 120 | ) 121 | logger = Logger(logging_freq_hz=int(test_env.SIM_FREQ/test_env.AGGR_PHY_STEPS), 122 | num_drones=1, 123 | output_folder=output_folder 124 | ) 125 | 126 | #Run Simulation for 10 times to find the average cloest distance,step reward and position error 127 | for j in range(10): 128 | obs = test_env.reset() 129 | start = time.time() 130 | adsbinfoes=[] 131 | noises=[] 132 | poses_true=[] 133 | poses_True=[] 134 | close_ds=[] 135 | mean_ds=[] 136 | epsilons=[] 137 | rewardts=[] 138 | for i in range(30*int(test_env.SIM_FREQ/test_env.AGGR_PHY_STEPS)): # Up to 6'' 139 | action, _states = model.predict(obs, 140 | deterministic=True # OPTIONAL 'deterministic=False' 141 | ) 142 | # action=test_env.action_space.sample() 143 | # print(obs,action) 144 | obs, reward, done, info = test_env.step(action) 145 | test_env.render() 146 | adsbinfo,noise,typeA,num_drones,pos_true,mean_d,close_d,epsilon,rewardt=test_env.receiveinfo() 147 | adsbinfoes.append(adsbinfo) 148 | noises.append(noise) 149 | poses_true=pos_true.copy() 150 | poses_True.append(poses_true) 151 | close_ds.append(close_d) 152 | mean_ds.append(mean_d) 153 | epsilons.append(epsilon) 154 | rewardts.append(rewardt) 155 | if OBS==ObservationType.KIN: 156 | logger.log(drone=0, 157 | timestamp=i/test_env.SIM_FREQ, 158 | state= np.hstack([obs[0:3], np.zeros(4), obs[3:15], np.resize(action, (1))]), 159 | control=np.zeros(12) 160 | ) 161 | sync(np.floor(i*test_env.AGGR_PHY_STEPS), start, test_env.TIMESTEP) 162 | # if done: obs = test_env.reset() # OPTIONAL EPISODE HALT 163 | #nums = list(map(int, cloest_d)) 164 | nums=np.array(close_ds) 165 | True_close_d= nums[0] 166 | for i in range(len(nums)): 167 | if nums[i] < True_close_d: 168 | True_close_d = nums[i] 169 | meannums=np.array(mean_ds) 170 | True_mean_ds=sum(nums)/len(nums) 171 | print(True_close_d,True_mean_ds) 172 | ep=np.array(epsilons) 173 | re=np.array(rewardts) 174 | np.savetxt('reward.txt', re, fmt="%.4f", delimiter=',') 175 | np.savetxt('output.txt', ep, fmt="%.4f", delimiter=',') 176 | test_env.close() 177 | 178 | ##Plot the path of drones 179 | logger.save_as_csv("sa") # Optional CSV save 180 | if plot: 181 | 182 | #logger.plot() 183 | 184 | fig = plt.figure(figsize=(12,12)) 185 | ax1 = plt.subplot(projection='3d') 186 | noisesdata=np.array(noises) 187 | 188 | ax1.scatter(noisesdata[:,typeA-1,0],noisesdata[:,typeA-1,1], noisesdata[:,typeA-1,2],cmap='Blues', s=1,label='original input') 189 | 190 | adsbinfoesdata=np.array(adsbinfoes) 191 | ax1.plot3D(adsbinfoesdata[:,typeA-1,0],adsbinfoesdata[:,typeA-1,1],adsbinfoesdata[:,typeA-1,2],'gray',label='filter output') 192 | 193 | posdata=np.array(poses_True) 194 | ax1.plot3D(posdata[:,typeA-1,0],posdata[:,typeA-1,1],posdata[:,typeA-1,2],'red',label='actual position') 195 | ax1.legend() 196 | ax1.set_xlabel('x-axis (m)') 197 | ax1.set_ylabel('y-axis (m)') 198 | ax1.set_zlabel('z-axis (m)') 199 | fig = plt.figure(figsize=(12,12)) 200 | ax1 = plt.subplot(projection='3d')#fig.gca(projection='3d') 201 | for i in range(typeA-3): 202 | ax1.plot3D(adsbinfoesdata[:,i,0],adsbinfoesdata[:,i,1],adsbinfoesdata[:,i,2],'gray') 203 | ax1.plot3D(adsbinfoesdata[:,typeA-2,0],adsbinfoesdata[:,typeA-2,1],adsbinfoesdata[:,typeA-2,2],'gray',label='Paths of the Type B UAV') 204 | ax1.plot3D(adsbinfoesdata[:,typeA-1,0],adsbinfoesdata[:,typeA-1,1],adsbinfoesdata[:,typeA-1,2],'red', label='Paths of Type A UAV') 205 | ax1.legend() 206 | ax1.set_xlabel('x-axis (m)') 207 | ax1.set_ylabel('y-axis (m)') 208 | ax1.set_zlabel('z-axis (m)') 209 | plt.show() 210 | 211 | 212 | # with np.load(exp+'/evaluations.npz') as data: 213 | # print(data.files) 214 | # print(data['timesteps']) 215 | # print(data['results']) 216 | # print(data['ep_lengths']) 217 | 218 | if __name__ == "__main__": 219 | #### Define and parse (optional) arguments for the script ## 220 | parser = argparse.ArgumentParser(description='Single agent reinforcement learning example script using TakeoffAviary') 221 | parser.add_argument('--exp', type=str, help='The experiment folder written as ./results/save-----', metavar='') 222 | parser.add_argument('--gui', default=DEFAULT_GUI, type=str2bool, help='Whether to use PyBullet GUI (default: False)', metavar='') 223 | parser.add_argument('--plot', default=DEFAULT_PLOT, type=str2bool, help='Whether to plot the simulation results (default: True)', metavar='') 224 | parser.add_argument('--output_folder', default=DEFAULT_OUTPUT_FOLDER, type=str, help='Folder where to save logs (default: "results")', metavar='') 225 | ARGS = parser.parse_args() 226 | 227 | run(**vars(ARGS)) -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/__init__.py: -------------------------------------------------------------------------------- 1 | from gym.envs.registration import register 2 | 3 | register( 4 | id='ctrl-aviary-v0', 5 | entry_point='gym_pybullet_drones.envs:CtrlAviary', 6 | ) 7 | register( 8 | id='ControlAviary-aviary-v0', 9 | entry_point='gym_pybullet_drones.envs:ControlAviary', 10 | ) 11 | register( 12 | id='dyn-aviary-v0', 13 | entry_point='gym_pybullet_drones.envs:DynAviary', 14 | ) 15 | 16 | register( 17 | id='velocity-aviary-v0', 18 | entry_point='gym_pybullet_drones.envs:VelocityAviary', 19 | ) 20 | 21 | register( 22 | id='vision-aviary-v0', 23 | entry_point='gym_pybullet_drones.envs:VisionAviary', 24 | ) 25 | 26 | 27 | 28 | 29 | register( 30 | id='takeoff-aviary-v0', 31 | entry_point='gym_pybullet_drones.envs.single_agent_rl:TakeoffAviary', 32 | ) 33 | 34 | register( 35 | id='hover-aviary-v0', 36 | entry_point='gym_pybullet_drones.envs.single_agent_rl:HoverAviary', 37 | ) 38 | 39 | register( 40 | id='flythrugate-aviary-v0', 41 | entry_point='gym_pybullet_drones.envs.single_agent_rl:FlyThruGateAviary', 42 | ) 43 | 44 | register( 45 | id='tune-aviary-v0', 46 | entry_point='gym_pybullet_drones.envs.single_agent_rl:TuneAviary', 47 | ) 48 | 49 | 50 | 51 | 52 | register( 53 | id='flock-aviary-v0', 54 | entry_point='gym_pybullet_drones.envs.multi_agent_rl:FlockAviary', 55 | ) 56 | 57 | register( 58 | id='leaderfollower-aviary-v0', 59 | entry_point='gym_pybullet_drones.envs.multi_agent_rl:LeaderFollowerAviary', 60 | ) 61 | 62 | register( 63 | id='meetup-aviary-v0', 64 | entry_point='gym_pybullet_drones.envs.multi_agent_rl:MeetupAviary', 65 | ) -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/__pycache__/__init__.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/__pycache__/__init__.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/__pycache__/__init__.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/__pycache__/__init__.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/assets/architrave.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/assets/cf2p.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/assets/cf2x.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/assets/example_trace.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/assets/example_trace.pkl -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/assets/hb.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/BaseControl.py: -------------------------------------------------------------------------------- 1 | import os 2 | import numpy as np 3 | import xml.etree.ElementTree as etxml 4 | import pkg_resources 5 | 6 | from gym_pybullet_drones.utils.enums import DroneModel 7 | 8 | class BaseControl(object): 9 | """Base class for control. 10 | 11 | Implements `__init__()`, `reset(), and interface `computeControlFromState()`, 12 | the main method `computeControl()` should be implemented by its subclasses. 13 | 14 | """ 15 | 16 | ################################################################################ 17 | 18 | def __init__(self, 19 | drone_model: DroneModel, 20 | g: float=9.8 21 | ): 22 | """Common control classes __init__ method. 23 | 24 | Parameters 25 | ---------- 26 | drone_model : DroneModel 27 | The type of drone to control (detailed in an .urdf file in folder `assets`). 28 | g : float, optional 29 | The gravitational acceleration in m/s^2. 30 | 31 | """ 32 | #### Set general use constants ############################# 33 | self.DRONE_MODEL = drone_model 34 | """DroneModel: The type of drone to control.""" 35 | self.GRAVITY = g*self._getURDFParameter('m') 36 | """float: The gravitational force (M*g) acting on each drone.""" 37 | self.KF = self._getURDFParameter('kf') 38 | """float: The coefficient converting RPMs into thrust.""" 39 | self.KM = self._getURDFParameter('km') 40 | """float: The coefficient converting RPMs into torque.""" 41 | self.reset() 42 | 43 | ################################################################################ 44 | 45 | def reset(self): 46 | """Reset the control classes. 47 | 48 | A general use counter is set to zero. 49 | 50 | """ 51 | self.control_counter = 0 52 | 53 | ################################################################################ 54 | 55 | def computeControlFromState(self, 56 | control_timestep, 57 | state, 58 | target_pos, 59 | target_rpy=np.zeros(3), 60 | target_vel=np.zeros(3), 61 | target_rpy_rates=np.zeros(3) 62 | ): 63 | """Interface method using `computeControl`. 64 | 65 | It can be used to compute a control action directly from the value of key "state" 66 | in the `obs` returned by a call to BaseAviary.step(). 67 | 68 | Parameters 69 | ---------- 70 | control_timestep : float 71 | The time step at which control is computed. 72 | state : ndarray 73 | (20,)-shaped array of floats containing the current state of the drone. 74 | target_pos : ndarray 75 | (3,1)-shaped array of floats containing the desired position. 76 | target_rpy : ndarray, optional 77 | (3,1)-shaped array of floats containing the desired orientation as roll, pitch, yaw. 78 | target_vel : ndarray, optional 79 | (3,1)-shaped array of floats containing the desired velocity. 80 | target_rpy_rates : ndarray, optional 81 | (3,1)-shaped array of floats containing the desired roll, pitch, and yaw rates. 82 | 83 | """ 84 | return self.computeControl(control_timestep=control_timestep, 85 | cur_pos=state[0:3], 86 | cur_quat=state[3:7], 87 | cur_vel=state[10:13], 88 | cur_ang_vel=state[13:16], 89 | target_pos=target_pos, 90 | target_rpy=target_rpy, 91 | target_vel=target_vel, 92 | target_rpy_rates=target_rpy_rates 93 | ) 94 | 95 | ################################################################################ 96 | 97 | def computeControl(self, 98 | control_timestep, 99 | cur_pos, 100 | cur_quat, 101 | cur_vel, 102 | cur_ang_vel, 103 | target_pos, 104 | target_rpy=np.zeros(3), 105 | target_vel=np.zeros(3), 106 | target_rpy_rates=np.zeros(3) 107 | ): 108 | """Abstract method to compute the control action for a single drone. 109 | 110 | It must be implemented by each subclass of `BaseControl`. 111 | 112 | Parameters 113 | ---------- 114 | control_timestep : float 115 | The time step at which control is computed. 116 | cur_pos : ndarray 117 | (3,1)-shaped array of floats containing the current position. 118 | cur_quat : ndarray 119 | (4,1)-shaped array of floats containing the current orientation as a quaternion. 120 | cur_vel : ndarray 121 | (3,1)-shaped array of floats containing the current velocity. 122 | cur_ang_vel : ndarray 123 | (3,1)-shaped array of floats containing the current angular velocity. 124 | target_pos : ndarray 125 | (3,1)-shaped array of floats containing the desired position. 126 | target_rpy : ndarray, optional 127 | (3,1)-shaped array of floats containing the desired orientation as roll, pitch, yaw. 128 | target_vel : ndarray, optional 129 | (3,1)-shaped array of floats containing the desired velocity. 130 | target_rpy_rates : ndarray, optional 131 | (3,1)-shaped array of floats containing the desired roll, pitch, and yaw rates. 132 | 133 | """ 134 | raise NotImplementedError 135 | 136 | ################################################################################ 137 | 138 | def setPIDCoefficients(self, 139 | p_coeff_pos=None, 140 | i_coeff_pos=None, 141 | d_coeff_pos=None, 142 | p_coeff_att=None, 143 | i_coeff_att=None, 144 | d_coeff_att=None 145 | ): 146 | """Sets the coefficients of a PID controller. 147 | 148 | This method throws an error message and exist is the coefficients 149 | were not initialized (e.g. when the controller is not a PID one). 150 | 151 | Parameters 152 | ---------- 153 | p_coeff_pos : ndarray, optional 154 | (3,1)-shaped array of floats containing the position control proportional coefficients. 155 | i_coeff_pos : ndarray, optional 156 | (3,1)-shaped array of floats containing the position control integral coefficients. 157 | d_coeff_pos : ndarray, optional 158 | (3,1)-shaped array of floats containing the position control derivative coefficients. 159 | p_coeff_att : ndarray, optional 160 | (3,1)-shaped array of floats containing the attitude control proportional coefficients. 161 | i_coeff_att : ndarray, optional 162 | (3,1)-shaped array of floats containing the attitude control integral coefficients. 163 | d_coeff_att : ndarray, optional 164 | (3,1)-shaped array of floats containing the attitude control derivative coefficients. 165 | 166 | """ 167 | ATTR_LIST = ['P_COEFF_FOR', 'I_COEFF_FOR', 'D_COEFF_FOR', 'P_COEFF_TOR', 'I_COEFF_TOR', 'D_COEFF_TOR'] 168 | if not all(hasattr(self, attr) for attr in ATTR_LIST): 169 | print("[ERROR] in BaseControl.setPIDCoefficients(), not all PID coefficients exist as attributes in the instantiated control class.") 170 | exit() 171 | else: 172 | self.P_COEFF_FOR = self.P_COEFF_FOR if p_coeff_pos is None else p_coeff_pos 173 | self.I_COEFF_FOR = self.I_COEFF_FOR if i_coeff_pos is None else i_coeff_pos 174 | self.D_COEFF_FOR = self.D_COEFF_FOR if d_coeff_pos is None else d_coeff_pos 175 | self.P_COEFF_TOR = self.P_COEFF_TOR if p_coeff_att is None else p_coeff_att 176 | self.I_COEFF_TOR = self.I_COEFF_TOR if i_coeff_att is None else i_coeff_att 177 | self.D_COEFF_TOR = self.D_COEFF_TOR if d_coeff_att is None else d_coeff_att 178 | 179 | ################################################################################ 180 | 181 | def _getURDFParameter(self, 182 | parameter_name: str 183 | ): 184 | """Reads a parameter from a drone's URDF file. 185 | 186 | This method is nothing more than a custom XML parser for the .urdf 187 | files in folder `assets/`. 188 | 189 | Parameters 190 | ---------- 191 | parameter_name : str 192 | The name of the parameter to read. 193 | 194 | Returns 195 | ------- 196 | float 197 | The value of the parameter. 198 | 199 | """ 200 | #### Get the XML tree of the drone model to control ######## 201 | URDF = self.DRONE_MODEL.value + ".urdf" 202 | path = pkg_resources.resource_filename('gym_pybullet_drones', 'assets/'+URDF) 203 | URDF_TREE = etxml.parse(path).getroot() 204 | #### Find and return the desired parameter ################# 205 | if parameter_name == 'm': 206 | return float(URDF_TREE[1][0][1].attrib['value']) 207 | elif parameter_name in ['ixx', 'iyy', 'izz']: 208 | return float(URDF_TREE[1][0][2].attrib[parameter_name]) 209 | elif parameter_name in ['arm', 'thrust2weight', 'kf', 'km', 'max_speed_kmh', 'gnd_eff_coeff' 'prop_radius', \ 210 | 'drag_coeff_xy', 'drag_coeff_z', 'dw_coeff_1', 'dw_coeff_2', 'dw_coeff_3']: 211 | return float(URDF_TREE[0].attrib[parameter_name]) 212 | elif parameter_name in ['length', 'radius']: 213 | return float(URDF_TREE[1][2][1][0].attrib[parameter_name]) 214 | elif parameter_name == 'collision_z_offset': 215 | COLLISION_SHAPE_OFFSETS = [float(s) for s in URDF_TREE[1][2][0].attrib['xyz'].split(' ')] 216 | return COLLISION_SHAPE_OFFSETS[2] 217 | -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/DSLPIDControl.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | import pybullet as p 4 | from scipy.spatial.transform import Rotation 5 | 6 | from gym_pybullet_drones.control.BaseControl import BaseControl 7 | from gym_pybullet_drones.utils.enums import DroneModel 8 | 9 | class DSLPIDControl(BaseControl): 10 | """PID control class for Crazyflies. 11 | 12 | Based on work conducted at UTIAS' DSL. Contributors: SiQi Zhou, James Xu, 13 | Tracy Du, Mario Vukosavljev, Calvin Ngan, and Jingyuan Hou. 14 | 15 | """ 16 | 17 | ################################################################################ 18 | 19 | def __init__(self, 20 | drone_model: DroneModel, 21 | g: float=9.8 22 | ): 23 | """Common control classes __init__ method. 24 | 25 | Parameters 26 | ---------- 27 | drone_model : DroneModel 28 | The type of drone to control (detailed in an .urdf file in folder `assets`). 29 | g : float, optional 30 | The gravitational acceleration in m/s^2. 31 | 32 | """ 33 | super().__init__(drone_model=drone_model, g=g) 34 | if self.DRONE_MODEL != DroneModel.CF2X and self.DRONE_MODEL != DroneModel.CF2P: 35 | print("[ERROR] in DSLPIDControl.__init__(), DSLPIDControl requires DroneModel.CF2X or DroneModel.CF2P") 36 | exit() 37 | self.P_COEFF_FOR = np.array([.4, .4, 1.25]) 38 | self.I_COEFF_FOR = np.array([.05, .05, .05]) 39 | self.D_COEFF_FOR = np.array([.2, .2, .5]) 40 | self.P_COEFF_TOR = np.array([70000., 70000., 60000.]) 41 | self.I_COEFF_TOR = np.array([.0, .0, 500.]) 42 | self.D_COEFF_TOR = np.array([20000., 20000., 12000.]) 43 | self.PWM2RPM_SCALE = 0.2685 44 | self.PWM2RPM_CONST = 4070.3 45 | self.MIN_PWM = 20000 46 | self.MAX_PWM = 65535 47 | if self.DRONE_MODEL == DroneModel.CF2X: 48 | self.MIXER_MATRIX = np.array([ [.5, -.5, -1], [.5, .5, 1], [-.5, .5, -1], [-.5, -.5, 1] ]) 49 | elif self.DRONE_MODEL == DroneModel.CF2P: 50 | self.MIXER_MATRIX = np.array([ [0, -1, -1], [+1, 0, 1], [0, 1, -1], [-1, 0, 1] ]) 51 | self.reset() 52 | 53 | ################################################################################ 54 | 55 | def reset(self): 56 | """Resets the control classes. 57 | 58 | The previous step's and integral errors for both position and attitude are set to zero. 59 | 60 | """ 61 | super().reset() 62 | #### Store the last roll, pitch, and yaw ################### 63 | self.last_rpy = np.zeros(3) 64 | #### Initialized PID control variables ##################### 65 | self.last_pos_e = np.zeros(3) 66 | self.integral_pos_e = np.zeros(3) 67 | self.last_rpy_e = np.zeros(3) 68 | self.integral_rpy_e = np.zeros(3) 69 | 70 | ################################################################################ 71 | 72 | def computeControl(self, 73 | control_timestep, 74 | cur_pos, 75 | cur_quat, 76 | cur_vel, 77 | cur_ang_vel, 78 | target_pos, 79 | target_rpy=np.zeros(3), 80 | target_vel=np.zeros(3), 81 | target_rpy_rates=np.zeros(3) 82 | ): 83 | """Computes the PID control action (as RPMs) for a single drone. 84 | 85 | This methods sequentially calls `_dslPIDPositionControl()` and `_dslPIDAttitudeControl()`. 86 | Parameter `cur_ang_vel` is unused. 87 | 88 | Parameters 89 | ---------- 90 | control_timestep : float 91 | The time step at which control is computed. 92 | cur_pos : ndarray 93 | (3,1)-shaped array of floats containing the current position. 94 | cur_quat : ndarray 95 | (4,1)-shaped array of floats containing the current orientation as a quaternion. 96 | cur_vel : ndarray 97 | (3,1)-shaped array of floats containing the current velocity. 98 | cur_ang_vel : ndarray 99 | (3,1)-shaped array of floats containing the current angular velocity. 100 | target_pos : ndarray 101 | (3,1)-shaped array of floats containing the desired position. 102 | target_rpy : ndarray, optional 103 | (3,1)-shaped array of floats containing the desired orientation as roll, pitch, yaw. 104 | target_vel : ndarray, optional 105 | (3,1)-shaped array of floats containing the desired velocity. 106 | target_rpy_rates : ndarray, optional 107 | (3,1)-shaped array of floats containing the desired roll, pitch, and yaw rates. 108 | 109 | Returns 110 | ------- 111 | ndarray 112 | (4,1)-shaped array of integers containing the RPMs to apply to each of the 4 motors. 113 | ndarray 114 | (3,1)-shaped array of floats containing the current XYZ position error. 115 | float 116 | The current yaw error. 117 | 118 | """ 119 | self.control_counter += 1 120 | thrust, computed_target_rpy, pos_e = self._dslPIDPositionControl(control_timestep, 121 | cur_pos, 122 | cur_quat, 123 | cur_vel, 124 | target_pos, 125 | target_rpy, 126 | target_vel 127 | ) 128 | rpm = self._dslPIDAttitudeControl(control_timestep, 129 | thrust, 130 | cur_quat, 131 | computed_target_rpy, 132 | target_rpy_rates 133 | ) 134 | cur_rpy = p.getEulerFromQuaternion(cur_quat) 135 | return rpm, pos_e, computed_target_rpy[2] - cur_rpy[2] 136 | 137 | ################################################################################ 138 | 139 | def _dslPIDPositionControl(self, 140 | control_timestep, 141 | cur_pos, 142 | cur_quat, 143 | cur_vel, 144 | target_pos, 145 | target_rpy, 146 | target_vel 147 | ): 148 | """DSL's CF2.x PID position control. 149 | 150 | Parameters 151 | ---------- 152 | control_timestep : float 153 | The time step at which control is computed. 154 | cur_pos : ndarray 155 | (3,1)-shaped array of floats containing the current position. 156 | cur_quat : ndarray 157 | (4,1)-shaped array of floats containing the current orientation as a quaternion. 158 | cur_vel : ndarray 159 | (3,1)-shaped array of floats containing the current velocity. 160 | target_pos : ndarray 161 | (3,1)-shaped array of floats containing the desired position. 162 | target_rpy : ndarray 163 | (3,1)-shaped array of floats containing the desired orientation as roll, pitch, yaw. 164 | target_vel : ndarray 165 | (3,1)-shaped array of floats containing the desired velocity. 166 | 167 | Returns 168 | ------- 169 | float 170 | The target thrust along the drone z-axis. 171 | ndarray 172 | (3,1)-shaped array of floats containing the target roll, pitch, and yaw. 173 | float 174 | The current position error. 175 | 176 | """ 177 | cur_rotation = np.array(p.getMatrixFromQuaternion(cur_quat)).reshape(3, 3) 178 | pos_e = target_pos - cur_pos 179 | vel_e = target_vel - cur_vel 180 | self.integral_pos_e = self.integral_pos_e + pos_e*control_timestep 181 | self.integral_pos_e = np.clip(self.integral_pos_e, -2., 2.) 182 | self.integral_pos_e[2] = np.clip(self.integral_pos_e[2], -0.15, .15) 183 | #### PID target thrust ##################################### 184 | target_thrust = np.multiply(self.P_COEFF_FOR, pos_e) \ 185 | + np.multiply(self.I_COEFF_FOR, self.integral_pos_e) \ 186 | + np.multiply(self.D_COEFF_FOR, vel_e) + np.array([0, 0, self.GRAVITY]) 187 | scalar_thrust = max(0., np.dot(target_thrust, cur_rotation[:,2])) 188 | thrust = (math.sqrt(scalar_thrust / (4*self.KF)) - self.PWM2RPM_CONST) / self.PWM2RPM_SCALE 189 | target_z_ax = target_thrust / np.linalg.norm(target_thrust) 190 | target_x_c = np.array([math.cos(target_rpy[2]), math.sin(target_rpy[2]), 0]) 191 | target_y_ax = np.cross(target_z_ax, target_x_c) / np.linalg.norm(np.cross(target_z_ax, target_x_c)) 192 | target_x_ax = np.cross(target_y_ax, target_z_ax) 193 | target_rotation = (np.vstack([target_x_ax, target_y_ax, target_z_ax])).transpose() 194 | #### Target rotation ####################################### 195 | target_euler = (Rotation.from_matrix(target_rotation)).as_euler('XYZ', degrees=False) 196 | if np.any(np.abs(target_euler) > math.pi): 197 | print("\n[ERROR] ctrl it", self.control_counter, "in Control._dslPIDPositionControl(), values outside range [-pi,pi]") 198 | return thrust, target_euler, pos_e 199 | 200 | ################################################################################ 201 | 202 | def _dslPIDAttitudeControl(self, 203 | control_timestep, 204 | thrust, 205 | cur_quat, 206 | target_euler, 207 | target_rpy_rates 208 | ): 209 | """DSL's CF2.x PID attitude control. 210 | 211 | Parameters 212 | ---------- 213 | control_timestep : float 214 | The time step at which control is computed. 215 | thrust : float 216 | The target thrust along the drone z-axis. 217 | cur_quat : ndarray 218 | (4,1)-shaped array of floats containing the current orientation as a quaternion. 219 | target_euler : ndarray 220 | (3,1)-shaped array of floats containing the computed target Euler angles. 221 | target_rpy_rates : ndarray 222 | (3,1)-shaped array of floats containing the desired roll, pitch, and yaw rates. 223 | 224 | Returns 225 | ------- 226 | ndarray 227 | (4,1)-shaped array of integers containing the RPMs to apply to each of the 4 motors. 228 | 229 | """ 230 | cur_rotation = np.array(p.getMatrixFromQuaternion(cur_quat)).reshape(3, 3) 231 | cur_rpy = np.array(p.getEulerFromQuaternion(cur_quat)) 232 | target_quat = (Rotation.from_euler('XYZ', target_euler, degrees=False)).as_quat() 233 | w,x,y,z = target_quat 234 | target_rotation = (Rotation.from_quat([w, x, y, z])).as_matrix() 235 | rot_matrix_e = np.dot((target_rotation.transpose()),cur_rotation) - np.dot(cur_rotation.transpose(),target_rotation) 236 | rot_e = np.array([rot_matrix_e[2, 1], rot_matrix_e[0, 2], rot_matrix_e[1, 0]]) 237 | rpy_rates_e = target_rpy_rates - (cur_rpy - self.last_rpy)/control_timestep 238 | self.last_rpy = cur_rpy 239 | self.integral_rpy_e = self.integral_rpy_e - rot_e*control_timestep 240 | self.integral_rpy_e = np.clip(self.integral_rpy_e, -1500., 1500.) 241 | self.integral_rpy_e[0:2] = np.clip(self.integral_rpy_e[0:2], -1., 1.) 242 | #### PID target torques #################################### 243 | target_torques = - np.multiply(self.P_COEFF_TOR, rot_e) \ 244 | + np.multiply(self.D_COEFF_TOR, rpy_rates_e) \ 245 | + np.multiply(self.I_COEFF_TOR, self.integral_rpy_e) 246 | target_torques = np.clip(target_torques, -3200, 3200) 247 | pwm = thrust + np.dot(self.MIXER_MATRIX, target_torques) 248 | pwm = np.clip(pwm, self.MIN_PWM, self.MAX_PWM) 249 | return self.PWM2RPM_SCALE * pwm + self.PWM2RPM_CONST 250 | 251 | ################################################################################ 252 | 253 | def _one23DInterface(self, 254 | thrust 255 | ): 256 | """Utility function interfacing 1, 2, or 3D thrust input use cases. 257 | 258 | Parameters 259 | ---------- 260 | thrust : ndarray 261 | Array of floats of length 1, 2, or 4 containing a desired thrust input. 262 | 263 | Returns 264 | ------- 265 | ndarray 266 | (4,1)-shaped array of integers containing the PWM (not RPMs) to apply to each of the 4 motors. 267 | 268 | """ 269 | DIM = len(np.array(thrust)) 270 | pwm = np.clip((np.sqrt(np.array(thrust)/(self.KF*(4/DIM)))-self.PWM2RPM_CONST)/self.PWM2RPM_SCALE, self.MIN_PWM, self.MAX_PWM) 271 | if DIM in [1, 4]: 272 | return np.repeat(pwm, 4/DIM) 273 | elif DIM==2: 274 | return np.hstack([pwm, np.flip(pwm)]) 275 | else: 276 | print("[ERROR] in DSLPIDControl._one23DInterface()") 277 | exit() 278 | -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/SimplePIDControl.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pybullet as p 3 | 4 | from gym_pybullet_drones.control.BaseControl import BaseControl 5 | from gym_pybullet_drones.utils.enums import DroneModel 6 | from gym_pybullet_drones.utils.utils import nnlsRPM 7 | 8 | class SimplePIDControl(BaseControl): 9 | """Generic PID control class without yaw control. 10 | 11 | Based on https://github.com/prfraanje/quadcopter_sim. 12 | 13 | """ 14 | 15 | ################################################################################ 16 | 17 | def __init__(self, 18 | drone_model: DroneModel, 19 | g: float=9.8 20 | ): 21 | """Common control classes __init__ method. 22 | 23 | Parameters 24 | ---------- 25 | drone_model : DroneModel 26 | The type of drone to control (detailed in an .urdf file in folder `assets`). 27 | g : float, optional 28 | The gravitational acceleration in m/s^2. 29 | 30 | """ 31 | super().__init__(drone_model=drone_model, g=g) 32 | if self.DRONE_MODEL != DroneModel.HB: 33 | print("[ERROR] in SimplePIDControl.__init__(), SimplePIDControl requires DroneModel.HB") 34 | exit() 35 | self.P_COEFF_FOR = np.array([.1, .1, .2]) 36 | self.I_COEFF_FOR = np.array([.0001, .0001, .0001]) 37 | self.D_COEFF_FOR = np.array([.3, .3, .4]) 38 | self.P_COEFF_TOR = np.array([.3, .3, .05]) 39 | self.I_COEFF_TOR = np.array([.0001, .0001, .0001]) 40 | self.D_COEFF_TOR = np.array([.3, .3, .5]) 41 | self.MAX_ROLL_PITCH = np.pi/6 42 | self.L = self._getURDFParameter('arm') 43 | self.THRUST2WEIGHT_RATIO = self._getURDFParameter('thrust2weight') 44 | self.MAX_RPM = np.sqrt((self.THRUST2WEIGHT_RATIO*self.GRAVITY) / (4*self.KF)) 45 | self.MAX_THRUST = (4*self.KF*self.MAX_RPM**2) 46 | self.MAX_XY_TORQUE = (self.L*self.KF*self.MAX_RPM**2) 47 | self.MAX_Z_TORQUE = (2*self.KM*self.MAX_RPM**2) 48 | self.A = np.array([ [1, 1, 1, 1], [0, 1, 0, -1], [-1, 0, 1, 0], [-1, 1, -1, 1] ]) 49 | self.INV_A = np.linalg.inv(self.A) 50 | self.B_COEFF = np.array([1/self.KF, 1/(self.KF*self.L), 1/(self.KF*self.L), 1/self.KM]) 51 | self.reset() 52 | 53 | ################################################################################ 54 | 55 | def reset(self): 56 | """Resets the control classes. 57 | 58 | The previous step's and integral errors for both position and attitude are set to zero. 59 | 60 | """ 61 | super().reset() 62 | #### Initialized PID control variables ##################### 63 | self.last_pos_e = np.zeros(3) 64 | self.integral_pos_e = np.zeros(3) 65 | self.last_rpy_e = np.zeros(3) 66 | self.integral_rpy_e = np.zeros(3) 67 | 68 | ################################################################################ 69 | 70 | def computeControl(self, 71 | control_timestep, 72 | cur_pos, 73 | cur_quat, 74 | cur_vel, 75 | cur_ang_vel, 76 | target_pos, 77 | target_rpy=np.zeros(3), 78 | target_vel=np.zeros(3), 79 | target_rpy_rates=np.zeros(3) 80 | ): 81 | """Computes the PID control action (as RPMs) for a single drone. 82 | 83 | This methods sequentially calls `_simplePIDPositionControl()` and `_simplePIDAttitudeControl()`. 84 | Parameters `cur_ang_vel`, `target_rpy`, `target_vel`, and `target_rpy_rates` are unused. 85 | 86 | Parameters 87 | ---------- 88 | control_timestep : float 89 | The time step at which control is computed. 90 | cur_pos : ndarray 91 | (3,1)-shaped array of floats containing the current position. 92 | cur_quat : ndarray 93 | (4,1)-shaped array of floats containing the current orientation as a quaternion. 94 | cur_vel : ndarray 95 | (3,1)-shaped array of floats containing the current velocity. 96 | cur_ang_vel : ndarray 97 | (3,1)-shaped array of floats containing the current angular velocity. 98 | target_pos : ndarray 99 | (3,1)-shaped array of floats containing the desired position. 100 | target_rpy : ndarray, optional 101 | (3,1)-shaped array of floats containing the desired orientation as roll, pitch, yaw. 102 | target_vel : ndarray, optional 103 | (3,1)-shaped array of floats containing the desired velocity. 104 | target_rpy_rates : ndarray, optional 105 | (3,1)-shaped array of floats containing the the desired roll, pitch, and yaw rates. 106 | 107 | Returns 108 | ------- 109 | ndarray 110 | (4,1)-shaped array of integers containing the RPMs to apply to each of the 4 motors. 111 | ndarray 112 | (3,1)-shaped array of floats containing the current XYZ position error. 113 | float 114 | The current yaw error. 115 | 116 | """ 117 | self.control_counter += 1 118 | if target_rpy[2]!=0: 119 | print("\n[WARNING] ctrl it", self.control_counter, "in SimplePIDControl.computeControl(), desired yaw={:.0f}deg but locked to 0. for DroneModel.HB".format(target_rpy[2]*(180/np.pi))) 120 | thrust, computed_target_rpy, pos_e = self._simplePIDPositionControl(control_timestep, 121 | cur_pos, 122 | cur_quat, 123 | target_pos 124 | ) 125 | rpm = self._simplePIDAttitudeControl(control_timestep, 126 | thrust, 127 | cur_quat, 128 | computed_target_rpy 129 | ) 130 | cur_rpy = p.getEulerFromQuaternion(cur_quat) 131 | return rpm, pos_e, computed_target_rpy[2] - cur_rpy[2] 132 | 133 | ################################################################################ 134 | 135 | def _simplePIDPositionControl(self, 136 | control_timestep, 137 | cur_pos, 138 | cur_quat, 139 | target_pos 140 | ): 141 | """Simple PID position control (with yaw fixed to 0). 142 | 143 | Parameters 144 | ---------- 145 | control_timestep : float 146 | The time step at which control is computed. 147 | cur_pos : ndarray 148 | (3,1)-shaped array of floats containing the current position. 149 | cur_quat : ndarray 150 | (4,1)-shaped array of floats containing the current orientation as a quaternion. 151 | target_pos : ndarray 152 | (3,1)-shaped array of floats containing the desired position. 153 | 154 | Returns 155 | ------- 156 | float 157 | The target thrust along the drone z-axis. 158 | ndarray 159 | (3,1)-shaped array of floats containing the target roll, pitch, and yaw. 160 | float 161 | The current position error. 162 | 163 | """ 164 | pos_e = target_pos - np.array(cur_pos).reshape(3) 165 | d_pos_e = (pos_e - self.last_pos_e) / control_timestep 166 | self.last_pos_e = pos_e 167 | self.integral_pos_e = self.integral_pos_e + pos_e*control_timestep 168 | #### PID target thrust ##################################### 169 | target_force = np.array([0, 0, self.GRAVITY]) \ 170 | + np.multiply(self.P_COEFF_FOR, pos_e) \ 171 | + np.multiply(self.I_COEFF_FOR, self.integral_pos_e) \ 172 | + np.multiply(self.D_COEFF_FOR, d_pos_e) 173 | target_rpy = np.zeros(3) 174 | sign_z = np.sign(target_force[2]) 175 | if sign_z == 0: 176 | sign_z = 1 177 | #### Target rotation ####################################### 178 | target_rpy[0] = np.arcsin(-sign_z*target_force[1] / np.linalg.norm(target_force)) 179 | target_rpy[1] = np.arctan2(sign_z*target_force[0], sign_z*target_force[2]) 180 | target_rpy[2] = 0. 181 | target_rpy[0] = np.clip(target_rpy[0], -self.MAX_ROLL_PITCH, self.MAX_ROLL_PITCH) 182 | target_rpy[1] = np.clip(target_rpy[1], -self.MAX_ROLL_PITCH, self.MAX_ROLL_PITCH) 183 | cur_rotation = np.array(p.getMatrixFromQuaternion(cur_quat)).reshape(3, 3) 184 | thrust = np.dot(cur_rotation, target_force) 185 | return thrust[2], target_rpy, pos_e 186 | 187 | ################################################################################ 188 | 189 | def _simplePIDAttitudeControl(self, 190 | control_timestep, 191 | thrust, 192 | cur_quat, 193 | target_rpy 194 | ): 195 | """Simple PID attitude control (with yaw fixed to 0). 196 | 197 | Parameters 198 | ---------- 199 | control_timestep : float 200 | The time step at which control is computed. 201 | thrust : float 202 | The target thrust along the drone z-axis. 203 | cur_quat : ndarray 204 | (4,1)-shaped array of floats containing the current orientation as a quaternion. 205 | target_rpy : ndarray 206 | (3,1)-shaped array of floats containing the computed the target roll, pitch, and yaw. 207 | 208 | Returns 209 | ------- 210 | ndarray 211 | (4,1)-shaped array of integers containing the RPMs to apply to each of the 4 motors. 212 | 213 | """ 214 | cur_rpy = p.getEulerFromQuaternion(cur_quat) 215 | rpy_e = target_rpy - np.array(cur_rpy).reshape(3,) 216 | if rpy_e[2] > np.pi: 217 | rpy_e[2] = rpy_e[2] - 2*np.pi 218 | if rpy_e[2] < -np.pi: 219 | rpy_e[2] = rpy_e[2] + 2*np.pi 220 | d_rpy_e = (rpy_e - self.last_rpy_e) / control_timestep 221 | self.last_rpy_e = rpy_e 222 | self.integral_rpy_e = self.integral_rpy_e + rpy_e*control_timestep 223 | #### PID target torques #################################### 224 | target_torques = np.multiply(self.P_COEFF_TOR, rpy_e) \ 225 | + np.multiply(self.I_COEFF_TOR, self.integral_rpy_e) \ 226 | + np.multiply(self.D_COEFF_TOR, d_rpy_e) 227 | return nnlsRPM(thrust=thrust, 228 | x_torque=target_torques[0], 229 | y_torque=target_torques[1], 230 | z_torque=target_torques[2], 231 | counter=self.control_counter, 232 | max_thrust=self.MAX_THRUST, 233 | max_xy_torque=self.MAX_XY_TORQUE, 234 | max_z_torque=self.MAX_Z_TORQUE, 235 | a=self.A, 236 | inv_a=self.INV_A, 237 | b_coeff=self.B_COEFF, 238 | gui=True 239 | ) 240 | -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/__init__.py -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/__pycache__/BaseControl.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/__pycache__/BaseControl.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/__pycache__/BaseControl.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/__pycache__/BaseControl.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/__pycache__/DSLPIDControl.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/__pycache__/DSLPIDControl.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/__pycache__/DSLPIDControl.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/__pycache__/DSLPIDControl.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/__pycache__/SimplePIDControl.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/__pycache__/SimplePIDControl.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/__pycache__/SimplePIDControl.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/__pycache__/SimplePIDControl.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/__pycache__/__init__.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/__pycache__/__init__.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/__pycache__/__init__.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/control/__pycache__/__init__.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/ControlAviary.py: -------------------------------------------------------------------------------- 1 | import os 2 | import random 3 | from tkinter import PhotoImage 4 | import numpy as np 5 | from gym import spaces 6 | from enum import Enum 7 | import math 8 | import pybullet as p 9 | from random import choice 10 | from gym_pybullet_drones.utils.adsbcenter import adsbcenter as adsbcenter 11 | from gym_pybullet_drones.envs.BaseAviary import BaseAviary 12 | from gym_pybullet_drones.utils.enums import DroneModel, Physics, ImageType 13 | from gym_pybullet_drones.utils.utils import nnlsRPM 14 | from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl 15 | from gym_pybullet_drones.control.SimplePIDControl import SimplePIDControl 16 | from gym_pybullet_drones.envs.single_agent_rl.BaseSingleAgentAviary import ActionType, ObservationType 17 | 18 | 19 | class ControlAviary(BaseAviary): 20 | """Multi-drone environment class for control applications.""" 21 | 22 | ################################################################################ 23 | 24 | 25 | def __init__(self, 26 | drone_model: DroneModel=DroneModel.CF2X, 27 | num_drones: int=30, 28 | neighbourhood_radius: float=np.inf, 29 | initial_xyzs=None, 30 | initial_rpys=None, 31 | physics: Physics=Physics.PYB, 32 | freq: int=240, 33 | aggregate_phy_steps: int=5, 34 | gui=False, 35 | record=False, 36 | obstacles=False, 37 | user_debug_gui=True, 38 | output_folder='results', 39 | obs: ObservationType=ObservationType.KIN, 40 | act: ActionType=ActionType.TUN 41 | ): 42 | self.PERIOD = 30 43 | vision_attributes = True if obs == ObservationType.RGB else False 44 | dynamics_attributes = True if act in [ActionType.DYN, ActionType.ONE_D_DYN] else False 45 | self.typeA = num_drones 46 | self.typeB = num_drones-1 47 | self.OBS_TYPE = obs 48 | self.ACT_TYPE = act 49 | self.ctrl_b = [DSLPIDControl(drone_model=DroneModel.CF2X) for i in range(self.typeB)] 50 | self.reward_c=0 51 | self.reward_collision=0 52 | self.reward_p=0 53 | self.reward_t=0 54 | self.Done=False 55 | self.INIT_XYZSforA=[0,0,0.5] 56 | self.INIT_RPYSforA=[0.0,0.0,0.0] 57 | self.INIT_XYZSforB = np.zeros((self.typeB,3)) 58 | self.INIT_RPYSforB = np.zeros((self.typeB,3)) 59 | self.epsilon=0 60 | self.NUM_WP = int((freq * self.PERIOD) /aggregate_phy_steps) 61 | self.ADSB_info = np.zeros((num_drones,6)) 62 | self.ADSB_unchange=np.zeros((num_drones,6)) 63 | self.ADSB_noise=np.zeros((num_drones,6)) 64 | self.TARGET_POS_B = np.zeros((self.typeB,self.NUM_WP,3)) 65 | self.cloest_d_collision=[] 66 | self.mean_d_collision=[] 67 | #### Create integrated controllers ######################### 68 | if act in [ActionType.PID, ActionType.VEL, ActionType.TUN, ActionType.ONE_D_PID]: 69 | os.environ['KMP_DUPLICATE_LIB_OK']='True' 70 | if drone_model in [DroneModel.CF2X, DroneModel.CF2P]: 71 | self.ctrl = DSLPIDControl(drone_model=DroneModel.CF2X) 72 | if act == ActionType.TUN: 73 | self.TUNED_P_POS = np.array([.4, .4, 1.25]) 74 | self.TUNED_I_POS = np.array([.05, .05, .05]) 75 | self.TUNED_D_POS = np.array([.2, .2, .5]) 76 | self.TUNED_P_ATT = np.array([70000., 70000., 60000.]) 77 | self.TUNED_I_ATT = np.array([.0, .0, 500.]) 78 | self.TUNED_D_ATT = np.array([20000., 20000., 12000.]) 79 | else: 80 | print("[ERROR] in BaseSingleAgentAviary.__init()__, no controller is available for the specified drone_model") 81 | """Initialization of an aviary environment for control applications. 82 | 83 | Parameters 84 | ---------- 85 | drone_model : DroneModel, optional 86 | The desired drone type (detailed in an .urdf file in folder `assets`). 87 | num_drones : int, optional 88 | The desired number of drones in the aviary. 89 | neighbourhood_radius : float, optional 90 | Radius used to compute the drones' adjacency matrix, in meters. 91 | initial_xyzs: ndarray | None, optional 92 | (NUM_DRONES, 3)-shaped array containing the initial XYZ position of the drones. 93 | initial_rpys: ndarray | None, optional 94 | (NUM_DRONES, 3)-shaped array containing the initial orientations of the drones (in radians). 95 | physics : Physics, optional 96 | The desired implementation of PyBullet physics/custom dynamics. 97 | freq : int, optional 98 | The frequency (Hz) at which the physics engine steps. 99 | aggregate_phy_steps : int, optional 100 | The number of physics steps within one call to `BaseAviary.step()`. 101 | gui : bool, optional 102 | Whether to use PyBullet's GUI. 103 | record : bool, optional 104 | Whether to save a video of the simulation in folder `files/videos/`. 105 | obstacles : bool, optional 106 | Whether to add obstacles to the simulation. 107 | user_debug_gui : bool, optional 108 | Whether to draw the drones' axes and the GUI RPMs sliders. 109 | 110 | """ 111 | 112 | self.TRAJ_STEPS = int((freq* self.PERIOD) / aggregate_phy_steps) 113 | self.CTRL_TIMESTEP = 1./freq*aggregate_phy_steps 114 | self.TARGET_POSITION = np.array([[50*i/self.TRAJ_STEPS+self.INIT_XYZSforA[0],10*np.sin(5*i/self.TRAJ_STEPS)+self.INIT_XYZSforA[1],30*i/self.TRAJ_STEPS+self.INIT_XYZSforA[2]] for i in range(self.TRAJ_STEPS)]) 115 | #### Derive the trajectory to obtain target velocity ####### 116 | self.TARGET_VELOCITY = np.zeros([self.TRAJ_STEPS, 3]) 117 | self.TARGET_VELOCITY[1:, :] = (self.TARGET_POSITION[1:, :] - self.TARGET_POSITION[0:-1, :]) / self.CTRL_TIMESTEP 118 | 119 | self._initialpositionforB() 120 | self._pathforBtype() 121 | 122 | initial_xyzs=np.vstack((self.INIT_XYZSforB,self.INIT_XYZSforA)) 123 | initial_rpys=np.vstack((self.INIT_RPYSforB,self.INIT_RPYSforA)) 124 | self.EPISODE_LEN_SEC = 30 125 | self.adsbserver = adsbcenter(num_drones,self.TRAJ_STEPS) 126 | 127 | super().__init__(drone_model=drone_model, 128 | num_drones=num_drones, 129 | neighbourhood_radius=neighbourhood_radius, 130 | initial_xyzs=initial_xyzs, 131 | initial_rpys=initial_rpys, 132 | physics=physics, 133 | freq=freq, 134 | aggregate_phy_steps=aggregate_phy_steps, 135 | gui=gui, 136 | record=record, 137 | obstacles=obstacles, 138 | user_debug_gui=user_debug_gui, 139 | output_folder=output_folder, 140 | dynamics_attributes=dynamics_attributes 141 | ) 142 | 143 | ################################################################################ 144 | def _initialpositionforB(self): 145 | """. 146 | Returns 147 | ------- 148 | ndarray[NUM_DRONES,3],ndarray[NUM_DRONES,3] 149 | A array of Box(3,) with NUM_DRONES entries, 150 | """ 151 | for j in range (self.typeB): 152 | #k_1 = random.uniform(10, -10) 153 | k_1=random.uniform(24.0+0,50.0+0 ) 154 | k_2=random.uniform(-24.0+0,24.0+0) 155 | k_3=random.uniform(3.5,5) 156 | 157 | self.INIT_XYZSforB[j,:] = k_1, k_2, k_3 158 | self.INIT_RPYSforB[j,:] = 0.0, 0.0, 0.0 159 | 160 | 161 | def _pathforBtype(self): 162 | 163 | NUM_WP = self.NUM_WP 164 | for j in range (self.typeB): 165 | aindex=random.randint(0.0,6.0) 166 | list=[1,-1] 167 | k_0=choice(list) 168 | k_1=random.uniform(120,150) 169 | k_2=random.uniform(5.0,6.0) 170 | k_3=random.uniform(80.0,100.0) 171 | k_4=random.uniform(0.0,45.05) 172 | k_5=random.uniform(20.0,25.0) 173 | R = random.uniform(0.0,1.0) 174 | for i in range(NUM_WP): 175 | if aindex==0: 176 | waypoints=np.array([self.INIT_XYZSforB[j, 0]+k_0*k_1*i/NUM_WP, self.INIT_XYZSforB[j, 1]+k_0*k_1*i/NUM_WP,self.INIT_XYZSforB[j, 2]+k_4*i/NUM_WP]) #Straight line 177 | elif aindex==1: 178 | waypoints=np.array([k_0*k_2*np.cos((i/NUM_WP)*6+np.pi/2)+self.INIT_XYZSforB[j, 0], k_2*k_0*np.sin((i/NUM_WP)*6)-R+self.INIT_XYZSforB[j, 1], self.INIT_XYZSforB[j, 2]+k_4*i/NUM_WP]) #Spirals 179 | elif aindex==2: 180 | waypoints=np.array([k_3*k_0*i/NUM_WP+self.INIT_XYZSforB[j, 0], k_0*k_5*math.log(10*i/NUM_WP+1)+self.INIT_XYZSforB[j, 1], self.INIT_XYZSforB[j, 2]+k_4*i/NUM_WP]) #Logarithmic curves 181 | elif aindex==3: 182 | waypoints=np.array([k_3*k_0*i/NUM_WP+self.INIT_XYZSforB[j, 0], k_2*k_0*np.sin(i/NUM_WP*6)+self.INIT_XYZSforB[j, 1], self.INIT_XYZSforB[j, 2]+(i/NUM_WP)*k_4]) #Sine 183 | elif aindex==4: 184 | waypoints=np.array([k_3*k_0*i/NUM_WP+self.INIT_XYZSforB[j, 0], k_2*k_0*np.cos(i/NUM_WP*6+0.5*np.pi)+self.INIT_XYZSforB[j, 1], self.INIT_XYZSforB[j, 2]+(i/NUM_WP)*k_4]) #Cosine 185 | else: 186 | waypoints=np.array([k_3*k_0*i/NUM_WP+self.INIT_XYZSforB[j, 0], k_3*k_0*(i/NUM_WP)*(i/NUM_WP)+self.INIT_XYZSforB[j, 1], self.INIT_XYZSforB[j, 2]+(i/NUM_WP)*k_4]) #Parabola 187 | self.TARGET_POS_B[j, i, :]=waypoints 188 | 189 | def _actionSpace(self): 190 | """Returns the action space of the environment. 191 | 192 | Returns 193 | ------- 194 | dict[str, ndarray] 195 | A Dict of Box(4,) with NUM_DRONES entries, 196 | indexed by drone Id in string format. 197 | 198 | """ 199 | self.ACT_TYPE == ActionType.TUN 200 | act_lower_bound = np.array([-0.2,-0.2,-0.2,-0.2,-0.2,-0.2]) 201 | act_upper_bound = np.array([0.2, 0.2, 0.2, 0.2,0.2, 0.2]) 202 | return spaces.Box(low=act_lower_bound, 203 | high=act_upper_bound, 204 | dtype=np.float32 205 | ) 206 | ################################################################################ 207 | 208 | def _observationSpace(self): 209 | """Returns the observation space of the environment. 210 | 211 | Returns 212 | ------- 213 | dict[str, dict[str, ndarray]] 214 | A Dict with NUM_DRONES entries indexed by Id in string format, 215 | each a Dict in the form {Box(20,), MultiBinary(NUM_DRONES)}. 216 | 217 | """ 218 | #x,y,z,r,p,y,u,v,w,dh,dv,xyr,zR,rpR,TRR 219 | 220 | return spaces.Box(low=np.array([-1,-1,0, -1,-1,-1, -1,-1,0, 0,-1,0,-1,0,-1]), 221 | high=np.array([1,1,1, 1,1,1, 1,1,1, 1,1,1,1,1,1]), 222 | dtype=np.float32 223 | ) 224 | 225 | ################################################################################ 226 | 227 | def _computeObs(self): 228 | """Returns the current observation of the environment. 229 | 230 | For the value of key "state", see the implementation of `_getDroneStateVector()`, 231 | the value of key "neighbors" is the drone's own row of the adjacency matrix. 232 | 233 | Returns 234 | ------- 235 | dict[str, dict[str, ndarray]] 236 | A Dict with NUM_DRONES entries indexed by Id in string format, 237 | each a Dict in the form {Box(20,), MultiBinary(NUM_DRONES)}. 238 | 239 | """ 240 | 241 | return self._clipAndNormalizeState(self._ProcessADSB()) 242 | ############################################################ 243 | #### OBS OF SIZE 20 (WITH QUATERNION AND RPMS) 244 | # return obs 245 | ############################################################ 246 | #### OBS SPACE OF SIZE 12 247 | #ret = np.hstack(obs[0:14]).reshape(12,) 248 | #ret = np.hstack(obs[0:14]).reshape(14,) 249 | 250 | #return ret.astype('float32') 251 | # adjacency_mat = self._getAdjacencyMatrix() 252 | # return {str(i): {"state": self._getDroneStateVector(i), "neighbors": adjacency_mat[i, :]} for i in range(self.NUM_DRONES)} 253 | 254 | ################################################################################ 255 | 256 | 257 | def _calculatestate(self): 258 | 259 | #output: state for type B UAVs 260 | 261 | Phi = np.zeros(self.NUM_DRONES) 262 | GS = np.zeros(self.NUM_DRONES) 263 | theta=np.zeros(self.NUM_DRONES) 264 | Ve= np.zeros(self.NUM_DRONES) 265 | for j in range(self.NUM_DRONES): 266 | if self.vel[j,0]==0: 267 | Phi[j]==0 268 | GS[j] = math.sqrt(math.pow(self.vel[j,0],2)+math.pow(self.vel[j,1],2)) 269 | Ve[j] = math.sqrt(math.pow(self.vel[j,1],2)+math.pow(self.vel[j,2],2)) 270 | elif self.vel[j,1]==0: 271 | Phi[j]==np.pi*0.5 272 | GS[j] = math.sqrt(math.pow(self.vel[j,0],2)+math.pow(self.vel[j,1],2)) 273 | Ve[j] = math.sqrt(math.pow(self.vel[j,1],2)+math.pow(self.vel[j,2],2)) 274 | else: 275 | Phi[j]= math.atan(self.vel[j,0]/self.vel[j,1]) 276 | GS[j] = math.sqrt(math.pow(self.vel[j,0],2)+math.pow(self.vel[j,1],2)) 277 | Ve[j] = math.sqrt(math.pow(self.vel[j,1],2)+math.pow(self.vel[j,2],2)) 278 | for j in range(self.NUM_DRONES): 279 | if self.vel[j,2]==0: 280 | theta[j]==0 281 | if self.vel[j,1]==0: 282 | theta[j]==math.pi 283 | else: 284 | theta[j]= math.atan(self.vel[j,2]/self.vel[j,1]) 285 | 286 | return GS,Phi,theta,Ve 287 | 288 | def _collisiondetection(self): 289 | 290 | #ID start 1 291 | #Detection collision 292 | P_min, P_max = p.getAABB((self.typeA)) 293 | id_tuple = p.getOverlappingObjects(P_min, P_max) 294 | if id_tuple==None: 295 | self.reward_collision=-1 296 | self.Done=True 297 | return P_max,self.reward_collision 298 | if len(id_tuple) > 1: 299 | for ID, _ in id_tuple: 300 | if ID == (self.typeA): 301 | continue 302 | else: 303 | print(f"UAV hits the obstacle {p.getBodyInfo(ID)}") 304 | self.reward_collision=-1 305 | self.Done=True 306 | break 307 | return P_max, self.reward_collision 308 | 309 | 310 | def _collisionforA(self,GS,Phi,theta,Ve): 311 | #def the detection/punishment area 312 | P_max, reward_collision=self._collisiondetection() 313 | r=24.0 #maximum horizontal speed for target UAV*time 314 | h=3.5 #maximum vertical speed for target UAV*time 315 | min_collision=1E15 316 | d_collisions=[] 317 | j_min=-1 318 | reward_c=-1 319 | ADSB_info = np.zeros((self.NUM_DRONES,6)) 320 | ADSB_noise=[] 321 | frequency=48 #Set the frequency of ADS-B signal e.g. 2,4,6,8,12,24,48 322 | time_step=48/frequency 323 | for j in range(self.NUM_DRONES): 324 | ADSB_info[j,0:3]=self.pos[j] 325 | ADSB_info[j,3]=Phi[j] 326 | ADSB_info[j,4]=GS[j] 327 | ADSB_info[j,5]=self.vel[j,2] 328 | step=int(self.step_counter / self.AGGR_PHY_STEPS) 329 | ADSB_noise=self.adsbserver.addnoise(ADSB_info) #add error for ADS-B signal 330 | 331 | if (step==0): 332 | self.adsbserver.SendToadsb(ADSB_noise,step) #Encode the ADS-B information 333 | ADSB_info=self.adsbserver.ReceiveFromadsb(step) #Decode the ADS-B signal 334 | ADSB_info=self.adsbserver.filterdata(ADSB_info,step) #send to Kalman filter 335 | self.ADSB_unchange=ADSB_info #save the information for this step 336 | elif (step % time_step==0): 337 | self.adsbserver.SendToadsb(ADSB_noise,step) 338 | ADSB_info=self.adsbserver.ReceiveFromadsb(step) 339 | ADSB_info=self.adsbserver.filterdata(ADSB_info,step) 340 | self.ADSB_unchange=ADSB_info 341 | else: 342 | ADSB_info=self.ADSB_unchange 343 | 344 | for j in range(self.typeB): 345 | #horizontal velocity and vertical velocity 346 | v_rh=math.sqrt(math.pow(ADSB_info[j,4],2)+math.pow(ADSB_info[self.typeA-1,4],2)-2*abs(ADSB_info[j,4])*abs(ADSB_info[self.typeA-1,4])*math.cos(ADSB_info[j,3]-ADSB_info[self.typeA-1,3])) 347 | v_ve=math.sqrt(math.pow(ADSB_info[j,5],2)+math.pow(ADSB_info[self.typeA-1,5],2)-2*abs(ADSB_info[j,5])*abs(ADSB_info[self.typeA-1,5])*math.cos(theta[j]-theta[(self.typeA-1)])) 348 | d_h=np.linalg.norm(ADSB_info[j,:3]-ADSB_info[self.typeA-1,:3],ord=2) 349 | V_rh=max(0.00001,v_rh) 350 | V_ve=max(0.00001,v_ve) 351 | d_v=np.linalg.norm([ADSB_info[j,1]-ADSB_info[self.typeA-1,1],ADSB_info[j,2]-ADSB_info[self.typeA-1,2]]) 352 | t_collision_h=d_h/V_rh 353 | t_collision_v=d_v/V_ve 354 | #determine the collision time 355 | t_collision=min(t_collision_h,t_collision_v) 356 | cloest_distance=min(d_h,d_v) 357 | d_collisions.append(cloest_distance) 358 | if min_collision>t_collision: 359 | min_collision=t_collision 360 | j_min=j #feedback the cloest UAV 361 | if (abs(ADSB_info[j,0]-ADSB_info[self.typeA-1,0])**2+abs(ADSB_info[j,1]-ADSB_info[self.typeA-1,1])**2)<=r**2 or \ 362 | (ADSB_info[self.typeA-1,2]+h)>ADSB_info[j,2] or \ 363 | (ADSB_info[self.typeA-1,2]-h) self.EPISODE_LEN_SEC: 515 | self._pathforBtype() 516 | self.Done=False 517 | return True 518 | else: 519 | return False 520 | 521 | ################################################################################ 522 | 523 | def _computeInfo(self): 524 | """Computes the current info dict(s). 525 | 526 | Unused as this subclass is not meant for reinforcement learning. 527 | 528 | Returns 529 | ------- 530 | dict[str, int] 531 | Dummy value. 532 | 533 | """ 534 | return {"answer": 42} 535 | 536 | # def _addObstacles(self): 537 | # super()._addObstacles() 538 | # p.loadURDF("cube_small.urdf", 539 | # [13.9, 9.8, 8.8], 540 | # p.getQuaternionFromEuler([0, 0, 0]), 541 | # physicsClientId=self.CLIENT) 542 | 543 | def _clipAndNormalizeState(self, 544 | state 545 | ): 546 | MAX_LIN_VEL_XY=10.0 547 | MAX_LIN_VEL_Z=2.0 548 | 549 | MAX_XY=MAX_LIN_VEL_XY*self.PERIOD 550 | MAX_Z=MAX_LIN_VEL_Z*self.PERIOD 551 | MAX_PITCH_ROLL = np.pi # Full range 552 | 553 | MAX_RELAV_H=14.0 554 | MAX_RELAV_V=2.0 555 | MAX_RELAD_H=MAX_RELAV_H*self.PERIOD 556 | MAX_RELAD_V=MAX_RELAV_V*self.PERIOD 557 | MAX_PITCH_ROLL_R = np.pi # Full range 558 | 559 | clipped_pos_xy = np.clip(state[0:2], -MAX_XY, MAX_XY) 560 | clipped_pos_z = np.clip(state[2], 0, MAX_Z) 561 | clipped_rp = np.clip(state[3:5], -MAX_PITCH_ROLL, MAX_PITCH_ROLL) 562 | clipped_vel_xy = np.clip(state[6:8], -MAX_LIN_VEL_XY, MAX_LIN_VEL_XY) 563 | clipped_vel_z = np.clip(state[8], 0, MAX_LIN_VEL_Z) 564 | clipped_pos_relaD_H=np.clip(state[9], 0, MAX_RELAD_H) 565 | clipped_pos_relaD_V=np.clip(state[10], -MAX_RELAD_V, MAX_RELAD_V) 566 | clipped_vel_xyR=np.clip(state[11], 0, MAX_RELAV_H) 567 | clipped_vel_zR=np.clip(state[12], -MAX_RELAV_V, MAX_RELAV_V) 568 | clipped_rpR=np.clip(state[13],0, MAX_PITCH_ROLL_R) 569 | 570 | normalized_pos_xy = clipped_pos_xy / MAX_XY 571 | normalized_pos_z = clipped_pos_z / MAX_Z 572 | normalized_rp = clipped_rp / MAX_PITCH_ROLL 573 | normalized_y = state[5] / np.pi # No reason to clip 574 | normalized_vel_xy = clipped_vel_xy / MAX_LIN_VEL_XY 575 | normalized_vel_z = clipped_vel_z / MAX_LIN_VEL_XY 576 | normalized_pos_relaD_H=clipped_pos_relaD_H/MAX_RELAD_H 577 | normalized_pos_relaD_V=clipped_pos_relaD_V/MAX_RELAD_V 578 | normalized_vel_xyR=clipped_vel_xyR/MAX_RELAV_H 579 | normalized_vel_zR=clipped_vel_zR/MAX_RELAV_V 580 | normalized_rpR=clipped_rpR/MAX_PITCH_ROLL_R 581 | normalized_TRR=state[14]/np.pi 582 | norm_and_clipped = np.hstack([normalized_pos_xy, 583 | normalized_pos_z, 584 | normalized_rp, 585 | normalized_y, 586 | normalized_vel_xy, 587 | normalized_vel_z, 588 | normalized_pos_relaD_H, 589 | normalized_pos_relaD_V, 590 | normalized_vel_xyR, 591 | normalized_vel_zR, 592 | normalized_rpR, 593 | normalized_TRR 594 | ]).reshape(15) 595 | 596 | return norm_and_clipped 597 | 598 | def _clipAndNormalizeStateWarning(self, 599 | state, 600 | clipped_pos_xy, 601 | clipped_pos_z, 602 | clipped_rp, 603 | clipped_vel_xy, 604 | clipped_vel_z, 605 | clipped_pos_relaD_H, 606 | clipped_pos_relaD_V, 607 | clipped_vel_xyR, 608 | clipped_vel_zR, 609 | clipped_rpR, 610 | ): 611 | """Debugging printouts associated to `_clipAndNormalizeState`. 612 | 613 | Print a warning if values in a state vector is out of the clipping range. 614 | 615 | """ 616 | if not(clipped_pos_xy == np.array(state[0:2])).all(): 617 | print("[WARNING] it", self.step_counter, "in ControlAviary._clipAndNormalizeState(), clipped xy position [{:.2f} {:.2f}]".format(state[0], state[1])) 618 | if not(clipped_pos_z == np.array(state[2])).all(): 619 | print("[WARNING] it", self.step_counter, "in ControlAviary._clipAndNormalizeState(), clipped z position [{:.2f}]".format(state[2])) 620 | if not(clipped_rp == np.array(state[3:5])).all(): 621 | print("[WARNING] it", self.step_counter, "in ControlAviary._clipAndNormalizeState(), clipped roll/pitch [{:.2f} {:.2f}]".format(state[3], state[5])) 622 | if not(clipped_vel_xy == np.array(state[6:8])).all(): 623 | print("[WARNING] it", self.step_counter, "in ControlAviary._clipAndNormalizeState(), clipped xy velocity [{:.2f} {:.2f}]".format(state[6], state[7])) 624 | if not(clipped_vel_z == np.array(state[8])).all(): 625 | print("[WARNING] it", self.step_counter, "in ControlAviary._clipAndNormalizeState(), clipped z velocity [{:.2f}]".format(state[8])) 626 | if not(clipped_pos_relaD_H == np.array(state[9])).all(): 627 | print("[WARNING] it", self.step_counter, "in ControlAviary._clipAndNormalizeState(), clipped z velocity [{:.2f}]".format(state[9])) 628 | if not(clipped_pos_relaD_V == np.array(state[10])).all(): 629 | print("[WARNING] it", self.step_counter, "in ControlAviary._clipAndNormalizeState(), clipped z velocity [{:.2f}]".format(state[10])) 630 | if not(clipped_vel_xyR == np.array(state[11])).all(): 631 | print("[WARNING] it", self.step_counter, "in ControlAviary._clipAndNormalizeState(), clipped z velocity [{:.2f}]".format(state[11])) 632 | if not(clipped_vel_zR == np.array(state[12])).all(): 633 | print("[WARNING] it", self.step_counter, "in ControlAviary._clipAndNormalizeState(), clipped z velocity [{:.2f}]".format(state[12])) 634 | if not(clipped_rpR == np.array(state[13])).all(): 635 | print("[WARNING] it", self.step_counter, "in ControlAviary._clipAndNormalizeState(), clipped z velocity [{:.2f}]".format(state[13])) 636 | 637 | 638 | 639 | 640 | 641 | 642 | 643 | 644 | 645 | 646 | 647 | 648 | -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__init__.py: -------------------------------------------------------------------------------- 1 | from gym_pybullet_drones.envs.ControlAviary import ControlAviary 2 | -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/BaseAviary.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/BaseAviary.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/BaseAviary.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/BaseAviary.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/ControlAviary.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/ControlAviary.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/ControlAviary.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/ControlAviary.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/ControlAviary_1.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/ControlAviary_1.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/ControlAviary_1_1.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/ControlAviary_1_1.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/CtrlAviary.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/CtrlAviary.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/CtrlAviary.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/CtrlAviary.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/DynAviary.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/DynAviary.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/DynAviary.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/DynAviary.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/VelocityAviary.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/VelocityAviary.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/VelocityAviary.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/VelocityAviary.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/VisionAviary.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/VisionAviary.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/VisionAviary.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/VisionAviary.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/__init__.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/__init__.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/__init__.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/__pycache__/__init__.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/BaseSingleAgentAviary.py: -------------------------------------------------------------------------------- 1 | import os 2 | from enum import Enum 3 | import numpy as np 4 | from gym import spaces 5 | import pybullet as p 6 | 7 | from gym_pybullet_drones.envs.BaseAviary import BaseAviary 8 | from gym_pybullet_drones.utils.enums import DroneModel, Physics, ImageType 9 | from gym_pybullet_drones.utils.utils import nnlsRPM 10 | from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl 11 | from gym_pybullet_drones.control.SimplePIDControl import SimplePIDControl 12 | 13 | class ActionType(Enum): 14 | """Action type enumeration class.""" 15 | RPM = "rpm" # RPMS 16 | DYN = "dyn" # Desired thrust and torques 17 | PID = "pid" # PID control 18 | VEL = "vel" # Velocity input (using PID control) 19 | TUN = "tun" # Tune the coefficients of a PID controller 20 | ONE_D_RPM = "one_d_rpm" # 1D (identical input to all motors) with RPMs 21 | ONE_D_DYN = "one_d_dyn" # 1D (identical input to all motors) with desired thrust and torques 22 | ONE_D_PID = "one_d_pid" # 1D (identical input to all motors) with PID control 23 | 24 | ################################################################################ 25 | 26 | class ObservationType(Enum): 27 | """Observation type enumeration class.""" 28 | KIN = "kin" # Kinematic information (pose, linear and angular velocities) 29 | RGB = "rgb" # RGB camera capture in each drone's POV 30 | 31 | ################################################################################ 32 | 33 | class BaseSingleAgentAviary(BaseAviary): 34 | """Base single drone environment class for reinforcement learning.""" 35 | 36 | ################################################################################ 37 | 38 | def __init__(self, 39 | drone_model: DroneModel=DroneModel.CF2X, 40 | initial_xyzs=None, 41 | initial_rpys=None, 42 | physics: Physics=Physics.PYB, 43 | freq: int=240, 44 | aggregate_phy_steps: int=1, 45 | gui=False, 46 | record=False, 47 | obs: ObservationType=ObservationType.KIN, 48 | act: ActionType=ActionType.RPM 49 | ): 50 | """Initialization of a generic single agent RL environment. 51 | 52 | Attribute `num_drones` is automatically set to 1; `vision_attributes` 53 | and `dynamics_attributes` are selected based on the choice of `obs` 54 | and `act`; `obstacles` is set to True and overridden with landmarks for 55 | vision applications; `user_debug_gui` is set to False for performance. 56 | 57 | Parameters 58 | ---------- 59 | drone_model : DroneModel, optional 60 | The desired drone type (detailed in an .urdf file in folder `assets`). 61 | initial_xyzs: ndarray | None, optional 62 | (NUM_DRONES, 3)-shaped array containing the initial XYZ position of the drones. 63 | initial_rpys: ndarray | None, optional 64 | (NUM_DRONES, 3)-shaped array containing the initial orientations of the drones (in radians). 65 | physics : Physics, optional 66 | The desired implementation of PyBullet physics/custom dynamics. 67 | freq : int, optional 68 | The frequency (Hz) at which the physics engine steps. 69 | aggregate_phy_steps : int, optional 70 | The number of physics steps within one call to `BaseAviary.step()`. 71 | gui : bool, optional 72 | Whether to use PyBullet's GUI. 73 | record : bool, optional 74 | Whether to save a video of the simulation in folder `files/videos/`. 75 | obs : ObservationType, optional 76 | The type of observation space (kinematic information or vision) 77 | act : ActionType, optional 78 | The type of action space (1 or 3D; RPMS, thurst and torques, waypoint or velocity with PID control; etc.) 79 | 80 | """ 81 | vision_attributes = True if obs == ObservationType.RGB else False 82 | dynamics_attributes = True if act in [ActionType.DYN, ActionType.ONE_D_DYN] else False 83 | self.OBS_TYPE = obs 84 | self.ACT_TYPE = act 85 | self.EPISODE_LEN_SEC = 5 86 | #### Create integrated controllers ######################### 87 | if act in [ActionType.PID, ActionType.VEL, ActionType.TUN, ActionType.ONE_D_PID]: 88 | os.environ['KMP_DUPLICATE_LIB_OK']='True' 89 | if drone_model in [DroneModel.CF2X, DroneModel.CF2P]: 90 | self.ctrl = DSLPIDControl(drone_model=DroneModel.CF2X) 91 | if act == ActionType.TUN: 92 | self.TUNED_P_POS = np.array([.4, .4, 1.25]) 93 | self.TUNED_I_POS = np.array([.05, .05, .05]) 94 | self.TUNED_D_POS = np.array([.2, .2, .5]) 95 | self.TUNED_P_ATT = np.array([70000., 70000., 60000.]) 96 | self.TUNED_I_ATT = np.array([.0, .0, 500.]) 97 | self.TUNED_D_ATT = np.array([20000., 20000., 12000.]) 98 | elif drone_model == DroneModel.HB: 99 | self.ctrl = SimplePIDControl(drone_model=DroneModel.HB) 100 | if act == ActionType.TUN: 101 | self.TUNED_P_POS = np.array([.1, .1, .2]) 102 | self.TUNED_I_POS = np.array([.0001, .0001, .0001]) 103 | self.TUNED_D_POS = np.array([.3, .3, .4]) 104 | self.TUNED_P_ATT = np.array([.3, .3, .05]) 105 | self.TUNED_I_ATT = np.array([.0001, .0001, .0001]) 106 | self.TUNED_D_ATT = np.array([.3, .3, .5]) 107 | else: 108 | print("[ERROR] in BaseSingleAgentAviary.__init()__, no controller is available for the specified drone_model") 109 | super().__init__(drone_model=drone_model, 110 | num_drones=1, 111 | initial_xyzs=initial_xyzs, 112 | initial_rpys=initial_rpys, 113 | physics=physics, 114 | freq=freq, 115 | aggregate_phy_steps=aggregate_phy_steps, 116 | gui=gui, 117 | record=record, 118 | obstacles=True, # Add obstacles for RGB observations and/or FlyThruGate 119 | user_debug_gui=False, # Remove of RPM sliders from all single agent learning aviaries 120 | vision_attributes=vision_attributes, 121 | dynamics_attributes=dynamics_attributes 122 | ) 123 | #### Set a limit on the maximum target speed ############### 124 | if act == ActionType.VEL: 125 | self.SPEED_LIMIT = 0.03 * self.MAX_SPEED_KMH * (1000/3600) 126 | #### Try _trajectoryTrackingRPMs exists IFF ActionType.TUN # 127 | if act == ActionType.TUN and not (hasattr(self.__class__, '_trajectoryTrackingRPMs') and callable(getattr(self.__class__, '_trajectoryTrackingRPMs'))): 128 | print("[ERROR] in BaseSingleAgentAviary.__init__(), ActionType.TUN requires an implementation of _trajectoryTrackingRPMs in the instantiated subclass") 129 | exit() 130 | 131 | ################################################################################ 132 | 133 | def _addObstacles(self): 134 | """Add obstacles to the environment. 135 | 136 | Only if the observation is of type RGB, 4 landmarks are added. 137 | Overrides BaseAviary's method. 138 | 139 | """ 140 | if self.OBS_TYPE == ObservationType.RGB: 141 | p.loadURDF("block.urdf", 142 | [1, 0, .1], 143 | p.getQuaternionFromEuler([0, 0, 0]), 144 | physicsClientId=self.CLIENT 145 | ) 146 | p.loadURDF("cube_small.urdf", 147 | [0, 1, .1], 148 | p.getQuaternionFromEuler([0, 0, 0]), 149 | physicsClientId=self.CLIENT 150 | ) 151 | p.loadURDF("duck_vhacd.urdf", 152 | [-1, 0, .1], 153 | p.getQuaternionFromEuler([0, 0, 0]), 154 | physicsClientId=self.CLIENT 155 | ) 156 | p.loadURDF("teddy_vhacd.urdf", 157 | [0, -1, .1], 158 | p.getQuaternionFromEuler([0, 0, 0]), 159 | physicsClientId=self.CLIENT 160 | ) 161 | else: 162 | pass 163 | 164 | ################################################################################ 165 | 166 | def _actionSpace(self): 167 | """Returns the action space of the environment. 168 | 169 | Returns 170 | ------- 171 | ndarray 172 | A Box() of size 1, 3, 4, or 6 depending on the action type. 173 | 174 | """ 175 | if self.ACT_TYPE == ActionType.TUN: 176 | size = 6 177 | elif self.ACT_TYPE in [ActionType.RPM, ActionType.DYN, ActionType.VEL]: 178 | size = 4 179 | elif self.ACT_TYPE == ActionType.PID: 180 | size = 3 181 | elif self.ACT_TYPE in [ActionType.ONE_D_RPM, ActionType.ONE_D_DYN, ActionType.ONE_D_PID]: 182 | size = 1 183 | else: 184 | print("[ERROR] in BaseSingleAgentAviary._actionSpace()") 185 | exit() 186 | return spaces.Box(low=-1*np.ones(size), 187 | # return spaces.Box(low=np.zeros(size), # Alternative action space, see PR #32 188 | high=np.ones(size), 189 | dtype=np.float32 190 | ) 191 | 192 | ################################################################################ 193 | 194 | def _preprocessAction(self, 195 | action 196 | ): 197 | """Pre-processes the action passed to `.step()` into motors' RPMs. 198 | 199 | Parameter `action` is processed differenly for each of the different 200 | action types: `action` can be of length 1, 3, 4, or 6 and represent 201 | RPMs, desired thrust and torques, the next target position to reach 202 | using PID control, a desired velocity vector, new PID coefficients, etc. 203 | 204 | Parameters 205 | ---------- 206 | action : ndarray 207 | The input action for each drone, to be translated into RPMs. 208 | 209 | Returns 210 | ------- 211 | ndarray 212 | (4,)-shaped array of ints containing to clipped RPMs 213 | commanded to the 4 motors of each drone. 214 | 215 | """ 216 | if self.ACT_TYPE == ActionType.TUN: 217 | self.ctrl.setPIDCoefficients(p_coeff_pos=(action[0]+1)*self.TUNED_P_POS, 218 | i_coeff_pos=(action[1]+1)*self.TUNED_I_POS, 219 | d_coeff_pos=(action[2]+1)*self.TUNED_D_POS, 220 | p_coeff_att=(action[3]+1)*self.TUNED_P_ATT, 221 | i_coeff_att=(action[4]+1)*self.TUNED_I_ATT, 222 | d_coeff_att=(action[5]+1)*self.TUNED_D_ATT 223 | ) 224 | return self._trajectoryTrackingRPMs() 225 | elif self.ACT_TYPE == ActionType.RPM: 226 | return np.array(self.HOVER_RPM * (1+0.05*action)) 227 | elif self.ACT_TYPE == ActionType.DYN: 228 | return nnlsRPM(thrust=(self.GRAVITY*(action[0]+1)), 229 | x_torque=(0.05*self.MAX_XY_TORQUE*action[1]), 230 | y_torque=(0.05*self.MAX_XY_TORQUE*action[2]), 231 | z_torque=(0.05*self.MAX_Z_TORQUE*action[3]), 232 | counter=self.step_counter, 233 | max_thrust=self.MAX_THRUST, 234 | max_xy_torque=self.MAX_XY_TORQUE, 235 | max_z_torque=self.MAX_Z_TORQUE, 236 | a=self.A, 237 | inv_a=self.INV_A, 238 | b_coeff=self.B_COEFF, 239 | gui=self.GUI 240 | ) 241 | elif self.ACT_TYPE == ActionType.PID: 242 | state = self._getDroneStateVector(0) 243 | rpm, _, _ = self.ctrl.computeControl(control_timestep=self.AGGR_PHY_STEPS*self.TIMESTEP, 244 | cur_pos=state[0:3], 245 | cur_quat=state[3:7], 246 | cur_vel=state[10:13], 247 | cur_ang_vel=state[13:16], 248 | target_pos=state[0:3]+0.1*action 249 | ) 250 | return rpm 251 | elif self.ACT_TYPE == ActionType.VEL: 252 | state = self._getDroneStateVector(0) 253 | if np.linalg.norm(action[0:3]) != 0: 254 | v_unit_vector = action[0:3] / np.linalg.norm(action[0:3]) 255 | else: 256 | v_unit_vector = np.zeros(3) 257 | rpm, _, _ = self.ctrl.computeControl(control_timestep=self.AGGR_PHY_STEPS*self.TIMESTEP, 258 | cur_pos=state[0:3], 259 | cur_quat=state[3:7], 260 | cur_vel=state[10:13], 261 | cur_ang_vel=state[13:16], 262 | target_pos=state[0:3], # same as the current position 263 | target_rpy=np.array([0,0,state[9]]), # keep current yaw 264 | target_vel=self.SPEED_LIMIT * np.abs(action[3]) * v_unit_vector # target the desired velocity vector 265 | ) 266 | return rpm 267 | elif self.ACT_TYPE == ActionType.ONE_D_RPM: 268 | return np.repeat(self.HOVER_RPM * (1+0.05*action), 4) 269 | elif self.ACT_TYPE == ActionType.ONE_D_DYN: 270 | return nnlsRPM(thrust=(self.GRAVITY*(1+0.05*action[0])), 271 | x_torque=0, 272 | y_torque=0, 273 | z_torque=0, 274 | counter=self.step_counter, 275 | max_thrust=self.MAX_THRUST, 276 | max_xy_torque=self.MAX_XY_TORQUE, 277 | max_z_torque=self.MAX_Z_TORQUE, 278 | a=self.A, 279 | inv_a=self.INV_A, 280 | b_coeff=self.B_COEFF, 281 | gui=self.GUI 282 | ) 283 | elif self.ACT_TYPE == ActionType.ONE_D_PID: 284 | state = self._getDroneStateVector(0) 285 | rpm, _, _ = self.ctrl.computeControl(control_timestep=self.AGGR_PHY_STEPS*self.TIMESTEP, 286 | cur_pos=state[0:3], 287 | cur_quat=state[3:7], 288 | cur_vel=state[10:13], 289 | cur_ang_vel=state[13:16], 290 | target_pos=state[0:3]+0.1*np.array([0,0,action[0]]) 291 | ) 292 | return rpm 293 | else: 294 | print("[ERROR] in BaseSingleAgentAviary._preprocessAction()") 295 | 296 | ################################################################################ 297 | 298 | def _observationSpace(self): 299 | """Returns the observation space of the environment. 300 | 301 | Returns 302 | ------- 303 | ndarray 304 | A Box() of shape (H,W,4) or (12,) depending on the observation type. 305 | 306 | """ 307 | if self.OBS_TYPE == ObservationType.RGB: 308 | return spaces.Box(low=0, 309 | high=255, 310 | shape=(self.IMG_RES[1], self.IMG_RES[0], 4), 311 | dtype=np.uint8 312 | ) 313 | elif self.OBS_TYPE == ObservationType.KIN: 314 | ############################################################ 315 | #### OBS OF SIZE 20 (WITH QUATERNION AND RPMS) 316 | #### Observation vector ### X Y Z Q1 Q2 Q3 Q4 R P Y VX VY VZ WX WY WZ P0 P1 P2 P3 317 | # obs_lower_bound = np.array([-1, -1, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1]) 318 | # obs_upper_bound = np.array([1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]) 319 | # return spaces.Box( low=obs_lower_bound, high=obs_upper_bound, dtype=np.float32 ) 320 | ############################################################ 321 | #### OBS SPACE OF SIZE 12 322 | return spaces.Box(low=np.array([-1,-1,0, -1,-1,-1, -1,-1,-1, -1,-1,-1]), 323 | high=np.array([1,1,1, 1,1,1, 1,1,1, 1,1,1]), 324 | dtype=np.float32 325 | ) 326 | ############################################################ 327 | else: 328 | print("[ERROR] in BaseSingleAgentAviary._observationSpace()") 329 | 330 | ################################################################################ 331 | 332 | def _computeObs(self): 333 | """Returns the current observation of the environment. 334 | 335 | Returns 336 | ------- 337 | ndarray 338 | A Box() of shape (H,W,4) or (12,) depending on the observation type. 339 | 340 | """ 341 | if self.OBS_TYPE == ObservationType.RGB: 342 | if self.step_counter%self.IMG_CAPTURE_FREQ == 0: 343 | self.rgb[0], self.dep[0], self.seg[0] = self._getDroneImages(0, 344 | segmentation=False 345 | ) 346 | #### Printing observation to PNG frames example ############ 347 | if self.RECORD: 348 | self._exportImage(img_type=ImageType.RGB, 349 | img_input=self.rgb[0], 350 | path=self.ONBOARD_IMG_PATH, 351 | frame_num=int(self.step_counter/self.IMG_CAPTURE_FREQ) 352 | ) 353 | return self.rgb[0] 354 | elif self.OBS_TYPE == ObservationType.KIN: 355 | obs = self._clipAndNormalizeState(self._getDroneStateVector(0)) 356 | ############################################################ 357 | #### OBS OF SIZE 20 (WITH QUATERNION AND RPMS) 358 | # return obs 359 | ############################################################ 360 | #### OBS SPACE OF SIZE 12 361 | ret = np.hstack([obs[0:3], obs[7:10], obs[10:13], obs[13:16]]).reshape(12,) 362 | return ret.astype('float32') 363 | ############################################################ 364 | else: 365 | print("[ERROR] in BaseSingleAgentAviary._computeObs()") 366 | 367 | ################################################################################ 368 | 369 | def _clipAndNormalizeState(self, 370 | state 371 | ): 372 | """Normalizes a drone's state to the [-1,1] range. 373 | 374 | Must be implemented in a subclass. 375 | 376 | Parameters 377 | ---------- 378 | state : ndarray 379 | Array containing the non-normalized state of a single drone. 380 | 381 | """ 382 | raise NotImplementedError 383 | -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__init__.py: -------------------------------------------------------------------------------- 1 | from gym_pybullet_drones.envs.single_agent_rl.BaseSingleAgentAviary import BaseSingleAgentAviary 2 | -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/BaseSingleAgentAviary.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/BaseSingleAgentAviary.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/BaseSingleAgentAviary.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/BaseSingleAgentAviary.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/FlyThruGateAviary.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/FlyThruGateAviary.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/FlyThruGateAviary.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/FlyThruGateAviary.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/HoverAviary.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/HoverAviary.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/HoverAviary.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/HoverAviary.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/TakeoffAviary.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/TakeoffAviary.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/TakeoffAviary.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/TakeoffAviary.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/TuneAviary.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/TuneAviary.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/TuneAviary.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/TuneAviary.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/__init__.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/__init__.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/__init__.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/envs/single_agent_rl/__pycache__/__init__.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/examples/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/examples/__init__.py -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/examples/fly_test.py: -------------------------------------------------------------------------------- 1 | """Script demonstrating the joint use of simulation and control. 2 | 3 | The simulation is run by a `ControlAviary` ` environment. 4 | The control is given by the PID implementation in `DSLPIDControl`. 5 | 6 | 7 | """ 8 | from http.cookiejar import FileCookieJar 9 | import os 10 | from re import A 11 | import time 12 | import argparse 13 | from datetime import datetime 14 | import pdb 15 | import math 16 | import random 17 | import numpy as np 18 | import pybullet as p 19 | import matplotlib.pyplot as plt 20 | import sys 21 | sys.path.append("C:/software/gym-pybullet-drones") 22 | from gym_pybullet_drones.utils.enums import DroneModel, Physics 23 | from gym_pybullet_drones.envs.ControlAviary import ControlAviary 24 | from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl 25 | from gym_pybullet_drones.control.SimplePIDControl import SimplePIDControl 26 | from gym_pybullet_drones.utils.Logger import Logger 27 | from gym_pybullet_drones.utils.utils import sync, str2bool 28 | if __name__ == "__main__": 29 | 30 | gui = False 31 | output_folder='results' 32 | colab=False 33 | simulation_freq_hz=240 34 | duration_sec=30 35 | env = ControlAviary(freq=simulation_freq_hz, 36 | gui = False,obstacles=True) 37 | #### Obtain the PyBullet Client ID from the environment #### 38 | PYB_CLIENT = env.getPyBulletClient() 39 | simulation_freq_hz=240 40 | AGGR_PHY_STEPS=5 41 | num_drones=2 42 | #### Initialize the logger ################################# 43 | logger = Logger(logging_freq_hz=int(simulation_freq_hz/AGGR_PHY_STEPS), 44 | num_drones=num_drones, 45 | output_folder=output_folder, 46 | colab=colab 47 | ) 48 | 49 | control_freq_hz=48 50 | #### Run the simulation #################################### 51 | CTRL_EVERY_N_STEPS = int(np.floor(simulation_freq_hz/control_freq_hz)) 52 | START = time.time() 53 | totalReward=0 54 | curreward=0 55 | epsilons=[] 56 | rewardts=[] 57 | for epi in range(1): 58 | env.reset() 59 | print (curreward) 60 | curreward=0 61 | for i in range(0, int(duration_sec*env.SIM_FREQ), AGGR_PHY_STEPS): 62 | 63 | #### Make it rain rubber ducks ############################# 64 | # 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) 65 | action=env.action_space.sample() 66 | #### Step the simulation ################################### 67 | obs, reward, done, info = env.step(action) 68 | adsbinfo,noise,typeA,num_drones,pos_true,mean_d,close_d,epsilon,rewardt=env.receiveinfo() 69 | # print (obs,action) 70 | curreward+=reward 71 | totalReward+=reward 72 | epsilons.append(epsilon) 73 | rewardts.append(rewardt) 74 | 75 | 76 | logger.log(drone=0, 77 | timestamp=i/env.SIM_FREQ, 78 | state= np.hstack([obs[0:3], np.zeros(4), obs[3:15], np.resize(action, (1))]), 79 | control=np.zeros(12) 80 | ) 81 | if i%env.SIM_FREQ == 0: 82 | env.render() 83 | print (curreward) 84 | print (totalReward/10) 85 | ep=np.array(epsilons) 86 | re=np.array(rewardts) 87 | np.savetxt('reward_PID.txt', re, fmt="%.4f", delimiter=',') 88 | np.savetxt('output_PID.txt', ep, fmt="%.4f", delimiter=',') 89 | #### Close the environment ################################# 90 | env.close() 91 | 92 | #### Save the simulation results ########################### 93 | logger.save() 94 | # logger.save_as_csv("pid") # Optional CSV save 95 | 96 | #### Plot the simulation results ########################### 97 | # if plot: 98 | logger.plot() 99 | -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/Logger.py: -------------------------------------------------------------------------------- 1 | import os 2 | from datetime import datetime 3 | from cycler import cycler 4 | import numpy as np 5 | import matplotlib.pyplot as plt 6 | 7 | os.environ['KMP_DUPLICATE_LIB_OK']='True' 8 | 9 | class Logger(object): 10 | """A class for logging and visualization. 11 | 12 | Stores, saves to file, and plots the kinematic information and RPMs 13 | of a simulation with one or more drones. 14 | 15 | """ 16 | 17 | ################################################################################ 18 | 19 | def __init__(self, 20 | logging_freq_hz: int, 21 | output_folder: str="results", 22 | num_drones: int=1, 23 | duration_sec: int=0, 24 | colab: bool=False, 25 | ): 26 | """Logger class __init__ method. 27 | 28 | Note: the order in which information is stored by Logger.log() is not the same 29 | as the one in, e.g., the obs["id"]["state"], check the implementation below. 30 | 31 | Parameters 32 | ---------- 33 | logging_freq_hz : int 34 | Logging frequency in Hz. 35 | num_drones : int, optional 36 | Number of drones. 37 | duration_sec : int, optional 38 | Used to preallocate the log arrays (improves performance). 39 | 40 | """ 41 | self.COLAB = colab 42 | self.OUTPUT_FOLDER = output_folder 43 | if not os.path.exists(self.OUTPUT_FOLDER): 44 | os.mkdir(self.OUTPUT_FOLDER) 45 | self.LOGGING_FREQ_HZ = logging_freq_hz 46 | self.NUM_DRONES = num_drones 47 | self.PREALLOCATED_ARRAYS = False if duration_sec == 0 else True 48 | self.counters = np.zeros(num_drones) 49 | self.timestamps = np.zeros((num_drones, duration_sec*self.LOGGING_FREQ_HZ)) 50 | self.TRAJ_STEPS = 1440 51 | self.INIT_XYZSforA=[0.0,0.0,0.5] 52 | self.MAX_LIN_VEL_XY=5.0 53 | self.MAX_LIN_VEL_Z=2.0 54 | self.PERIOD=30 55 | 56 | self.MAX_XY=self.MAX_LIN_VEL_XY*self.PERIOD 57 | self.MAX_Z=self.MAX_LIN_VEL_Z*self.PERIOD 58 | self.TARGET_POSITION = np.array([[55*i/self.TRAJ_STEPS+self.INIT_XYZSforA[0],\ 59 | 10*np.sin(5*i/self.TRAJ_STEPS)+self.INIT_XYZSforA[1],\ 60 | 25.05*i/self.TRAJ_STEPS+self.INIT_XYZSforA[2]] for i in range(self.TRAJ_STEPS)]) 61 | #### Note: this is the suggest information to log ############################## 62 | self.states = np.zeros((num_drones, 16, duration_sec*self.LOGGING_FREQ_HZ)) #### 16 states: pos_x, 63 | # pos_y, 64 | # pos_z, 65 | # vel_x, 66 | # vel_y, 67 | # vel_z, 68 | # roll, 69 | # pitch, 70 | # yaw, 71 | # ang_vel_x, 72 | # ang_vel_y, 73 | # ang_vel_z, 74 | # rpm0, 75 | # rpm1, 76 | # rpm2, 77 | # rpm3 78 | #### Note: this is the suggest information to log ############################## 79 | self.controls = np.zeros((num_drones, 12, duration_sec*self.LOGGING_FREQ_HZ)) #### 12 control targets: pos_x, 80 | # pos_y, 81 | # pos_z, 82 | # vel_x, 83 | # vel_y, 84 | # vel_z, 85 | # roll, 86 | # pitch, 87 | # yaw, 88 | # ang_vel_x, 89 | # ang_vel_y, 90 | # ang_vel_z 91 | 92 | ################################################################################ 93 | 94 | def log(self, 95 | drone: int, 96 | timestamp, 97 | state, 98 | control=np.zeros(12) 99 | ): 100 | """Logs entries for a single simulation step, of a single drone. 101 | 102 | Parameters 103 | ---------- 104 | drone : int 105 | Id of the drone associated to the log entry. 106 | timestamp : float 107 | Timestamp of the log in simulation clock. 108 | state : ndarray 109 | (20,)-shaped array of floats containing the drone's state. 110 | control : ndarray, optional 111 | (12,)-shaped array of floats containing the drone's control target. 112 | 113 | """ 114 | if drone < 0 or drone >= self.NUM_DRONES or timestamp < 0 or len(state) != 20 or len(control) != 12: 115 | print("[ERROR] in Logger.log(), invalid data") 116 | current_counter = int(self.counters[drone]) 117 | #### Add rows to the matrices if a counter exceeds their size 118 | if current_counter >= self.timestamps.shape[1]: 119 | self.timestamps = np.concatenate((self.timestamps, np.zeros((self.NUM_DRONES, 1))), axis=1) 120 | self.states = np.concatenate((self.states, np.zeros((self.NUM_DRONES, 16, 1))), axis=2) 121 | self.controls = np.concatenate((self.controls, np.zeros((self.NUM_DRONES, 12, 1))), axis=2) 122 | #### Advance a counter is the matrices have overgrown it ### 123 | elif not self.PREALLOCATED_ARRAYS and self.timestamps.shape[1] > current_counter: 124 | current_counter = self.timestamps.shape[1]-1 125 | #### Log the information and increase the counter ########## 126 | self.timestamps[drone, current_counter] = timestamp 127 | #### Re-order the kinematic obs (of most Aviaries) ######### 128 | self.states[drone, :, current_counter] = np.hstack([state[0:3], state[10:13], state[7:10], state[13:20]]) 129 | self.controls[drone, :, current_counter] = control 130 | self.counters[drone] = current_counter + 1 131 | 132 | ################################################################################ 133 | 134 | def save(self): 135 | """Save the logs to file. 136 | """ 137 | with open(os.path.join(self.OUTPUT_FOLDER, "save-flight-"+datetime.now().strftime("%m.%d.%Y_%H.%M.%S")+".npy"), 'wb') as out_file: 138 | np.savez(out_file, timestamps=self.timestamps, states=self.states, controls=self.controls) 139 | 140 | ################################################################################ 141 | 142 | def save_as_csv(self, 143 | comment: str="" 144 | ): 145 | """Save the logs---on your Desktop---as comma separated values. 146 | 147 | Parameters 148 | ---------- 149 | comment : str, optional 150 | Added to the foldername. 151 | 152 | """ 153 | csv_dir = os.path.join(self.OUTPUT_FOLDER, "save-flight-"+comment+"-"+datetime.now().strftime("%m.%d.%Y_%H.%M.%S")) 154 | if not os.path.exists(csv_dir): 155 | os.makedirs(csv_dir+'/') 156 | t = np.arange(0, self.timestamps.shape[1]/self.LOGGING_FREQ_HZ, 1/self.LOGGING_FREQ_HZ) 157 | for i in range(self.NUM_DRONES): 158 | with open(csv_dir+"/x"+str(i)+".csv", 'wb') as out_file: 159 | np.savetxt(out_file, np.transpose(np.vstack([t, self.states[i, 0, :]])), delimiter=",") 160 | with open(csv_dir+"/y"+str(i)+".csv", 'wb') as out_file: 161 | np.savetxt(out_file, np.transpose(np.vstack([t, self.states[i, 1, :]])), delimiter=",") 162 | with open(csv_dir+"/z"+str(i)+".csv", 'wb') as out_file: 163 | np.savetxt(out_file, np.transpose(np.vstack([t, self.states[i, 2, :]])), delimiter=",") 164 | #### 165 | with open(csv_dir+"/r"+str(i)+".csv", 'wb') as out_file: 166 | np.savetxt(out_file, np.transpose(np.vstack([t, self.states[i, 6, :]])), delimiter=",") 167 | with open(csv_dir+"/p"+str(i)+".csv", 'wb') as out_file: 168 | np.savetxt(out_file, np.transpose(np.vstack([t, self.states[i, 7, :]])), delimiter=",") 169 | with open(csv_dir+"/ya"+str(i)+".csv", 'wb') as out_file: 170 | np.savetxt(out_file, np.transpose(np.vstack([t, self.states[i, 8, :]])), delimiter=",") 171 | #### 172 | with open(csv_dir+"/rr"+str(i)+".csv", 'wb') as out_file: 173 | rdot = np.hstack([0, (self.states[i, 6, 1:] - self.states[i, 6, 0:-1]) * self.LOGGING_FREQ_HZ ]) 174 | np.savetxt(out_file, np.transpose(np.vstack([t, rdot])), delimiter=",") 175 | with open(csv_dir+"/pr"+str(i)+".csv", 'wb') as out_file: 176 | pdot = np.hstack([0, (self.states[i, 7, 1:] - self.states[i, 7, 0:-1]) * self.LOGGING_FREQ_HZ ]) 177 | np.savetxt(out_file, np.transpose(np.vstack([t, pdot])), delimiter=",") 178 | with open(csv_dir+"/yar"+str(i)+".csv", 'wb') as out_file: 179 | ydot = np.hstack([0, (self.states[i, 8, 1:] - self.states[i, 8, 0:-1]) * self.LOGGING_FREQ_HZ ]) 180 | np.savetxt(out_file, np.transpose(np.vstack([t, ydot])), delimiter=",") 181 | ### 182 | with open(csv_dir+"/vx"+str(i)+".csv", 'wb') as out_file: 183 | np.savetxt(out_file, np.transpose(np.vstack([t, self.states[i, 3, :]])), delimiter=",") 184 | with open(csv_dir+"/vy"+str(i)+".csv", 'wb') as out_file: 185 | np.savetxt(out_file, np.transpose(np.vstack([t, self.states[i, 4, :]])), delimiter=",") 186 | with open(csv_dir+"/vz"+str(i)+".csv", 'wb') as out_file: 187 | np.savetxt(out_file, np.transpose(np.vstack([t, self.states[i, 5, :]])), delimiter=",") 188 | #### 189 | with open(csv_dir+"/wx"+str(i)+".csv", 'wb') as out_file: 190 | np.savetxt(out_file, np.transpose(np.vstack([t, self.states[i, 9, :]])), delimiter=",") 191 | with open(csv_dir+"/wy"+str(i)+".csv", 'wb') as out_file: 192 | np.savetxt(out_file, np.transpose(np.vstack([t, self.states[i, 10, :]])), delimiter=",") 193 | with open(csv_dir+"/wz"+str(i)+".csv", 'wb') as out_file: 194 | np.savetxt(out_file, np.transpose(np.vstack([t, self.states[i, 11, :]])), delimiter=",") 195 | #### 196 | with open(csv_dir+"/rpm0-"+str(i)+".csv", 'wb') as out_file: 197 | np.savetxt(out_file, np.transpose(np.vstack([t, self.states[i, 12, :]])), delimiter=",") 198 | with open(csv_dir+"/rpm1-"+str(i)+".csv", 'wb') as out_file: 199 | np.savetxt(out_file, np.transpose(np.vstack([t, self.states[i, 13, :]])), delimiter=",") 200 | with open(csv_dir+"/rpm2-"+str(i)+".csv", 'wb') as out_file: 201 | np.savetxt(out_file, np.transpose(np.vstack([t, self.states[i, 14, :]])), delimiter=",") 202 | with open(csv_dir+"/rpm3-"+str(i)+".csv", 'wb') as out_file: 203 | np.savetxt(out_file, np.transpose(np.vstack([t, self.states[i, 15, :]])), delimiter=",") 204 | #### 205 | with open(csv_dir+"/pwm0-"+str(i)+".csv", 'wb') as out_file: 206 | np.savetxt(out_file, np.transpose(np.vstack([t, (self.states[i, 12, :] - 4070.3) / 0.2685])), delimiter=",") 207 | with open(csv_dir+"/pwm1-"+str(i)+".csv", 'wb') as out_file: 208 | np.savetxt(out_file, np.transpose(np.vstack([t, (self.states[i, 13, :] - 4070.3) / 0.2685])), delimiter=",") 209 | with open(csv_dir+"/pwm2-"+str(i)+".csv", 'wb') as out_file: 210 | np.savetxt(out_file, np.transpose(np.vstack([t, (self.states[i, 14, :] - 4070.3) / 0.2685])), delimiter=",") 211 | with open(csv_dir+"/pwm3-"+str(i)+".csv", 'wb') as out_file: 212 | np.savetxt(out_file, np.transpose(np.vstack([t, (self.states[i, 15, :] - 4070.3) / 0.2685])), delimiter=",") 213 | 214 | ################################################################################ 215 | 216 | def plot(self, pwm=False): 217 | """Logs entries for a single simulation step, of a single drone. 218 | 219 | Parameters 220 | ---------- 221 | pwm : bool, optional 222 | If True, converts logged RPM into PWM values (for Crazyflies). 223 | 224 | """ 225 | #### Loop over colors and line styles ###################### 226 | plt.rc('axes', prop_cycle=(cycler('color', ['r', 'g', 'b', 'y']) + cycler('linestyle', ['-', '--', ':', '-.']))) 227 | fig, axs = plt.subplots(10, 2) 228 | t = np.arange(0, self.timestamps.shape[1]/self.LOGGING_FREQ_HZ, 1/self.LOGGING_FREQ_HZ) 229 | 230 | #### Column ################################################ 231 | col = 0 232 | 233 | #### XYZ ################################################### 234 | row = 0 235 | for j in range(self.NUM_DRONES): 236 | # axs[row, col].plot(t, self.states[j, 0, :]*self.MAX_XY, label="drone_"+str(j)) 237 | axs[row, col].plot(t, self.TARGET_POSITION[:, 0], label="drone_x") 238 | axs[row, col].set_xlabel('time') 239 | axs[row, col].set_ylabel('x (m)') 240 | 241 | row = 1 242 | for j in range(self.NUM_DRONES): 243 | # axs[row, col].plot(t, self.states[j, 1, :]*self.MAX_XY, label="drone_"+str(j)) 244 | axs[row, col].plot(t, self.TARGET_POSITION[:, 1], label="drone_x") 245 | axs[row, col].set_xlabel('time') 246 | axs[row, col].set_ylabel('y (m)') 247 | 248 | row = 2 249 | for j in range(self.NUM_DRONES): 250 | # axs[row, col].plot(t, self.states[j, 2, :]*self.MAX_Z, label="drone_"+str(j)) 251 | axs[row, col].plot(t, self.TARGET_POSITION[:, 2], label="drone_x") 252 | axs[row, col].set_xlabel('time') 253 | axs[row, col].set_ylabel('z (m)') 254 | 255 | #### RPY ################################################### 256 | row = 3 257 | for j in range(self.NUM_DRONES): 258 | axs[row, col].plot(t, self.states[j, 6, :], label="drone_"+str(j)) 259 | axs[row, col].set_xlabel('time') 260 | axs[row, col].set_ylabel('r (rad)') 261 | row = 4 262 | for j in range(self.NUM_DRONES): 263 | axs[row, col].plot(t, self.states[j, 7, :], label="drone_"+str(j)) 264 | axs[row, col].set_xlabel('time') 265 | axs[row, col].set_ylabel('p (rad)') 266 | row = 5 267 | for j in range(self.NUM_DRONES): 268 | axs[row, col].plot(t, self.states[j, 8, :], label="drone_"+str(j)) 269 | axs[row, col].set_xlabel('time') 270 | axs[row, col].set_ylabel('y (rad)') 271 | 272 | #### Ang Vel ############################################### 273 | row = 6 274 | for j in range(self.NUM_DRONES): 275 | axs[row, col].plot(t, self.states[j, 9, :], label="drone_"+str(j)) 276 | axs[row, col].set_xlabel('time') 277 | axs[row, col].set_ylabel('wx') 278 | row = 7 279 | for j in range(self.NUM_DRONES): 280 | axs[row, col].plot(t, self.states[j, 10, :], label="drone_"+str(j)) 281 | axs[row, col].set_xlabel('time') 282 | axs[row, col].set_ylabel('wy') 283 | row = 8 284 | for j in range(self.NUM_DRONES): 285 | axs[row, col].plot(t, self.states[j, 11, :], label="drone_"+str(j)) 286 | axs[row, col].set_xlabel('time') 287 | axs[row, col].set_ylabel('wz') 288 | 289 | #### Time ################################################## 290 | row = 9 291 | axs[row, col].plot(t, t, label="time") 292 | axs[row, col].set_xlabel('time') 293 | axs[row, col].set_ylabel('time') 294 | 295 | #### Column ################################################ 296 | col = 1 297 | 298 | #### Velocity ############################################## 299 | row = 0 300 | for j in range(self.NUM_DRONES): 301 | axs[row, col].plot(t, self.states[j, 3, :], label="drone_"+str(j)) 302 | axs[row, col].set_xlabel('time') 303 | axs[row, col].set_ylabel('vx (m/s)') 304 | row = 1 305 | for j in range(self.NUM_DRONES): 306 | axs[row, col].plot(t, self.states[j, 4, :], label="drone_"+str(j)) 307 | axs[row, col].set_xlabel('time') 308 | axs[row, col].set_ylabel('vy (m/s)') 309 | row = 2 310 | for j in range(self.NUM_DRONES): 311 | axs[row, col].plot(t, self.states[j, 5, :], label="drone_"+str(j)) 312 | axs[row, col].set_xlabel('time') 313 | axs[row, col].set_ylabel('vz (m/s)') 314 | 315 | #### RPY Rates ############################################# 316 | row = 3 317 | for j in range(self.NUM_DRONES): 318 | rdot = np.hstack([0, (self.states[j, 6, 1:] - self.states[j, 6, 0:-1]) * self.LOGGING_FREQ_HZ ]) 319 | axs[row, col].plot(t, rdot, label="drone_"+str(j)) 320 | axs[row, col].set_xlabel('time') 321 | axs[row, col].set_ylabel('rdot (rad/s)') 322 | row = 4 323 | for j in range(self.NUM_DRONES): 324 | pdot = np.hstack([0, (self.states[j, 7, 1:] - self.states[j, 7, 0:-1]) * self.LOGGING_FREQ_HZ ]) 325 | axs[row, col].plot(t, pdot, label="drone_"+str(j)) 326 | axs[row, col].set_xlabel('time') 327 | axs[row, col].set_ylabel('pdot (rad/s)') 328 | row = 5 329 | for j in range(self.NUM_DRONES): 330 | ydot = np.hstack([0, (self.states[j, 8, 1:] - self.states[j, 8, 0:-1]) * self.LOGGING_FREQ_HZ ]) 331 | axs[row, col].plot(t, ydot, label="drone_"+str(j)) 332 | axs[row, col].set_xlabel('time') 333 | axs[row, col].set_ylabel('ydot (rad/s)') 334 | 335 | ### This IF converts RPM into PWM for all drones ########### 336 | #### except drone_0 (only used in examples/compare.py) ##### 337 | for j in range(self.NUM_DRONES): 338 | for i in range(12,16): 339 | if pwm and j > 0: 340 | self.states[j, i, :] = (self.states[j, i, :] - 4070.3) / 0.2685 341 | 342 | #### RPMs ################################################## 343 | row = 6 344 | for j in range(self.NUM_DRONES): 345 | axs[row, col].plot(t, self.states[j, 12, :], label="drone_"+str(j)) 346 | axs[row, col].set_xlabel('time') 347 | if pwm: 348 | axs[row, col].set_ylabel('PWM0') 349 | else: 350 | axs[row, col].set_ylabel('RPM0') 351 | row = 7 352 | for j in range(self.NUM_DRONES): 353 | axs[row, col].plot(t, self.states[j, 13, :], label="drone_"+str(j)) 354 | axs[row, col].set_xlabel('time') 355 | if pwm: 356 | axs[row, col].set_ylabel('PWM1') 357 | else: 358 | axs[row, col].set_ylabel('RPM1') 359 | row = 8 360 | for j in range(self.NUM_DRONES): 361 | axs[row, col].plot(t, self.states[j, 14, :], label="drone_"+str(j)) 362 | axs[row, col].set_xlabel('time') 363 | if pwm: 364 | axs[row, col].set_ylabel('PWM2') 365 | else: 366 | axs[row, col].set_ylabel('RPM2') 367 | row = 9 368 | for j in range(self.NUM_DRONES): 369 | axs[row, col].plot(t, self.states[j, 15, :], label="drone_"+str(j)) 370 | axs[row, col].set_xlabel('time') 371 | if pwm: 372 | axs[row, col].set_ylabel('PWM3') 373 | else: 374 | axs[row, col].set_ylabel('RPM3') 375 | 376 | #### Drawing options ####################################### 377 | for i in range (10): 378 | for j in range (2): 379 | axs[i, j].grid(True) 380 | axs[i, j].legend(loc='upper right', 381 | frameon=True 382 | ) 383 | fig.subplots_adjust(left=0.06, 384 | bottom=0.05, 385 | right=0.99, 386 | top=0.98, 387 | wspace=0.15, 388 | hspace=0.0 389 | ) 390 | if self.COLAB: 391 | plt.savefig(os.path.join('results', 'output_figure.png')) 392 | else: 393 | plt.show() 394 | -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__init__.py -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/Logger.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/Logger.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/Logger.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/Logger.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/__init__.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/__init__.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/__init__.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/__init__.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/adsb.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/adsb.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/adsbcenter.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/adsbcenter.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/adsbcenter.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/adsbcenter.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/enums.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/enums.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/enums.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/enums.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/utils.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/utils.cpython-310.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/utils.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/__pycache__/utils.cpython-37.pyc -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/adsbcenter.py: -------------------------------------------------------------------------------- 1 | from adsb.sbs import message 2 | from pyproj import Proj 3 | from pyproj import Transformer 4 | from cmath import sqrt 5 | import math 6 | from filterpy.kalman import KalmanFilter 7 | from filterpy.common import Q_discrete_white_noise 8 | from pyproj import CRS 9 | import numpy as np 10 | import string 11 | # from gym_pybullet_drones.envs.ControlAviary import ControlAviary 12 | utm=Proj("+proj=utm +zone=30 +datum=WGS84 +units=m +no_defs ") 13 | #EPSG:4326 14 | # # See :class:`MessageType` 15 | # message_type = 0 16 | # # See :class:`TransmissionType` 17 | # transmission_type = 1 18 | # # SBS Database session record number. 19 | # session_id = 2 20 | # # SBS Database aircraft record number. 21 | # aircraft_id = 3 22 | # # 24-bit ICAO ID in hex. 23 | # hex_ident = 4 24 | # # SBS Database flight record number. 25 | # flight_id = 5 26 | # # Date the message was generated. 27 | # generated_date = 6 28 | # # Time the message was generated. 29 | # generated_time = 7 30 | # # Date the message was logged by SBS 31 | # logged_date = 8 32 | # # Time the message was logged by SBS. 33 | # logged_time = 9 34 | # # Eight character flight ID or callsign. 35 | # callsign = 10 36 | # # Altitude (ft) relative to 1013 mb (29.92" Hg). 37 | # altitude = 11 38 | # # Speed over ground (kt) 39 | # ground_speed = 12 40 | # # ground heading 41 | # track = 13 42 | # # Latitude in degrees 43 | # lat = 14 44 | # # Longitude in degrees 45 | # lon = 15 46 | # # Rate of climb 47 | # vertical_rate = 16 48 | # # Squawk 49 | # squawk = 17 50 | # # Squawk flag - indicating squawk has changed. 51 | # alert = 18 52 | # # Squawk flag indicating emergency code has been set. 53 | # emergency = 19 54 | # # Flag indicating the Special Position Indicator has been set. 55 | # spi = 20 56 | # # Flag indicating whether aircraft is on the ground 57 | # is_on_ground = 21 58 | class adsbcenter(): 59 | def __init__(self,num_drones: int=50,traj_steps:int=1): 60 | D=3 61 | N=traj_steps+5 62 | self.msglist=[[0]*N for _ in range(num_drones)] 63 | self.NUMDRONES=num_drones 64 | self.TRAJ_STEPS=traj_steps 65 | self.TEST_MSG_STR = "MSG,3,1,1,7C79B7,1,2017/03/25,10:41:45.365,2017/03/25,10:41:45.384,,2850,,,-34.84658,138.67962,,,,,," 66 | #SBSMessage(message_type='MSG', transmission_type=3, session_id=1, aircraft_id=1, hex_ident='7C79B7', \ 67 | # flight_id=1, generated_date=datetime.date(2017, 3, 25), generated_time=datetime.time(10, 41, 45, 365000), \ 68 | # logged_date=datetime.date(2017, 3, 25), logged_time=datetime.time(10, 41, 45, 384000), callsign=None, \ 69 | # altitude=2850, ground_speed=None, track=None, lat=-34.84658, lon=138.67962, vertical_rate=None, \ 70 | # squawk=None, alert=None, emergency=None, spi=None, is_on_ground=None) 71 | self.MSGARRAY=self.TEST_MSG_STR.split(",") 72 | #Define filters, only for position variables, also extendable to velocity and acceleration, one filter per drone 73 | self.myfilter=[] 74 | for i in range(num_drones): 75 | self.myfilter.append(KalmanFilter(dim_x=3,dim_z=3)) 76 | #Initial value 77 | self.myfilter[i].x=np.zeros([D,N]) 78 | #Filtered data 79 | self.myfilter[i].z=np.zeros([D,N]) 80 | #State transfer matrix, no conversion to the previous state, unit matrix 81 | self.myfilter[i].F=np.eye(D) 82 | #The covariance matrix, assuming no relationship between velocity and position, is taken as the unit matrix 83 | self.myfilter[i].P=np.eye(D) 84 | #Process noise covariance matrix, estimated values 85 | self.myfilter[i].Q=1e-2*np.eye(D) 86 | #Measurement noise covariance matrix 87 | self.myfilter[i].R=[[0.5102,0,0],[0,4.5918,0],[0,0,7.62]] 88 | #Observation Matrix 89 | self.myfilter[i].H=np.eye(D) 90 | 91 | 92 | #send from controlaviary 93 | def SendToadsb(self,adsbData,step): 94 | #msglist[][]Two-dimensional arrays，self.NUMDRONES-1:msg 95 | for i in range(self.NUMDRONES): 96 | 97 | lon,lat=self.convertlong(adsbData[i,0]+527978.0745555542,adsbData[i,1]+5700200.643251519) 98 | self.MSGARRAY[14]=str(lon)#Latitude in degrees 99 | self.MSGARRAY[15]=str(lat)#Longitude in degrees 100 | self.MSGARRAY[5]=str(i)#flight id 101 | self.MSGARRAY[17]=str(i)#Squawk 102 | self.MSGARRAY[11]=str(adsbData[i,2])#altitude(m) 103 | self.MSGARRAY[12]=str(adsbData[i,4])#Speed over ground (m/s) 104 | self.MSGARRAY[16]=str(adsbData[i,5])#Rate of climb 105 | self.MSGARRAY[13]=str(adsbData[i,3])#ground heading 106 | msg_data = ",".join(self.MSGARRAY) 107 | # msg = message.fromString(msg_data) 108 | self.msglist[i][step]=msg_data 109 | # print(msg_data.aircraft_id) 110 | 111 | 112 | def ReceiveFromadsb(self,step): 113 | msg_data=[[0]*6 for _ in range(self.NUMDRONES)] 114 | Phi = np.zeros(self.NUMDRONES) 115 | GS = np.zeros(self.NUMDRONES) 116 | pos_x=np.zeros(self.NUMDRONES) 117 | pos_y= np.zeros(self.NUMDRONES) 118 | pos_z= np.zeros(self.NUMDRONES) 119 | vel = np.zeros(self.NUMDRONES) 120 | ADSB_info=np.zeros([self.NUMDRONES,6]) 121 | for i in range(self.NUMDRONES): 122 | msg=self.msglist[i][step] 123 | recovered_msg_data = msg#message.toString(msg) 124 | # print(recovered_msg_data) 125 | self.MSGARRAY= recovered_msg_data.split(",") 126 | GS[i]=(self.MSGARRAY[12]) 127 | Phi[i]=(self.MSGARRAY[13]) 128 | pos_x[i],pos_y[i]=self.convertXY(self.MSGARRAY[14],self.MSGARRAY[15]) 129 | pos_z[i]=(self.MSGARRAY[11]) 130 | vel[i]=(self.MSGARRAY[16]) 131 | ADSB_info[i,0]=pos_x[i]-527978.0745555542 132 | ADSB_info[i,1]=pos_y[i]-5700200.643251519 133 | ADSB_info[i,2]=pos_z[i] 134 | ADSB_info[i,3]=Phi[i] 135 | ADSB_info[i,4]=GS[i] 136 | ADSB_info[i,5]=vel[i] 137 | #msg_data[i][0]=msg.aircraft_id 138 | return ADSB_info 139 | 140 | def convertlong(self,X,Y): 141 | #3.Convert XY coordinate to WGS84 coordinate 142 | lon, lat =utm(X,Y,inverse=True) 143 | #4.wgs84-30N-UTM 144 | X2, Y2 =utm(lon, lat) 145 | return lon, lat 146 | def convertXY(self,lon,lat): 147 | #3.Convert WGS84 to XY coordinate 148 | X, Y =utm(lon,lat,inverse=False) 149 | return X, Y 150 | 151 | def get_normal_random_number(self,loc, scale): 152 | """ 153 | :param loc: mean number 154 | :param scale: Standard deviation of normal distribution 155 | :return:Random numbers generated from a normal distribution 156 | """ 157 | # generate random number in normal distribution 158 | number = np.random.normal(loc=loc, scale=scale) 159 | return number 160 | 161 | def addnoise(self,adsbData): 162 | #add noise to adsb info 163 | Error_vh = np.zeros(self.NUMDRONES) 164 | Error_z = np.zeros(self.NUMDRONES) 165 | Error_h = np.zeros(self.NUMDRONES) 166 | error_h = np.zeros(self.NUMDRONES) 167 | GS_r=np.zeros(self.NUMDRONES) 168 | x_r=np.zeros(self.NUMDRONES) 169 | y_r=np.zeros(self.NUMDRONES) 170 | z_r=np.zeros(self.NUMDRONES) 171 | for j in range(self.NUMDRONES): 172 | #Simulate the error of the ADS-B info 173 | Error_vh[j] =self.get_normal_random_number(loc=-0.2, scale=2.78) 174 | Error_z[j]=self.get_normal_random_number(loc=-3.5,scale=11.16) 175 | if Error_z[j]>0: 176 | Error_z[j]=min(Error_z[j],7.62) 177 | else: 178 | Error_z[j]=max(Error_z[j],-7.62) 179 | Error_h[j]=self.get_normal_random_number(loc=0,scale=5.102) 180 | GS_r[j]=adsbData[j,4]+Error_vh[j] 181 | error_h[j]=abs(Error_h[j]) 182 | if error_h[j]>Error_h[j]: 183 | x_r[j]=adsbData[j,0]-0.1*sqrt(error_h[j]) 184 | y_r[j]=adsbData[j,1]-0.9*sqrt(error_h[j]) 185 | else: 186 | x_r[j]=adsbData[j,0]+0.1*sqrt(error_h[j]) 187 | y_r[j]=adsbData[j,1]+0.9*sqrt(error_h[j]) 188 | z_r[j]=adsbData[j,2]+Error_z[j] 189 | adsbData[j,0]=x_r[j] 190 | adsbData[j,1]=y_r[j] 191 | adsbData[j,2]=z_r[j] 192 | adsbData[j,4]=GS_r[j] 193 | return adsbData 194 | 195 | def filterdata(self,adsbData,step): 196 | #updated every step 197 | #（x,y,z,phi,gs,vel） 198 | for j in range(self.NUMDRONES): 199 | self.myfilter[j].predict() 200 | #The parameter is the value of the step observation, with random error 201 | znoise=adsbData[j,:3] 202 | self.myfilter[j].update(znoise) 203 | #size:j-3 204 | #return to receive adsbdata 205 | adsbData[j,0]=self.myfilter[j].x[0,step] 206 | adsbData[j,1]=self.myfilter[j].x[1,step] 207 | adsbData[j,2]=self.myfilter[j].x[2,step] 208 | return adsbData 209 | 210 | if __name__ == "__main__": 211 | adsbcenter=adsbcenter(3) 212 | # ADSB_info=np.zeros((3,2)) 213 | # for j in range(3): 214 | # ADSB_info[j,0]=-2.60376 215 | # ADSB_info[j,1]=51.4585712 216 | 217 | # adsbcenter.SendToadsb(ADSB_info) 218 | # adsbcenter.ReceiveFromadsb() 219 | lon=-2.5973512 220 | lat=51.452295 221 | X,Y=adsbcenter.convertXY(lon,lat) 222 | print (X,Y) 223 | lon,lat=adsbcenter.convertlong(X,Y) 224 | print (lon,lat) -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/enums.py: -------------------------------------------------------------------------------- 1 | from enum import Enum 2 | 3 | class DroneModel(Enum): 4 | """Drone models enumeration class.""" 5 | 6 | CF2X = "cf2x" # Bitcraze Craziflie 2.0 in the X configuration 7 | CF2P = "cf2p" # Bitcraze Craziflie 2.0 in the + configuration 8 | HB = "hb" # Generic quadrotor (with AscTec Hummingbird inertial properties) 9 | 10 | ################################################################################ 11 | 12 | class Physics(Enum): 13 | """Physics implementations enumeration class.""" 14 | 15 | PYB = "pyb" # Base PyBullet physics update 16 | DYN = "dyn" # Update with an explicit model of the dynamics 17 | PYB_GND = "pyb_gnd" # PyBullet physics update with ground effect 18 | PYB_DRAG = "pyb_drag" # PyBullet physics update with drag 19 | PYB_DW = "pyb_dw" # PyBullet physics update with downwash 20 | PYB_GND_DRAG_DW = "pyb_gnd_drag_dw" # PyBullet physics update with ground effect, drag, and downwash 21 | 22 | ################################################################################ 23 | 24 | class ImageType(Enum): 25 | """Camera capture image type enumeration class.""" 26 | 27 | RGB = 0 # Red, green, blue (and alpha) 28 | DEP = 1 # Depth 29 | SEG = 2 # Segmentation by object id 30 | BW = 3 # Black and white 31 | 32 | ################################################################################ -------------------------------------------------------------------------------- /gym-pybullet-drones/gym-pybullet-drones/gym_pybullet_drones/utils/utils.py: -------------------------------------------------------------------------------- 1 | """General use functions. 2 | """ 3 | import time 4 | import argparse 5 | import numpy as np 6 | from scipy.optimize import nnls 7 | 8 | ################################################################################ 9 | 10 | def sync(i, start_time, timestep): 11 | """Syncs the stepped simulation with the wall-clock. 12 | 13 | Function `sync` calls time.sleep() to pause a for-loop 14 | running faster than the expected timestep. 15 | 16 | Parameters 17 | ---------- 18 | i : int 19 | Current simulation iteration. 20 | start_time : timestamp 21 | Timestamp of the simulation start. 22 | timestep : float 23 | Desired, wall-clock step of the simulation's rendering. 24 | 25 | """ 26 | if timestep > .04 or i%(int(1/(24*timestep))) == 0: 27 | elapsed = time.time() - start_time 28 | if elapsed < (i*timestep): 29 | time.sleep(timestep*i - elapsed) 30 | 31 | ################################################################################ 32 | 33 | def str2bool(val): 34 | """Converts a string into a boolean. 35 | 36 | Parameters 37 | ---------- 38 | val : str | bool 39 | Input value (possibly string) to interpret as boolean. 40 | 41 | Returns 42 | ------- 43 | bool 44 | Interpretation of `val` as True or False. 45 | 46 | """ 47 | if isinstance(val, bool): 48 | return val 49 | elif val.lower() in ('yes', 'true', 't', 'y', '1'): 50 | return True 51 | elif val.lower() in ('no', 'false', 'f', 'n', '0'): 52 | return False 53 | else: 54 | raise argparse.ArgumentTypeError("[ERROR] in str2bool(), a Boolean value is expected") 55 | 56 | ################################################################################ 57 | 58 | def nnlsRPM(thrust, 59 | x_torque, 60 | y_torque, 61 | z_torque, 62 | counter, 63 | max_thrust, 64 | max_xy_torque, 65 | max_z_torque, 66 | a, 67 | inv_a, 68 | b_coeff, 69 | gui=False 70 | ): 71 | """Non-negative Least Squares (NNLS) RPMs from desired thrust and torques. 72 | 73 | This function uses the NNLS implementation in `scipy.optimize`. 74 | 75 | Parameters 76 | ---------- 77 | thrust : float 78 | Desired thrust along the drone's z-axis. 79 | x_torque : float 80 | Desired drone's x-axis torque. 81 | y_torque : float 82 | Desired drone's y-axis torque. 83 | z_torque : float 84 | Desired drone's z-axis torque. 85 | counter : int 86 | Simulation or control iteration, only used for printouts. 87 | max_thrust : float 88 | Maximum thrust of the quadcopter. 89 | max_xy_torque : float 90 | Maximum torque around the x and y axes of the quadcopter. 91 | max_z_torque : float 92 | Maximum torque around the z axis of the quadcopter. 93 | a : ndarray 94 | (4, 4)-shaped array of floats containing the motors configuration. 95 | inv_a : ndarray 96 | (4, 4)-shaped array of floats, inverse of a. 97 | b_coeff : ndarray 98 | (4,1)-shaped array of floats containing the coefficients to re-scale thrust and torques. 99 | gui : boolean, optional 100 | Whether a GUI is active or not, only used for printouts. 101 | 102 | Returns 103 | ------- 104 | ndarray 105 | (4,)-shaped array of ints containing the desired RPMs of each propeller. 106 | 107 | """ 108 | #### Check the feasibility of thrust and torques ########### 109 | if gui and thrust < 0 or thrust > max_thrust: 110 | print("[WARNING] iter", counter, "in utils.nnlsRPM(), unfeasible thrust {:.2f} outside range [0, {:.2f}]".format(thrust, max_thrust)) 111 | if gui and np.abs(x_torque) > max_xy_torque: 112 | print("[WARNING] iter", counter, "in utils.nnlsRPM(), unfeasible roll torque {:.2f} outside range [{:.2f}, {:.2f}]".format(x_torque, -max_xy_torque, max_xy_torque)) 113 | if gui and np.abs(y_torque) > max_xy_torque: 114 | print("[WARNING] iter", counter, "in utils.nnlsRPM(), unfeasible pitch torque {:.2f} outside range [{:.2f}, {:.2f}]".format(y_torque, -max_xy_torque, max_xy_torque)) 115 | if gui and np.abs(z_torque) > max_z_torque: 116 | print("[WARNING] iter", counter, "in utils.nnlsRPM(), unfeasible yaw torque {:.2f} outside range [{:.2f}, {:.2f}]".format(z_torque, -max_z_torque, max_z_torque)) 117 | B = np.multiply(np.array([thrust, x_torque, y_torque, z_torque]), b_coeff) 118 | sq_rpm = np.dot(inv_a, B) 119 | #### NNLS if any of the desired ang vel is negative ######## 120 | if np.min(sq_rpm) < 0: 121 | sol, res = nnls(a, 122 | B, 123 | maxiter=3*a.shape[1] 124 | ) 125 | if gui: 126 | print("[WARNING] iter", counter, "in utils.nnlsRPM(), unfeasible squared rotor speeds, using NNLS") 127 | print("Negative sq. rotor speeds:\t [{:.2f}, {:.2f}, {:.2f}, {:.2f}]".format(sq_rpm[0], sq_rpm[1], sq_rpm[2], sq_rpm[3]), 128 | "\t\tNormalized: [{:.2f}, {:.2f}, {:.2f}, {:.2f}]".format(sq_rpm[0]/np.linalg.norm(sq_rpm), sq_rpm[1]/np.linalg.norm(sq_rpm), sq_rpm[2]/np.linalg.norm(sq_rpm), sq_rpm[3]/np.linalg.norm(sq_rpm))) 129 | print("NNLS:\t\t\t\t [{:.2f}, {:.2f}, {:.2f}, {:.2f}]".format(sol[0], sol[1], sol[2], sol[3]), 130 | "\t\t\tNormalized: [{:.2f}, {:.2f}, {:.2f}, {:.2f}]".format(sol[0]/np.linalg.norm(sol), sol[1]/np.linalg.norm(sol), sol[2]/np.linalg.norm(sol), sol[3]/np.linalg.norm(sol)), 131 | "\t\tResidual: {:.2f}".format(res)) 132 | sq_rpm = sol 133 | return np.sqrt(sq_rpm) 134 | -------------------------------------------------------------------------------- /results/save-ControlAviary-ddpg-kin-tun-48HZ-1M/success_model.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/results/save-ControlAviary-ddpg-kin-tun-48HZ-1M/success_model.zip -------------------------------------------------------------------------------- /results/save-ControlAviary-ddpg-kin-tun-48HZ-1M/tb/DDPG_1/events.out.tfevents.1677372157.hecs-340817.8560.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/results/save-ControlAviary-ddpg-kin-tun-48HZ-1M/tb/DDPG_1/events.out.tfevents.1677372157.hecs-340817.8560.0 -------------------------------------------------------------------------------- /results/save-ControlAviary-ppo-kin-tun-12HZ-1M/success_model.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/results/save-ControlAviary-ppo-kin-tun-12HZ-1M/success_model.zip -------------------------------------------------------------------------------- /results/save-ControlAviary-ppo-kin-tun-12HZ-1M/tb/PPO_1/events.out.tfevents.1679256062.hecs-340817.5200.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/results/save-ControlAviary-ppo-kin-tun-12HZ-1M/tb/PPO_1/events.out.tfevents.1679256062.hecs-340817.5200.0 -------------------------------------------------------------------------------- /results/save-ControlAviary-ppo-kin-tun-2HZ-1M/success_model.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/results/save-ControlAviary-ppo-kin-tun-2HZ-1M/success_model.zip -------------------------------------------------------------------------------- /results/save-ControlAviary-ppo-kin-tun-2HZ-1M/tb/PPO_1/events.out.tfevents.1679076909.hecs-340817.7528.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/results/save-ControlAviary-ppo-kin-tun-2HZ-1M/tb/PPO_1/events.out.tfevents.1679076909.hecs-340817.7528.0 -------------------------------------------------------------------------------- /results/save-ControlAviary-ppo-kin-tun-2Hz-2M/success_model.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/results/save-ControlAviary-ppo-kin-tun-2Hz-2M/success_model.zip -------------------------------------------------------------------------------- /results/save-ControlAviary-ppo-kin-tun-2Hz-2M/tb/PPO_1/events.out.tfevents.1679153911.hecs-340817.3700.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/results/save-ControlAviary-ppo-kin-tun-2Hz-2M/tb/PPO_1/events.out.tfevents.1679153911.hecs-340817.3700.0 -------------------------------------------------------------------------------- /results/save-ControlAviary-ppo-kin-tun-48HZ-1M-Ob/success_model.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/results/save-ControlAviary-ppo-kin-tun-48HZ-1M-Ob/success_model.zip -------------------------------------------------------------------------------- /results/save-ControlAviary-ppo-kin-tun-48HZ-1M-Ob/tb/PPO_1/events.out.tfevents.1677942124.hecs-340817.8428.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/results/save-ControlAviary-ppo-kin-tun-48HZ-1M-Ob/tb/PPO_1/events.out.tfevents.1677942124.hecs-340817.8428.0 -------------------------------------------------------------------------------- /results/save-ControlAviary-ppo-kin-tun-48HZ-1M/success_model.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/results/save-ControlAviary-ppo-kin-tun-48HZ-1M/success_model.zip -------------------------------------------------------------------------------- /results/save-ControlAviary-ppo-kin-tun-48HZ-1M/tb/PPO_1/events.out.tfevents.1675884099.hecs-340817.6760.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/results/save-ControlAviary-ppo-kin-tun-48HZ-1M/tb/PPO_1/events.out.tfevents.1675884099.hecs-340817.6760.0 -------------------------------------------------------------------------------- /results/save-ControlAviary-ppo-kin-tun-8HZ-1M/success_model.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/results/save-ControlAviary-ppo-kin-tun-8HZ-1M/success_model.zip -------------------------------------------------------------------------------- /results/save-ControlAviary-ppo-kin-tun-8HZ-1M/tb/PPO_1/events.out.tfevents.1679318052.hecs-340817.4352.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AbsoluteNegativity/DRL4DAPTA/8d928d4401043ff4916ce20add926b7d7a470060/results/save-ControlAviary-ppo-kin-tun-8HZ-1M/tb/PPO_1/events.out.tfevents.1679318052.hecs-340817.4352.0 --------------------------------------------------------------------------------