diff --git a/.gitignore b/.gitignore
index 81c4d846b..87200fe4d 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,6 @@
+# TO-DO list
+TODOs.md
+
# macOS users
.DS_Store
diff --git a/LICENSE b/LICENSE
index f6ed15e43..5751ab52a 100644
--- a/LICENSE
+++ b/LICENSE
@@ -1,6 +1,6 @@
MIT License
-Copyright (c) 2020 JacopoPan
+Copyright (c) 2020 Jacopo Panerati
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
diff --git a/README.md b/README.md
index 7af2e31b4..7ebb79bc2 100644
--- a/README.md
+++ b/README.md
@@ -1,7 +1,7 @@
# gym-pybullet-drones
-Simple [OpenAI Gym environment](https://gym.openai.com/envs/#classic_control) based on [PyBullet](https://github.com/bulletphysics/bullet3) to simulate one (or more, WIP) quadrotors
+Simple [OpenAI Gym environment](https://gym.openai.com/envs/#classic_control) based on [PyBullet](https://github.com/bulletphysics/bullet3) to simulate one or more quadrotors
-
+
- The default `DroneModel.CF2X` dynamics are based on [Bitcraze's Crazyflie 2.x nano-quadrotor](https://www.bitcraze.io/documentation/hardware/crazyflie_2_1/crazyflie_2_1-datasheet.pdf)
@@ -18,10 +18,10 @@ $ pip install gym
$ pip install pybullet
$ pip install stable-baselines3
$ pip install `ray[rllib]`
-$ brew install ffmpeg # on macOS
-$ sudo apt install ffmpeg # on Linux
+$ brew install ffmpeg # on macOS
+$ sudo apt install ffmpeg # on Linux
```
-With a [`conda` environment](https://github.com/JacopoPan/a-minimalist-guide#install-conda),
+Using a [`conda` environment](https://github.com/JacopoPan/a-minimalist-guide#install-conda),
dependencies (except `ffmpeg`), can be installed from file
```
$ cd gym-pybullet-drones/
@@ -44,65 +44,65 @@ $ pip install -e .
## Use
-There are 4 main scripts in `examples/`: `sa_trace.py`, `sa_flight.py`, `sa_learning.py`, `test_physics.py`
+There are 4 main scripts in `examples/`: `compare.py`, `fly.py`, `learn.py`, and `physics.py`
-- `test_physics.py` can be used to test the effects of PyBullet's forces and torques to different [URDF links](http://wiki.ros.org/urdf/XML/link) in `p.WORLD_FRAME` and `p.LINK_FRAME`
+- `compare.py` replays and compare to a trace saved in [`example_trace.pkl`](https://github.com/JacopoPan/gym-pybullet-drones/tree/master/files/example_trace.pkl)
```
-$ conda activate myenv # If using a conda environment
+$ conda activate myenv # If using a conda environment
$ cd gym-pybullet-drones/examples/
-$ python test_physics.py
+$ python compare.py
```
-> Tip: also check the examples in [pybullet-examples](https://github.com/JacopoPan/pybullet-examples)
+
-- `sa_trace.py` replays and compare to a trace saved in [`trace.pkl`](https://github.com/JacopoPan/gym-pybullet-drones/tree/master/files/traces)
+- `fly.py` runs an independent flight **using PID control** implemented in class [`Control`](https://github.com/JacopoPan/gym-pybullet-drones/tree/master/gym_pybullet_drones/env/Control.py)
```
-$ conda activate myenv # If using a conda environment
+$ conda activate myenv # If using a conda environment
$ cd gym-pybullet-drones/examples/
-$ python sa_trace.py
+$ python fly.py
```
-
+> Tip: use the GUI's sliders and button `Use GUI RPM` to override the control with interactive inputs
+
+
+
-- `sa_flight.py` runs an independent flight **using PID control** implemented in class [`SingleDroneControl`](https://github.com/JacopoPan/gym-pybullet-drones/tree/master/gym_pybullet_drones/control/SingleDroneControl.py)
+- `learn.py` is an **RL example** to learn take-off using `stable-baselines3`'s [A2C](https://stable-baselines3.readthedocs.io/en/master/modules/a2c.html) or `rllib`'s [PPO](https://docs.ray.io/en/master/rllib-algorithms.html#ppo)
```
-$ conda activate myenv # If using a conda environment
+$ conda activate myenv # If using a conda environment
$ cd gym-pybullet-drones/examples/
-$ python sa_flight.py
+$ python learn.py
```
-> Tip: use the GUI's sliders and button `Use GUI RPM` to override the control with interactive inputs
-
-
+
+
-- `sa_learning.py` is an **RL example** to learn take-off using `stable-baselines3`'s [A2C](https://stable-baselines3.readthedocs.io/en/master/modules/a2c.html) or `rllib`'s [PPO](https://docs.ray.io/en/master/rllib-algorithms.html#ppo)
+- `physics.py` is an accessory script that can be used to understand PyBullet's force and torque APIs for different [URDF links](http://wiki.ros.org/urdf/XML/link) in `p.WORLD_FRAME` and `p.LINK_FRAME`
```
-$ conda activate myenv # If using a conda environment
+$ conda activate myenv # If using a conda environment
$ cd gym-pybullet-drones/examples/
-$ python sa_learning.py
+$ python physics.py
```
-
-
-
-
+> Tip: also check the examples in [pybullet-examples](https://github.com/JacopoPan/pybullet-examples)
-## SingleDroneEnv
-A single quadrotor enviroment can be created with `SingleDroneEnv()`—see [`sa_flight.py`](https://github.com/JacopoPan/gym-pybullet-drones/blob/master/examples/sa_flight.py) for an example
+## Aviary
+A `gym.Env` flight arena for one (ore more) quadrotor can be created with `Aviary()`—see [`fly.py`](https://github.com/JacopoPan/gym-pybullet-drones/blob/master/examples/fly.py) for an example
```
->>> env = SingleDroneEnv( \
->>> drone_model=DroneModel.CF2X, \ # See DroneModel.py for other quadcopter models (remove this comment)
->>> initial_xyz=[0,0,0.1], \ # Initial XYZ position of the drone (remove this comment)
->>> initial_rpy=[0,0,0], \ # Initial roll, pitch, and yaw of the drone in radians (remove this comment)
->>> pybullet=True, \ # Whether to use PyBullet physics or the dynamics in method _noPyBulletDynamics() (remove this comment)
->>> aero_effects=False \ # Whether to include PyBullet-based drag, ground effect, and downwash (WIP) (remove this comment)
+>>> env = Aviary( \
+>>> drone_model=DroneModel.CF2X, \ # See DroneModel Enum class for other quadcopter models (remove this comment)
+>>> num_drones=1, \ # Number of drones (remove this comment)
+>>> visibility_radius=np.inf, \ # Distance at which drones are considered neighbors, only used for multiple drones (remove this comment)
+>>> initial_xyzs=None, \ # Initial XYZ positions of the drones (remove this comment)
+>>> initial_rpys=None, \ # Initial roll, pitch, and yaw of the drones in radians (remove this comment)
+>>> physics: Physics=Physics.PYB, \ # Choice of (PyBullet) physics implementation (remove this comment)
>>> normalized_spaces=True, \ # Whether to use normalized action and observation spaces—use True for learning (default), False for simulation (remove this comment)
->>> freq=240, \ # The stepping frequency of the simulation (remove this comment)
+>>> freq=240, \ # Stepping frequency of the simulation (remove this comment)
>>> gui=True, \ # Whether to display PyBullet's GUI (remove this comment)
>>> obstacles=False, \ # Whether to add obstacles to the environment (remove this comment)
->>> record=False, \ # Whether to save a .mp4 video in gym-pybullet-drones/files/saves/ (remove this comment)
->>> user="Default") # String to choose reward and done functions in class SingleDroneUserDefinedFunctions (remove this comment)
+>>> record=False, \ # Whether to save a .mp4 video in gym-pybullet-drones/files/ (remove this comment)
+>>> problem: Problem=Problem.DEFAULT) # Choice of reward and done functions in class ProblemSpecificFunctions (remove this comment)
````
-Or using `gym.make()`—see [`sa_learning.py`](https://github.com/JacopoPan/gym-pybullet-drones/blob/master/examples/sa_learning.py) for an example
+Or using `gym.make()`—see [`learn.py`](https://github.com/JacopoPan/gym-pybullet-drones/blob/master/examples/learn.py) for an example
```
->>> env = gym.make('single-drone-v0') # See sa_learning.py
+>>> env = gym.make('the-aviary-v1') # See learn.py
```
Then, the environment can be stepped with
```
@@ -118,97 +118,122 @@ Then, the environment can be stepped with
### Action Space
-The action space is a [`Box(4,)`](https://github.com/openai/gym/blob/master/gym/spaces/box.py) containing the desired rotation speed of each motor, if `normalized_spaces=True`, they range from -1 to 1, if `False` from 0 to `SingleDroneEnv.MAX_RPM`
+The action space is a [`Dict()`](https://github.com/openai/gym/blob/master/gym/spaces/dict.py) of [`Box(4,)`](https://github.com/openai/gym/blob/master/gym/spaces/box.py)'s containing the desired control inputs
+
+Keys are `"0"`, `"1"`, .., `"n"`, where `n` is the number of drones
+
+For all `Physics` implementations, except `PYB_PM` and `PYB_KIN`, these are the rotation speeds of all motors ranging from `-1` to `1` if `normalized_spaces=True`, or from `0` to `SingleDroneEnv.MAX_RPM` otherwise
+
+For `physics=Physics.PYB_PM` and `Physics.PYB_KIN`, the control inputs are the desired acceleration and velocity, respectively
+
+
+
### Observation Space
-The observation space is a [`Box(20,)`](https://github.com/openai/gym/blob/master/gym/spaces/box.py) containing the drone's
-- XYZ position
-- Quaternion pose
-- Roll, Pitch and Yaw angles
-- Velocity
-- Angular velocity
-- Motors' speeds
+The observation space is a [`Dict()`](https://github.com/openai/gym/blob/master/gym/spaces/dict.py) of pairs `{"state": Box(20,), "neighbors": MultiBinary(num_drones)}`
+
+Keys are `"0"`, `"1"`, .., `"n"`, where `n` is the number of drones
+
+Each [`Box(20,)`](https://github.com/openai/gym/blob/master/gym/spaces/box.py) contains the drone's
+- X, Y, Z position in `WORLD_FRAME` (3 value, meters unless normalized)
+- Quaternion orientation in `WORLD_FRAME` (4 values)
+- Roll, pitch and yaw angles in `WORLD_FRAME` (3 values, radians unless normalized)
+- The velocity vector in `WORLD_FRAME` (3 values, m/s unless normalized)
+- Angular velocities in `WORLD_FRAME` (3 values, rad/s unless normalized)
+- Motors' speeds (4 values, RPM)
+
+Check [`ProblemSpecificFunctions.clipAndNormalizeState()`](https://github.com/JacopoPan/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/ProblemSpecificFunctions.py) for the mapping from raw simulation data to normalized observations in the `[-1,1]` range (i.e., when `normalized_spaces==True`)
+
+Each [`MultiBinary(num_drones)`](https://github.com/openai/gym/blob/master/gym/spaces/multi_binary.py) contains the drone's own row of the multi-robot system adjacency matrix
+
+
+### IMPORTANT NOTE: Single Agent Action and Observation Spaces
+When `num_drones==1`, action and observations spaces are simplified to [`Box(4,)`](https://github.com/openai/gym/blob/master/gym/spaces/box.py) and [`Box(20,)`](https://github.com/openai/gym/blob/master/gym/spaces/box.py), respectively
+
+
+
+
+## ProblemSpecificFunctions
+Class [`ProblemSpecificFunctions`](https://github.com/JacopoPan/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/ProblemSpecificFunctions.py) contains implementations of reward, done, and normalization functions that can be selected when create an environment with `problem=Problem.CUSTOM`
+
+
-Check [`SingleDroneUserDefinedFunctions.clipAndNormalizeState()`](https://github.com/JacopoPan/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/SingleDroneUserDefinedFunctions.py) for the mapping from raw simulation data to normalized observations in the [-1,1] range (i.e., when using `normalized_spaces=True`)
### Reward
-The reward function can/should be modified in class [`SingleDroneUserDefinedFunctions`](https://github.com/JacopoPan/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/SingleDroneUserDefinedFunctions.py), for example
+Reward functions can/should be modified in class [`ProblemSpecificFunctions`](https://github.com/JacopoPan/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/ProblemSpecificFunctions.py), for example
```
->>> def rewardFunction(self, state):
->>> if self.USER == "Default":
+>>> def rewardFunction(self, obs):
+>>> if self.PROBLEM==Problem.DEFAULT:
>>> ...
->>> elif self.USER == "Custom": # Use with: >>> env = SingleDroneEnv(user="Custom")
->>> height = state[2]
+>>> elif self.PROBLEM==Problem.CUSTOM: # Use with: >>> env = Aviary(problem=Problem.CUSTOM)
+>>> height = obs[2]
>>> if height > 0.5: return 1000
>>> elif height > 0.1: return 100
>>> else: return -1
->>> else: print("[ERROR] in rewardFunction(), unknown user")
+>>> else: print("[ERROR] unknown user")
```
+
+
+
### Done
-The halting conditions can/should be modified in class [`SingleDroneUserDefinedFunctions`](https://github.com/JacopoPan/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/SingleDroneUserDefinedFunctions.py), for example
+Stopping conditions can/should be modified in class [`ProblemSpecificFunctions`](https://github.com/JacopoPan/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/ProblemSpecificFunctions.py), for example
```
->>> def doneFunction(self, state, sim_time):
->>> if self.USER == "Default":
+>>> def doneFunction(self, obs, sim_time):
+>>> if self.PROBLEM==Problem.DEFAULT:
>>> ...
->>> elif self.USER == "Custom": # Use with: >>> env = SingleDroneEnv(user="Custom")
->>> x = state[0]; y = state[1]; z = state[2]
->>> roll = state[7]; pitch = state[8]
+>>> elif self.PROBLEM==Problem.CUSTOM: # Use with: >>> env = Aviary(problem=Problem.CUSTOM)
+>>> x = obs[0]; y = obs[1]; z = obs[2]
+>>> roll = obs[7]; pitch = obs[8]
>>> if np.abs(x)>.5 or np.abs(y)>.5 or z>=1 \
>>> or np.abs(roll)>np.pi/2 or np.abs(pitch)>np.pi/2 \
>>> or sim_time > 5:
>>> return True
>>> else:
>>> return False
->>> else: print("[ERROR] in doneFunction(), unknown user")
+>>> else: print("[ERROR] unknown user")
```
-### Drag and Ground Effect Models
-Simple drag and ground effect models can be included in the simulation initializing `SingleDroneEnv()` with `aero_effects=True`, these are based on the system identification of [Forster (2015)](http://mikehamer.info/assets/papers/Crazyflie%20Modelling.pdf) (see Eq. 4.2)
-and the analytical model used as a baseline for comparison by [Shi et al. (2019)](https://arxiv.org/pdf/1811.08027.pdf) (see Eq. 15)
-
-Check the implementation of `_simpleAerodynamicEffects()` in class [`SingleDroneEnv`](https://github.com/JacopoPan/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/SingleDroneEnv.py) for more detail
+### Drag, Ground Effect, and Downwash Models
+Simple drag, ground effect, and downwash models can be included in the simulation initializing `Aviary()` with `physics=Physics.PYB_GND_DRAG_DW`; these are based on the system identification of [Forster (2015)](http://mikehamer.info/assets/papers/Crazyflie%20Modelling.pdf) (see Eq. 4.2), the analytical model used as a baseline for comparison by [Shi et al. (2019)](https://arxiv.org/pdf/1811.08027.pdf) (see Eq. 15), and [DSL](https://www.dynsyslab.org/vision-news/)'s experimental work by SiQi Zhou
-
-
-
-## MultiDroneEnv
-A multi-agent extension of SingleDroneEnv (WIP)
-### Actions
-...
-### Observation Space
-...
-### Reward
-...
-### Done
-...
+Check the implementations of `_drag()`, `_groundEffect()`, and `_downwash()` in class [`Aviary`](https://github.com/JacopoPan/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/Aviary.py) for more detail
-### Downwash Model
-WIP
-
-
-
-
-## Simpler Environments for Swarm Applications
-WIP
-
-
-
-
-### PointMassSwarmEnv
-A multi-agent environment with simplifed dynamics, using point-mass models (WIP)
+## Control
+Class [`Control`](https://github.com/JacopoPan/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/Control.py) contains implementations of controllers that can be used to command the quadcopters, for example
+```
+>>> ctrl = [Control(env, control_type=ControlType.PID) for i in range(num_drones)] # Initialize "num_drones" controllers
+>>> ...
+>>> for i in range(num_drones): # Compute control for each drone
+>>> action[str(i)], _, _ = ctrl[i].computeControlFromState( \ # Write the action in a dictionary
+>>> control_timestep=env.TIMESTEP, \
+>>> state=obs[str(j)]["state"], \
+>>> target_pos=TARGET_POS)
+```
-### VelocitySwarmEnv
-A multi-agent environment without dynamics, where velocities can be arbitrarily set (WIP)
+## Logger
+Class [`Logger`](https://github.com/JacopoPan/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/Control.py) contains helper functions to save an plot the simulation data, for example
+```
+>>> logger = Logger(duration_sec=T, simulation_freq_hz=freq, num_drones=num_drones) # Initialize the logger
+>>> ...
+>>> for i in range(NUM_DRONES): # Log information for each drone
+>>> logger.log(drone=i, \
+>>> timestamp=K/env.SIM_FREQ, \
+>>> state= obs[str(i)]["state"], \
+>>> control=np.hstack([ TARGET_POS, np.zeros(9) ]))
+>>> ...
+>>> logger.save() # Save data to file
+>>> logger.plot() # Plot data
+```
@@ -217,8 +242,8 @@ A multi-agent environment without dynamics, where velocities can be arbitrarily
If you wish, please refer to this work as
```
@MISC{gym-pybullet-drones2020,
- author = {Panerati, Jacopo and Zhou, SiQi and Zheng, Hehui and Xu, James and Prorok, Amanda and Schoellig, Angela P.},
- title = {A PyBullet-based Gym environment to simulate and learn nano-quadcopter control},
+ author = {Panerati, Jacopo and Zheng, Hehui and Zhou, SiQi and Xu, James and Prorok, Amanda and Schoellig, Angela P.},
+ title = {Learning to Fly: a Gym environment based on PyBullet to simulate and learn the control of multiple nano-quadcopters},
howpublished = {\url{https://github.com/JacopoPan/gym-pybullet-drones}},
year = {2020}
}
diff --git a/examples/compare.py b/examples/compare.py
new file mode 100644
index 000000000..8150f38b2
--- /dev/null
+++ b/examples/compare.py
@@ -0,0 +1,84 @@
+import os
+import time
+from datetime import datetime
+import pdb
+import math
+import numpy as np
+import pybullet as p
+import pickle
+import matplotlib.pyplot as plt
+
+from utils import *
+from gym_pybullet_drones.envs.Aviary import DroneModel, Physics, Aviary
+from gym_pybullet_drones.envs.Logger import Logger
+from gym_pybullet_drones.envs.Control import ControlType, Control
+
+GUI = False
+RECORD_VIDEO = False
+TRACE_FILE = "example_trace.pkl"
+PHYSICS = Physics.PYB
+
+if __name__ == "__main__":
+
+ #### Load a trace and control reference from a .pkl file ###########################################
+ with open(os.path.dirname(os.path.abspath(__file__))+"/../files/"+TRACE_FILE, 'rb') as in_file:
+ TRACE_TIMESTAMPS, TRACE_DATA, TRACE_CTRL_REFERENCE, _, _, _ = pickle.load(in_file)
+
+
+ #TRACE_CTRL_REFERENCE = np.array(TRACE_CTRL_REFERENCE); TRACE_DATA = np.array(TRACE_DATA)
+
+
+
+ #### Compute trace's parameters ####################################################################
+ DURATION_SEC = int(TRACE_TIMESTAMPS[-1]); SIMULATION_FREQ_HZ = int(len(TRACE_TIMESTAMPS)/TRACE_TIMESTAMPS[-1])
+
+ #### Initialize the simulation #####################################################################
+ env = Aviary(drone_model=DroneModel.CF2X, initial_xyzs=np.array([0,0,.1]).reshape(1,3), physics=PHYSICS, \
+ normalized_spaces=False, freq=SIMULATION_FREQ_HZ, gui=GUI, obstacles=False, record=RECORD_VIDEO)
+ INITIAL_STATE = env.reset(); action = np.zeros(4); pos_err = 9999.
+
+ #### Assuming TRACE_FILE starts at position [0,0,0] and the sim starts at [0,0,INITIAL_STATE[2]] ###
+ TRACE_CTRL_REFERENCE[:,2] = INITIAL_STATE[2]
+
+ #### Initialize the logger #########################################################################
+ logger = Logger(duration_sec=DURATION_SEC, simulation_freq_hz=SIMULATION_FREQ_HZ, num_drones=2)
+
+ #### Initialize the controller #####################################################################
+ ctrl = Control(env, control_type=ControlType.PID)
+
+ #### Run the comparison ############################################################################
+ START = time.time()
+ for i in range(DURATION_SEC*env.SIM_FREQ):
+
+ #### Step the simulation ###########################################################################
+ obs, reward, done, info = env.step(action)
+
+ #### Compute the next action using the set points from the trace file ##############################
+ action, pos_err, yaw_err = ctrl.computeControlFromState(control_timestep=env.TIMESTEP, state=obs, \
+ target_pos=TRACE_CTRL_REFERENCE[i,0:3], target_vel=TRACE_CTRL_REFERENCE[i,3:6])
+
+
+ #### Re-arrange the trace for consistency with the logger #########################################
+ trace_obs = np.hstack([TRACE_DATA[i,0:3], np.zeros(4), TRACE_DATA[i,6:9], TRACE_DATA[i,3:6], TRACE_DATA[i,9:12], TRACE_DATA[i,12:16]])
+
+ #### Log the trace #################################################################################
+ logger.log(drone=0, timestamp=TRACE_TIMESTAMPS[i], state=trace_obs, control=np.hstack([TRACE_CTRL_REFERENCE[i,:], np.zeros(6)]))
+
+ #### Log the simulation ############################################################################
+ logger.log(drone=1, timestamp=i/env.SIM_FREQ, state=obs, control=np.hstack([TRACE_CTRL_REFERENCE[i,:], np.zeros(6)]))
+
+ #### Printout ######################################################################################
+ env.render()
+
+ #### Sync the simulation ###########################################################################
+ if GUI: sync(i, START, env.TIMESTEP)
+
+ #### Close the environment #########################################################################
+ env.close()
+
+ #### Save the simulation results ###################################################################
+ logger.save()
+
+ #### Plot the simulation results ###################################################################
+ logger.plot(pwm=True)
+
diff --git a/examples/fly.py b/examples/fly.py
new file mode 100644
index 000000000..583d57d84
--- /dev/null
+++ b/examples/fly.py
@@ -0,0 +1,81 @@
+import os
+import time
+from datetime import datetime
+import pdb
+import math
+import numpy as np
+import pybullet as p
+import pickle
+import matplotlib.pyplot as plt
+
+from utils import *
+from gym_pybullet_drones.envs.Aviary import DroneModel, Physics, Aviary
+from gym_pybullet_drones.envs.Logger import Logger
+from gym_pybullet_drones.envs.Control import ControlType, Control
+
+DRONE = DroneModel.CF2X
+NUM_DRONES = 3
+GUI = True
+PHYSICS = Physics.PYB
+RECORD_VIDEO = False
+SIMULATION_FREQ_HZ = 240
+CONTROL_FREQ_HZ = 48
+DURATION_SEC = 10
+
+if __name__ == "__main__":
+
+ #### Initialize the simulation #####################################################################
+ H = .5; R = .5; INIT_XYZS = np.array([ [R*np.cos((i/NUM_DRONES)*2*np.pi+np.pi/2), R*np.sin((i/NUM_DRONES)*2*np.pi+np.pi/2)-R, H] for i in range(NUM_DRONES) ])
+ env = Aviary(drone_model=DRONE, num_drones=NUM_DRONES, initial_xyzs=INIT_XYZS, physics=PHYSICS, visibility_radius=10, \
+ normalized_spaces=False, freq=SIMULATION_FREQ_HZ, gui=GUI, obstacles=True, record=RECORD_VIDEO); env.reset()
+
+ #### Initialize a circular trajectory ##############################################################
+ PERIOD = 10; NUM_WP = CONTROL_FREQ_HZ*PERIOD; TARGET_POS = np.zeros((NUM_WP,3))
+ for i in range(NUM_WP): TARGET_POS[i,:] = R*np.cos((i/NUM_WP)*(2*np.pi)+np.pi/2)+INIT_XYZS[0,0], R*np.sin((i/NUM_WP)*(2*np.pi)+np.pi/2)-R+INIT_XYZS[0,1], INIT_XYZS[0,2]
+ wp_counters = [ int(i*NUM_WP/NUM_DRONES) for i in range(NUM_DRONES) ]
+
+ #### Initialize the logger #########################################################################
+ logger = Logger(duration_sec=DURATION_SEC, simulation_freq_hz=SIMULATION_FREQ_HZ, num_drones=NUM_DRONES)
+
+ #### Initialize the controllers ####################################################################
+ ctrl = [Control(env, control_type=ControlType.PID) for i in range(NUM_DRONES)]
+
+ #### Run the simulation ############################################################################
+ CTRL_EVERY_N_STEPS= int(np.floor(env.SIM_FREQ/CONTROL_FREQ_HZ))
+ action = { str(i): np.array([0,0,0,0]) for i in range(NUM_DRONES) } if NUM_DRONES>1 else np.array([0,0,0,0])
+ START = time.time()
+ for i in range(DURATION_SEC*env.SIM_FREQ):
+
+ #### Step the simulation ###########################################################################
+ obs, reward, done, info = env.step(action)
+
+ #### Transform 1-drone obs into the Dict format of multiple drones to simplify the code ############
+ if NUM_DRONES==1: obs = {"0": {"state": obs}}
+
+ #### Compute control at the desired frequency @@@@@#################################################
+ if i%CTRL_EVERY_N_STEPS==0:
+
+ #### Compute control for the current waypoint ######################################################
+ for j in range(NUM_DRONES): action[str(j)], _, _ = ctrl[j].computeControlFromState(control_timestep=CTRL_EVERY_N_STEPS*env.TIMESTEP, \
+ state=obs[str(j)]["state"], target_pos=TARGET_POS[wp_counters[j],:])
+
+ #### Go to the next waypoint and loop ##############################################################
+ for j in range(NUM_DRONES): wp_counters[j] = wp_counters[j] + 1 if wp_counters[j]<(NUM_WP-1) else 0
+
+ #### Log the simulation ############################################################################
+ for j in range(NUM_DRONES): logger.log(drone=j, timestamp=i/env.SIM_FREQ, state= obs[str(j)]["state"], control=np.hstack([ TARGET_POS[wp_counters[j],:], np.zeros(9) ]))
+
+ #### Printout ######################################################################################
+ env.render()
+
+ #### Sync the simulation ###########################################################################
+ if GUI: sync(i, START, env.TIMESTEP)
+
+ #### Close the environment #########################################################################
+ env.close()
+
+ #### Save the simulation results ###################################################################
+ logger.save()
+
+ #### Plot the simulation results ###################################################################
+ logger.plot()
diff --git a/examples/sa_learning.py b/examples/learn.py
similarity index 55%
rename from examples/sa_learning.py
rename to examples/learn.py
index 2811ea7fd..56eaeec25 100644
--- a/examples/sa_learning.py
+++ b/examples/learn.py
@@ -16,40 +16,35 @@
from ray.rllib.agents import ppo
from utils import *
-from gym_pybullet_drones.envs.SingleDroneEnv import SingleDroneEnv
+from gym_pybullet_drones.envs.Aviary import Aviary
RLLIB = False
if __name__ == "__main__":
- ####################################################################################################
- #### Check out the environment's spaces ############################################################
- ####################################################################################################
- env = gym.make("single-drone-v0")
- print("Action space:", env.action_space)
- print("Observation space:", env.observation_space)
+ #### Check the environment's spaces ################################################################
+ env = gym.make("the-aviary-v1")
+ print("[INFO] Action space:", env.action_space)
+ print("[INFO] Observation space:", env.observation_space)
check_env(env, warn=True, skip_render_check=True)
- ####################################################################################################
#### Train the model ###############################################################################
- ####################################################################################################
if not RLLIB:
model = A2C(MlpPolicy, env, verbose=1)
model.learn(total_timesteps=500000)
else:
config = ppo.DEFAULT_CONFIG.copy()
config["num_workers"] = 0
- agent = ppo.PPOTrainer(config, env="single-drone-v0")
+ agent = ppo.PPOTrainer(config, env="the-aviary-v1")
for i in range(100):
results = agent.train()
- print("{:d}: episode_reward max {:f} min {:f} mean {:f}".format(i, results["episode_reward_max"], results["episode_reward_min"], results["episode_reward_mean"]))
+ print("[INFO] {:d}: episode_reward max {:f} min {:f} mean {:f}".format(i, \
+ results["episode_reward_max"], results["episode_reward_min"], results["episode_reward_mean"]))
policy = agent.get_policy()
print(policy.model.base_model.summary())
- ####################################################################################################
#### Show (and record a video of) the model's performance ##########################################
- ####################################################################################################
- env = SingleDroneEnv(normalized_spaces=True, gui=True, record=True)
+ env = Aviary(normalized_spaces=True, gui=True, record=True)
obs = env.reset()
start = time.time()
for i in range(10*env.SIM_FREQ):
diff --git a/examples/physics.py b/examples/physics.py
new file mode 100644
index 000000000..a1412c43d
--- /dev/null
+++ b/examples/physics.py
@@ -0,0 +1,77 @@
+import os
+import time
+import pdb
+import math
+import numpy as np
+import pybullet as p
+
+from utils import *
+from gym_pybullet_drones.envs.Aviary import DroneModel, Aviary
+
+DURATION_SEC = 60
+NUM_RESETS = 0
+
+if __name__ == "__main__":
+
+ #### Initialize the simulation #####################################################################
+ env = Aviary(drone_model=DroneModel.CF2X, initial_xyzs=np.array([-.7, -.5, .3]).reshape(1,3), \
+ initial_rpys=np.array([0, -30*(np.pi/180), 0]).reshape(1,3), gui=True, obstacles=True); env.reset()
+
+ #### Get PyBullet's and drone's ids ################################################################
+ PYB_CLIENT = env.getPyBulletClient(); DRONE_IDS = env.getDroneIds()
+
+ #### Make the drone weightless #####################################################################
+ p.setGravity(0, 0, 0, physicsClientId=PYB_CLIENT)
+
+ #### Run the simulation ############################################################################
+ START = time.time()
+ for i in range(DURATION_SEC*env.SIM_FREQ):
+
+ #### Apply x-axis force to the base ################################################################
+ # p.applyExternalForce(DRONE_IDS[0], linkIndex=-1, forceObj=[1e-4,0.,0,], posObj=[0.,0.,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
+ # p.applyExternalForce(DRONE_IDS[0], linkIndex=-1, forceObj=[1e-4,0.,0,], posObj=[0.,0.,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
+
+ #### Apply y-axis force to the base ################################################################
+ # p.applyExternalForce(DRONE_IDS[0], linkIndex=-1, forceObj=[0.,1e-4,0.], posObj=[0.,0.,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
+ # p.applyExternalForce(DRONE_IDS[0], linkIndex=-1, forceObj=[0.,1e-4,0.], posObj=[0.,0.,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
+
+ #### Apply z-axis force to the base ################################################################
+ # p.applyExternalForce(DRONE_IDS[0], linkIndex=-1, forceObj=[0.,0.,1e-4], posObj=[0.,0.,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
+ # p.applyExternalForce(DRONE_IDS[0], linkIndex=-1, forceObj=[0.,0.,1e-4], posObj=[0.,0.,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
+ #### To propeller 0 ################################################################################
+ # p.applyExternalForce(DRONE_IDS[0], linkIndex=0, forceObj=[0.,0.,1e-4], posObj=[0.,0.,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
+ p.applyExternalForce(DRONE_IDS[0], linkIndex=0, forceObj=[0.,0.,1e-4], posObj=[0.,0.,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
+ #### To the center of mass #########################################################################
+ # p.applyExternalForce(DRONE_IDS[0], linkIndex=4, forceObj=[0.,0.,1e-4], posObj=[0.,0.,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
+ # p.applyExternalForce(DRONE_IDS[0], linkIndex=4, forceObj=[0.,0.,1e-4], posObj=[0.,0.,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
+
+ #### Apply x-axis torque to the base ###############################################################
+ # p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[5e-6,0.,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
+ # p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[5e-6,0.,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
+
+ #### Apply y-axis torque to the base ###############################################################
+ # p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,5e-6,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
+ # p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,5e-6,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
+
+ #### Apply z-axis torque to the base ###############################################################
+ # p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
+ # p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,0.,5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
+ #### To propeller 0 ################################################################################
+ # p.applyExternalTorque(DRONE_IDS[0], linkIndex=0, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
+ # p.applyExternalTorque(DRONE_IDS[0], linkIndex=0, torqueObj=[0.,0.,5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
+ #### To the center of mass #########################################################################
+ # p.applyExternalTorque(DRONE_IDS[0], linkIndex=4, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
+ p.applyExternalTorque(DRONE_IDS[0], linkIndex=4, torqueObj=[0.,0.,5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
+
+ #### Draw base frame ###############################################################################
+ env._showDroneFrame(0)
+
+ #### Step, printout the state, and sync the simulation #############################################
+ p.stepSimulation(physicsClientId=PYB_CLIENT); env.render(); sync(i, START, env.TIMESTEP)
+
+ #### Reset #########################################################################################
+ if i>1 and i%((DURATION_SEC/(NUM_RESETS+1))*env.SIM_FREQ)==0: env.reset()
+
+ #### Close the environment #########################################################################
+ env.close()
+
diff --git a/examples/sa_flight.py b/examples/sa_flight.py
deleted file mode 100644
index 243b76fbc..000000000
--- a/examples/sa_flight.py
+++ /dev/null
@@ -1,120 +0,0 @@
-import os
-import time
-from datetime import datetime
-import pdb
-import math
-import numpy as np
-import pybullet as p
-import pickle
-import matplotlib.pyplot as plt
-
-from utils import *
-from gym_pybullet_drones.envs.SingleDroneEnv import DroneModel, SingleDroneEnv
-from gym_pybullet_drones.control.SingleDroneControl import ControlType, SingleDroneControl
-
-DRONE = DroneModel.CF2X
-GUI = True
-AERO_EFFECTS = False
-RECORD_VIDEO = True
-SAVE_TO_FILE = True
-SIMULATION_FREQ_HZ = 240
-CONTROL_FREQ_HZ = 48
-DURATION_SEC = 15
-NUM_RESTARTS = 0
-
-if __name__ == "__main__":
-
- ####################################################################################################
- #### Alternatively, initialize a 1m by 1m, 41-waypoint trajectory ##################################
- ####################################################################################################
- height = .4
- zeros = np.zeros(20); ones = -1*np.ones(20); zero2one = np.array([-x/20-0.05 for x in range(20)]); one2zero = np.flip(zero2one)
- waypoints = (np.vstack([np.hstack([0,zero2one,ones,one2zero,zeros]), np.hstack([0,zeros,zero2one,ones,one2zero]),height*np.ones(81)])).transpose()
- wp_counter = 0; current_wp = waypoints[wp_counter]
-
- ####################################################################################################
- #### Initialize the simulation #####################################################################
- ####################################################################################################
- start = time.time()
- start_xyz = [0,0,.35] if DRONE == DroneModel.HB else [0,0,.025]
- env = SingleDroneEnv(drone_model=DRONE, initial_xyz=start_xyz, pybullet=True, aero_effects=AERO_EFFECTS, normalized_spaces=False, freq=SIMULATION_FREQ_HZ, gui=GUI, obstacles=True, record=RECORD_VIDEO)
- initial_state = env.reset()
- action = np.zeros(4); pos_err = 9999.
- control_every_n_steps = int(np.floor(env.SIM_FREQ/CONTROL_FREQ_HZ))
- simulation_data = np.zeros((DURATION_SEC*SIMULATION_FREQ_HZ,16))
- simulation_control_reference = np.zeros((DURATION_SEC*SIMULATION_FREQ_HZ,12))
- simulation_timestamps = np.zeros((DURATION_SEC*SIMULATION_FREQ_HZ,1))
- ####################################################################################################
- #### Setup the controller ##########################################################################
- ####################################################################################################
- ctrl = SingleDroneControl(env, control_type=ControlType.PID)
- for i in range(DURATION_SEC*env.SIM_FREQ):
-
- ####################################################################################################
- #### Step the simulation ##########################################################################
- ####################################################################################################
- state, reward, done, info = env.step(action)
-
- ####################################################################################################
- #### Alternatively, navigate the waypoints in the waypoint list ####################################
- ####################################################################################################
- if i%control_every_n_steps==0:
- target_rpy=np.array([0,0,-45])*env.DEG2RAD
- action, pos_err, yaw_err = ctrl.computeControl(control_timestep=control_every_n_steps*env.TIMESTEP, cur_pos=state[0:3], cur_quat_rpy=state[3:7], cur_vel=state[10:13], cur_ang_vel=state[13:16], \
- target_pos=current_wp) # Waypoints
- # target_pos=np.array([-.5, -1., .8])) # XYZ transfer
- # target_pos=np.array([0., 0., 0.5]), target_rpy=target_rpy) # Hover and yaw
- ####################################################################################################
- #### Go to the next waypoint #######################################################################
- ####################################################################################################
- if np.linalg.norm(pos_err) < 0.05 and np.linalg.norm(state[10:13])<0.1:
- wp_counter = 0 if wp_counter==(len(waypoints)-1) else wp_counter+1
- current_wp = waypoints[wp_counter]
-
- ####################################################################################################
- #### Log the simulation ############################################################################
- ####################################################################################################
- simulation_timestamps[i] = i/env.SIM_FREQ
- simulation_data[i,:] = np.hstack([state[0:3], state[10:13], state[7:10], state[13:20]])
- simulation_control_reference[i,:] = np.hstack([current_wp, np.zeros(3), target_rpy, np.zeros(3)])
-
- ####################################################################################################
- #### Printout and sync #############################################################################
- ####################################################################################################
- env.render()
- if GUI: sync(i, start, env.TIMESTEP)
-
- ####################################################################################################
- #### Reset the simulation ##########################################################################
- ####################################################################################################
- if i>1 and i%((DURATION_SEC/(NUM_RESTARTS+1))*env.SIM_FREQ)==0: initial_state = env.reset(); wp_counter = 0
-
- ####################################################################################################
- #### Close the environment #########################################################################
- ####################################################################################################
- env.close()
-
- ####################################################################################################
- #### Save the simulation results ###################################################################
- ####################################################################################################
- if SAVE_TO_FILE:
- with open(os.path.dirname(os.path.abspath(__file__))+"/../files/saves/save-flight-"+datetime.now().strftime("%m.%d.%Y_%H.%M.%S")+".npy", 'wb') as out_file:
- np.save(out_file, simulation_timestamps); np.save(out_file, simulation_data); np.save(out_file, simulation_control_reference)
-
- ####################################################################################################
- #### Plot the simulation results ###################################################################
- ####################################################################################################
- min_last_step = simulation_data.shape[0]
- simulation_plot_data = simulation_data[:min_last_step,:]
- fig, axs = plt.subplots(8,2)
- t = np.arange(0, DURATION_SEC, 1/SIMULATION_FREQ_HZ)
- labels = ['X', 'Y', 'Z', 'VX', 'VY', 'VZ', 'R', 'P', 'Yaw', 'WR', 'WP', 'WY', 'RPM1', 'RPM2', 'RPM3', 'RPM4']
- for i in range(16):
- axs[i%8,i//8].plot(t, simulation_plot_data[:,i], '--r', label='sim.')
- axs[i%8,i//8].set_xlabel('time')
- axs[i%8,i//8].set_ylabel(labels[i])
- axs[i%8,i//8].grid(True)
- axs[i%8,i//8].legend(loc='upper right', frameon=True)
- fig.subplots_adjust(left=0.06, bottom=0.05, right=0.99, top=0.98, wspace=0.15, hspace=0.0)
- plt.show()
-
diff --git a/examples/sa_trace.py b/examples/sa_trace.py
deleted file mode 100644
index 05f9799cf..000000000
--- a/examples/sa_trace.py
+++ /dev/null
@@ -1,104 +0,0 @@
-import os
-import time
-from datetime import datetime
-import pdb
-import math
-import numpy as np
-import pybullet as p
-import pickle
-import matplotlib.pyplot as plt
-
-from utils import *
-from gym_pybullet_drones.envs.SingleDroneEnv import DroneModel, SingleDroneEnv
-from gym_pybullet_drones.control.SingleDroneControl import ControlType, SingleDroneControl
-
-GUI = False
-RECORD_VIDEO = False
-SAVE_TO_FILE = True
-TRACE_FILE = "trace.pkl"
-
-if __name__ == "__main__":
-
- ####################################################################################################
- #### Load a trace and control reference from a .pkl file ###########################################
- ####################################################################################################
- with open(os.path.dirname(os.path.abspath(__file__))+"/../files/traces/"+TRACE_FILE, 'rb') as in_file:
- trace_timestamps, trace_data, trace_control_reference, _, trace_error, trace_rmse = pickle.load(in_file)
- SIMULATION_FREQ_HZ = int(len(trace_timestamps)/trace_timestamps[-1])
- DURATION_SEC = int(trace_timestamps[-1])
- trace_control_reference = np.array(trace_control_reference)
-
- ####################################################################################################
- #### Initialize the simulation #####################################################################
- ####################################################################################################
- start = time.time()
- env = SingleDroneEnv(drone_model=DroneModel.CF2X, initial_xyz=[0,0,.1], pybullet=True, aero_effects=False, normalized_spaces=False, freq=SIMULATION_FREQ_HZ, gui=GUI, obstacles=False, record=RECORD_VIDEO)
- initial_state = env.reset()
- action = np.zeros(4); pos_err = 9999.
- trace_control_reference[:,2] = initial_state[2] # assuming TRACE_FILE starts at position (0,0,0) while the sim starts at (0,0,initial_state[2])
- simulation_data = np.zeros((DURATION_SEC*SIMULATION_FREQ_HZ,16))
- simulation_control_reference = np.zeros((DURATION_SEC*SIMULATION_FREQ_HZ,12))
- simulation_timestamps = np.zeros((DURATION_SEC*SIMULATION_FREQ_HZ,1))
- ####################################################################################################
- #### Setup the controller ##########################################################################
- ####################################################################################################
- ctrl = SingleDroneControl(env, control_type=ControlType.PID)
- for i in range(DURATION_SEC*env.SIM_FREQ):
-
- ####################################################################################################
- #### Step the simulation ##########################################################################
- ####################################################################################################
- state, reward, done, info = env.step(action)
-
- ####################################################################################################
- #### Compute the next action using the set points from the trace file ##############################
- ####################################################################################################
- action, pos_err, yaw_err = ctrl.computeControl(control_timestep=env.TIMESTEP, cur_pos=state[0:3], cur_quat_rpy=state[3:7], cur_vel=state[10:13], cur_ang_vel=state[13:16], \
- target_pos=trace_control_reference[i,0:3], target_rpy=np.array([0,0,0]), target_vel=trace_control_reference[i,3:6])
-
- ####################################################################################################
- #### Log the simulation ############################################################################
- ####################################################################################################
- simulation_timestamps[i] = i/env.SIM_FREQ
- simulation_data[i,:] = np.hstack([state[0:3], state[10:13], state[7:10], state[13:20]])
- simulation_control_reference[i,:] = np.hstack([trace_control_reference[i,:], np.zeros(3), np.zeros(3)])
-
- ####################################################################################################
- #### Printout and sync #############################################################################
- ####################################################################################################
- env.render()
- if GUI: sync(i, start, env.TIMESTEP)
-
- ####################################################################################################
- #### Close the environment #########################################################################
- ####################################################################################################
- env.close()
-
- ####################################################################################################
- #### Save the simulation results ###################################################################
- ####################################################################################################
- if SAVE_TO_FILE:
- with open(os.path.dirname(os.path.abspath(__file__))+"/../files/saves/save-trace-comparison-"+datetime.now().strftime("%m.%d.%Y_%H.%M.%S")+".npy", 'wb') as out_file:
- np.save(out_file, simulation_timestamps); np.save(out_file, simulation_data); np.save(out_file, simulation_control_reference)
-
- ####################################################################################################
- #### Plot the simulation results ###################################################################
- ####################################################################################################
- min_last_step = min(trace_data.shape[0],simulation_data.shape[0])
- simulation_plot_data = simulation_data[:min_last_step,:]
- trace_plot_data = trace_data[:min_last_step,:]
- fig, axs = plt.subplots(8,2)
- t = np.arange(0, DURATION_SEC, 1/SIMULATION_FREQ_HZ)
- labels = ['X', 'Y', 'Z', 'VX', 'VY', 'VZ', 'R', 'P', 'Yaw', 'WR', 'WP', 'WY', 'PWM1', 'PWM2', 'PWM3', 'PWM4']
- for i in range(16):
- axs[i%8,i//8].plot(t, trace_plot_data[:,i], '-b', label='trace')
- if i > 11: # Comparison to CF2's PWM instead of RPM is specific to file trace.pkl
- simulation_plot_data[:,i] = (simulation_plot_data[:,i] - 4070.3) / 0.2685
- axs[i%8,i//8].plot(t, simulation_plot_data[:,i], '--r', label='sim.')
- axs[i%8,i//8].set_xlabel('time')
- axs[i%8,i//8].set_ylabel(labels[i])
- axs[i%8,i//8].grid(True)
- axs[i%8,i//8].legend(loc='upper right', frameon=True)
- fig.subplots_adjust(left=0.06, bottom=0.05, right=0.99, top=0.98, wspace=0.15, hspace=0.0)
- plt.show()
-
diff --git a/examples/test_physics.py b/examples/test_physics.py
deleted file mode 100644
index 05877ae9b..000000000
--- a/examples/test_physics.py
+++ /dev/null
@@ -1,101 +0,0 @@
-import os
-import time
-import pdb
-import math
-import numpy as np
-import pybullet as p
-
-from utils import *
-from gym_pybullet_drones.envs.SingleDroneEnv import DroneModel, SingleDroneEnv
-
-DURATION_SEC = 60
-NUM_RESTARTS = 0
-
-if __name__ == "__main__":
- start = time.time()
- env = SingleDroneEnv(drone_model=DroneModel.CF2X, initial_xyz=[-.7,-.5,.3], initial_rpy=[0,-30*(np.pi/180),0], gui=True, record=False, obstacles=True)
- env.reset()
- PYB_CLIENT = env.getPyBulletClient()
- DRONE_ID = env.getDroneId()
-
- ####################################################################################################
- #### Make the drone weightless #####################################################################
- ####################################################################################################
- p.setGravity(0, 0, 0, physicsClientId=PYB_CLIENT)
- for i in range(DURATION_SEC*env.SIM_FREQ):
-
- ####################################################################################################
- #### Apply x-axis force to the base ################################################################
- ####################################################################################################
- # p.applyExternalForce(DRONE_ID, linkIndex=-1, forceObj=[1e-4,0.,0,], posObj=[0.,0.,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
- # p.applyExternalForce(DRONE_ID, linkIndex=-1, forceObj=[1e-4,0.,0,], posObj=[0.,0.,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
-
- ####################################################################################################
- #### Apply y-axis force to the base ################################################################
- ####################################################################################################
- # p.applyExternalForce(DRONE_ID, linkIndex=-1, forceObj=[0.,1e-4,0.], posObj=[0.,0.,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
- # p.applyExternalForce(DRONE_ID, linkIndex=-1, forceObj=[0.,1e-4,0.], posObj=[0.,0.,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
-
- ####################################################################################################
- #### Apply z-axis force to the base ################################################################
- ####################################################################################################
- # p.applyExternalForce(DRONE_ID, linkIndex=-1, forceObj=[0.,0.,1e-4], posObj=[0.,0.,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
- # p.applyExternalForce(DRONE_ID, linkIndex=-1, forceObj=[0.,0.,1e-4], posObj=[0.,0.,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
- ####################################################################################################
- #### To propeller 0 ################################################################################
- ####################################################################################################
- # p.applyExternalForce(DRONE_ID, linkIndex=0, forceObj=[0.,0.,1e-4], posObj=[0.,0.,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
- p.applyExternalForce(DRONE_ID, linkIndex=0, forceObj=[0.,0.,1e-4], posObj=[0.,0.,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
- ####################################################################################################
- #### To the center of mass #########################################################################
- ####################################################################################################
- # p.applyExternalForce(DRONE_ID, linkIndex=4, forceObj=[0.,0.,1e-4], posObj=[0.,0.,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
- # p.applyExternalForce(DRONE_ID, linkIndex=4, forceObj=[0.,0.,1e-4], posObj=[0.,0.,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
-
- ####################################################################################################
- #### Apply x-axis torque to the base ###############################################################
- ####################################################################################################
- # p.applyExternalTorque(DRONE_ID, linkIndex=-1, torqueObj=[5e-6,0.,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
- # p.applyExternalTorque(DRONE_ID, linkIndex=-1, torqueObj=[5e-6,0.,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
-
- ####################################################################################################
- #### Apply y-axis torque to the base ###############################################################
- ####################################################################################################
- # p.applyExternalTorque(DRONE_ID, linkIndex=-1, torqueObj=[0.,5e-6,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
- # p.applyExternalTorque(DRONE_ID, linkIndex=-1, torqueObj=[0.,5e-6,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
-
- ####################################################################################################
- #### Apply z-axis torque to the base ###############################################################
- ####################################################################################################
- # p.applyExternalTorque(DRONE_ID, linkIndex=-1, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
- # p.applyExternalTorque(DRONE_ID, linkIndex=-1, torqueObj=[0.,0.,5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
- ####################################################################################################
- #### To propeller 0 ################################################################################
- ####################################################################################################
- # p.applyExternalTorque(DRONE_ID, linkIndex=0, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
- # p.applyExternalTorque(DRONE_ID, linkIndex=0, torqueObj=[0.,0.,5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
- ####################################################################################################
- #### To the center of mass #########################################################################
- ####################################################################################################
- # p.applyExternalTorque(DRONE_ID, linkIndex=4, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
- p.applyExternalTorque(DRONE_ID, linkIndex=4, torqueObj=[0.,0.,5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
-
- ####################################################################################################
- #### Draw base frame ###############################################################################
- ####################################################################################################
- env._showDroneFrame()
-
- ####################################################################################################
- #### Printout, step, and sync ######################################################################
- ####################################################################################################
- env.render()
- p.stepSimulation(physicsClientId=PYB_CLIENT)
- sync(i, start, env.TIMESTEP)
-
- ####################################################################################################
- #### Reset #########################################################################################
- ####################################################################################################
- if i>1 and i%((DURATION_SEC/(NUM_RESTARTS+1))*env.SIM_FREQ)==0: env.reset()
-
- env.close()
-
diff --git a/examples/utils.py b/examples/utils.py
index c68ebee43..270ece7be 100644
--- a/examples/utils.py
+++ b/examples/utils.py
@@ -4,11 +4,11 @@
#### Sync the stepped simulation with the wall-clock ###############################################
####################################################################################################
#### Arguments #####################################################################################
-#### - i (Integer) current step of the simulation #########################
-#### - start_time (Timestamp) timestamp of the simulation start ######################
-#### - timestep (Float) desired, wall-clock timestep to render the simulation ##
+#### - i (int) current simulation iteration ###########################
+#### - start_time (timestamp) timestamp of the simulation start ######################
+#### - timestep (float) desired, wall-clock step of the simulation's rendering #
####################################################################################################
def sync(i, start_time, timestep):
- elapsed = time.time()-start_time
- if elapsed < i*(timestep): time.sleep(i*(timestep)-elapsed)
+ elapsed = time.time() - start_time
+ if elapsed<(i*timestep): time.sleep(timestep*i - elapsed)
diff --git a/files/traces/trace.pkl b/files/example_trace.pkl
similarity index 100%
rename from files/traces/trace.pkl
rename to files/example_trace.pkl
diff --git a/files/readme_images/3_drones.gif b/files/readme_images/3_drones.gif
new file mode 100644
index 000000000..b24ce1c3e
Binary files /dev/null and b/files/readme_images/3_drones.gif differ
diff --git a/files/readme_images/3_drones.png b/files/readme_images/3_drones.png
new file mode 100644
index 000000000..f42ae375c
Binary files /dev/null and b/files/readme_images/3_drones.png differ
diff --git a/files/images/crash.gif b/files/readme_images/crash.gif
similarity index 100%
rename from files/images/crash.gif
rename to files/readme_images/crash.gif
diff --git a/files/images/crash.png b/files/readme_images/crash.png
similarity index 100%
rename from files/images/crash.png
rename to files/readme_images/crash.png
diff --git a/files/images/learn1.gif b/files/readme_images/learn1.gif
similarity index 100%
rename from files/images/learn1.gif
rename to files/readme_images/learn1.gif
diff --git a/files/images/learn2.gif b/files/readme_images/learn2.gif
similarity index 100%
rename from files/images/learn2.gif
rename to files/readme_images/learn2.gif
diff --git a/files/images/learn3.gif b/files/readme_images/learn3.gif
similarity index 100%
rename from files/images/learn3.gif
rename to files/readme_images/learn3.gif
diff --git a/files/images/learn4.gif b/files/readme_images/learn4.gif
similarity index 100%
rename from files/images/learn4.gif
rename to files/readme_images/learn4.gif
diff --git a/files/images/trace_comparison.gif b/files/readme_images/trace_comparison.gif
similarity index 100%
rename from files/images/trace_comparison.gif
rename to files/readme_images/trace_comparison.gif
diff --git a/files/images/trace_comparison.png b/files/readme_images/trace_comparison.png
similarity index 100%
rename from files/images/trace_comparison.png
rename to files/readme_images/trace_comparison.png
diff --git a/files/images/wp.gif b/files/readme_images/wp.gif
similarity index 100%
rename from files/images/wp.gif
rename to files/readme_images/wp.gif
diff --git a/files/images/wp.png b/files/readme_images/wp.png
similarity index 100%
rename from files/images/wp.png
rename to files/readme_images/wp.png
diff --git a/files/saves/foo.txt b/files/saves/foo.txt
deleted file mode 100644
index e69de29bb..000000000
diff --git a/gym_pybullet_drones/__init__.py b/gym_pybullet_drones/__init__.py
index c587cf2ac..15fc405c0 100644
--- a/gym_pybullet_drones/__init__.py
+++ b/gym_pybullet_drones/__init__.py
@@ -1,11 +1,6 @@
from gym.envs.registration import register
register(
- id='single-drone-v0',
- entry_point='gym_pybullet_drones.envs:SingleDroneEnv',
-)
-
-register(
- id='multi-drone-v0',
- entry_point='gym_pybullet_drones.envs:MultiDroneEnv',
+ id='the-aviary-v1',
+ entry_point='gym_pybullet_drones.envs:Aviary',
)
diff --git a/gym_pybullet_drones/assets/cf2p.urdf b/gym_pybullet_drones/assets/cf2p.urdf
index 44cd87109..b93e3af6e 100644
--- a/gym_pybullet_drones/assets/cf2p.urdf
+++ b/gym_pybullet_drones/assets/cf2p.urdf
@@ -2,7 +2,7 @@
-
+
diff --git a/gym_pybullet_drones/assets/cf2x.urdf b/gym_pybullet_drones/assets/cf2x.urdf
index 50699aaae..9615b85b8 100644
--- a/gym_pybullet_drones/assets/cf2x.urdf
+++ b/gym_pybullet_drones/assets/cf2x.urdf
@@ -2,7 +2,7 @@
-
+
diff --git a/gym_pybullet_drones/assets/hb.urdf b/gym_pybullet_drones/assets/hb.urdf
index 6ce818213..d9c54324a 100644
--- a/gym_pybullet_drones/assets/hb.urdf
+++ b/gym_pybullet_drones/assets/hb.urdf
@@ -2,7 +2,7 @@
-
+
diff --git a/gym_pybullet_drones/control/MultiDroneControl.py b/gym_pybullet_drones/control/MultiDroneControl.py
deleted file mode 100644
index 41e23db3e..000000000
--- a/gym_pybullet_drones/control/MultiDroneControl.py
+++ /dev/null
@@ -1,22 +0,0 @@
-import math
-import numpy as np
-import pybullet as p
-from enum import Enum
-from scipy.optimize import nnls
-from scipy.spatial.transform import Rotation
-
-from gym_pybullet_drones.envs.SingleDroneEnv import DroneModel
-from gym_pybullet_drones.envs.MultiDroneEnv import MultiDroneEnv
-from gym_pybullet_drones.control.SingleDroneControl import ControlType
-
-
-######################################################################################################################################################
-#### Control class ###################################################################################################################################
-######################################################################################################################################################
-class MultiDroneControl(object):
-
- ####################################################################################################
- #### Initialize the controller #####################################################################
- ####################################################################################################
- def __init__(self, env: MultiDroneEnv, control_type: ControlType=ControlType.PID):
- pass
\ No newline at end of file
diff --git a/gym_pybullet_drones/control/SingleDroneControl.py b/gym_pybullet_drones/control/SingleDroneControl.py
deleted file mode 100644
index f5f0add15..000000000
--- a/gym_pybullet_drones/control/SingleDroneControl.py
+++ /dev/null
@@ -1,264 +0,0 @@
-import math
-import numpy as np
-import pybullet as p
-from enum import Enum
-from scipy.optimize import nnls
-from scipy.spatial.transform import Rotation
-
-from gym_pybullet_drones.envs.SingleDroneEnv import DroneModel, SingleDroneEnv
-
-
-######################################################################################################################################################
-#### Control types enumeration #######################################################################################################################
-######################################################################################################################################################
-class ControlType(Enum):
- PID = 0 # PID control
-
-
-######################################################################################################################################################
-#### Control class ###################################################################################################################################
-######################################################################################################################################################
-class SingleDroneControl(object):
-
- ####################################################################################################
- #### Initialize the controller #####################################################################
- ####################################################################################################
- #### Arguments #####################################################################################
- #### - env (SingleDroneEnv) simulation environment #####################################
- #### - control_type (ControlType) type of controller #########################################
- ####################################################################################################
- def __init__(self, env: SingleDroneEnv, control_type: ControlType=ControlType.PID):
- ####################################################################################################
- #### Set general use constants #####################################################################
- ####################################################################################################
- self.DRONE_MODEL = env.DRONE_MODEL; self.GRAVITY = env.GRAVITY; self.KF = env.KF; self.KT = env.KM
- self.MAX_THRUST = env.MAX_THRUST; self.MAX_XY_TORQUE = env.MAX_XY_TORQUE; self.MAX_Z_TORQUE = env.MAX_Z_TORQUE
- self.CONTROLTYPE = control_type
- self.A = np.array([ [1, 1, 1, 1], [0, 1, 0, -1], [-1, 0, 1, 0], [-1, 1, -1, 1] ]); self.INV_A = np.linalg.inv(self.A)
- self.B_COEFF = np.array([1/env.KF, 1/(env.KF*env.L), 1/(env.KF*env.L), 1/env.KM])
- self.RAD2DEG = 180/np.pi; self.DEG2RAD = np.pi/180
- ####################################################################################################
- #### Set drone model-specific constants ###########################################################
- ####################################################################################################
- if self.DRONE_MODEL==DroneModel.HB:
- self.P_COEFF_FOR = np.array([.1, .1, .2]); self.I_COEFF_FOR = np.array([.0001, .0001, .0001]); self.D_COEFF_FOR = np.array([.3, .3, .4])
- self.P_COEFF_TOR = np.array([.3, .3, .05]); self.I_COEFF_TOR = np.array([.0001, .0001, .0001]); self.D_COEFF_TOR = np.array([.3, .3, .5])
- self.MAX_ROLL_PITCH = np.pi/6
- elif self.DRONE_MODEL==DroneModel.CF2X or self.DRONE_MODEL==DroneModel.CF2P:
- self.P_COEFF_FOR = np.array([.4, .4, 1.25]); self.I_COEFF_FOR = np.array([.05, .05, .05]); self.D_COEFF_FOR = np.array([.2, .2, .5])
- self.P_COEFF_TOR = np.array([70000., 70000., 60000.]); self.I_COEFF_TOR = np.array([.0, .0, 500.]); self.D_COEFF_TOR = np.array([20000., 20000., 12000.])
- self.PWM2RPM_SCALE = 0.2685; self.PWM2RPM_CONST = 4070.3; self.MIN_PWM = 20000; self.MAX_PWM = 65535
- if self.DRONE_MODEL==DroneModel.CF2X: self.MIXER_MATRIX = np.array([ [.5, -.5, -1], [.5, .5, 1], [-.5, .5, -1], [-.5, -.5, 1] ])
- elif self.DRONE_MODEL==DroneModel.CF2P: self.MIXER_MATRIX = np.array([ [0, -1, -1], [+1, 0, 1], [0, 1, -1], [-1, 0, 1] ])
- self.reset()
-
- ####################################################################################################
- #### Reset the controller ##########################################################################
- ####################################################################################################
- def reset(self):
- self.last_pos_e = np.zeros(3); self.integral_pos_e = np.zeros(3); self.last_rpy_e = np.zeros(3); self.integral_rpy_e = np.zeros(3)
- self.control_counter = 0
-
- ####################################################################################################
- #### Compute the control action ####################################################################
- ####################################################################################################
- #### Arguments #####################################################################################
- #### - control_timestep (Float) timestep at which control is computed ######################
- #### - cur_pos (3-by-1 array) current position ###########################################
- #### - cur_quat_rpy (4-by-1 array) current orientation as a quaternion ########################
- #### - cur_vel (3-by-1 array) current velocity ###########################################
- #### - cur_ang_vel (3-by-1 array) current angular velocity ###################################
- #### - target_pos (3-by-1 array) desired position ###########################################
- #### - target_rpy (3-by-1 array) desired orientation as roll, pitch, yaw ####################
- #### - target_vel (3-by-1 array) desired velocity ###########################################
- #### - target_ang_vel (3-by-1 array) desired angular velocity ###################################
- ####################################################################################################
- #### Returns #######################################################################################
- #### - rpm (4-by-1 array) RPM values to apply to the 4 motors ########################
- #### - pos_err (3-by-1 array) current XYZ position error #################################
- #### - yaw_err (Float) current yaw error ##########################################
- ####################################################################################################
- def computeControl(self, control_timestep, cur_pos, cur_quat_rpy, cur_vel, cur_ang_vel, target_pos, target_rpy=np.zeros(3), target_vel=np.zeros(3), target_ang_vel=np.zeros(3)):
- self.control_counter += 1
- if self.CONTROLTYPE==ControlType.PID:
- ####################################################################################################
- #### XYZ PID control tuned for DroneModel.HB #######################################################
- #### based on https://github.com/prfraanje/quadcopter_sim ##########################################
- ####################################################################################################
- if self.DRONE_MODEL==DroneModel.HB:
- if target_rpy[2] != 0: print("\n[WARNING] ctrl it:", self.control_counter, "in computeControl(), desired yaw={:.0f}deg but locked to 0. for DroneModel.HB".format(target_rpy[2]*self.RAD2DEG))
- thrust, computed_target_rpy, pos_err = self._simplePIDPositionControl(control_timestep, cur_pos, cur_quat_rpy, target_pos)
- rpm = self._simplePIDAttitudeControl(control_timestep, cur_quat_rpy, thrust, computed_target_rpy)
- ####################################################################################################
- #### PID control tuned for Bitcraze's Crazyflie 2.x ################################################
- ####################################################################################################
- elif self.DRONE_MODEL==DroneModel.CF2X or self.DRONE_MODEL==DroneModel.CF2P:
- base_thrust, computed_target_rpy, pos_err = self._dslPIDPositionControl(control_timestep, cur_pos, cur_quat_rpy, cur_vel, target_pos, target_rpy, target_vel)
- rpm = self._dslPIDAttitudeControl(control_timestep, cur_quat_rpy, base_thrust, computed_target_rpy, cur_ang_vel, target_ang_vel)
- cur_rpy = p.getEulerFromQuaternion(cur_quat_rpy)
- return rpm, pos_err, computed_target_rpy[2]-cur_rpy[2]
- else: print("[ERROR] ctrl it:", self.control_counter, "ControlleType not yet implemented")
-
- ####################################################################################################
- #### Generic PID position control (with yaw locked to 0.) ##########################################
- ####################################################################################################
- #### Arguments #####################################################################################
- #### - control_timestep (Float) timestep at which control is computed ######################
- #### - cur_pos (3-by-1 array) current position ###########################################
- #### - cur_quat_rpy (4-by-1 array) current orientation as a quaternion ########################
- #### - target_pos (3-by-1 array) desired position ###########################################
- ####################################################################################################
- #### Returns #######################################################################################
- #### - thrust (Float) thrust along the drone z-axis ##############################
- #### - target_rpy (3-by-1 array) computed target roll, pitch, and yaw #######################
- #### - yaw_err (Float) current yaw error ##########################################
- ####################################################################################################
- def _simplePIDPositionControl(self, control_timestep, cur_pos, cur_quat_rpy, target_pos):
- pos_e = target_pos - np.array(cur_pos).reshape(3)
- d_pos_e = (pos_e - self.last_pos_e) / control_timestep
- self.last_pos_e = pos_e
- self.integral_pos_e = self.integral_pos_e + pos_e*control_timestep
- target_force = np.array([0,0,self.GRAVITY]) + np.multiply(self.P_COEFF_FOR,pos_e) + np.multiply(self.I_COEFF_FOR,self.integral_pos_e) + np.multiply(self.D_COEFF_FOR,d_pos_e)
- computed_target_rpy = np.zeros(3)
- sign_z = np.sign(target_force[2])
- if sign_z == 0: sign_z = 1
- computed_target_rpy[0] = np.arcsin(-sign_z*target_force[1] / np.linalg.norm(target_force))
- computed_target_rpy[1] = np.arctan2(sign_z*target_force[0],sign_z*target_force[2])
- computed_target_rpy[2] = 0.
- computed_target_rpy[0] = np.clip(computed_target_rpy[0], -self.MAX_ROLL_PITCH, self.MAX_ROLL_PITCH)
- computed_target_rpy[1] = np.clip(computed_target_rpy[1], -self.MAX_ROLL_PITCH, self.MAX_ROLL_PITCH)
- cur_rotation = np.array(p.getMatrixFromQuaternion(cur_quat_rpy)).reshape(3,3)
- thrust = np.dot(cur_rotation, target_force)
- return thrust[2], computed_target_rpy, pos_e
-
- ####################################################################################################
- #### Generic PID attiutude control (with yaw locked to 0.) #########################################
- ####################################################################################################
- #### Arguments #####################################################################################
- #### - control_timestep (Float) timestep at which control is computed ######################
- #### - cur_quat_rpy (4-by-1 array) current orientation as a quaternion ########################
- #### - thrust (Float) desired thrust along the drone z-axis ######################
- #### - comp_tar_rpy (3-by-1 array) computed target roll, pitch, and yaw #######################
- ####################################################################################################
- #### Returns #######################################################################################
- #### - rpm (4-by-1 array) RPM values to apply to the 4 motors ########################
- ####################################################################################################
- def _simplePIDAttitudeControl(self, control_timestep, cur_quat_rpy, thrust, computed_target_rpy):
- cur_rpy = p.getEulerFromQuaternion(cur_quat_rpy)
- rpy_e = computed_target_rpy - np.array(cur_rpy).reshape(3,)
- if rpy_e[2] > np.pi: rpy_e[2] = rpy_e[2] - 2*np.pi
- if rpy_e[2] < -np.pi: rpy_e[2] = rpy_e[2] + 2*np.pi
- d_rpy_e = (rpy_e - self.last_rpy_e) / control_timestep
- self.last_rpy_e = rpy_e
- self.integral_rpy_e = self.integral_rpy_e + rpy_e*control_timestep
- target_torques = np.multiply(self.P_COEFF_TOR,rpy_e) + np.multiply(self.I_COEFF_TOR,self.integral_rpy_e) + np.multiply(self.D_COEFF_TOR,d_rpy_e)
- return self._physicsToRPM(thrust, target_torques[0], target_torques[1], target_torques[2])
-
- ####################################################################################################
- #### DSL's CF2.x PID position control ##############################################################
- ####################################################################################################
- #### Arguments #####################################################################################
- #### - control_timestep (Float) timestep at which control is computed ######################
- #### - cur_pos (3-by-1 array) current position ###########################################
- #### - cur_quat_rpy (4-by-1 array) current orientation as a quaternion ########################
- #### - cur_vel (3-by-1 array) current velocity ###########################################
- #### - target_pos (3-by-1 array) desired position ###########################################
- #### - target_rpy (3-by-1 array) desired orientation as roll, pitch, yaw ####################
- #### - target_vel (3-by-1 array) desired velocity ###########################################
- ####################################################################################################
- #### Returns #######################################################################################
- #### - thrust (Float) thrust along the drone z-axis ##############################
- #### - target_rpy (3-by-1 array) target roll, pitch, and yaw ################################
- #### - yaw_err (Float) current yaw error ##########################################
- ####################################################################################################
- def _dslPIDPositionControl(self, control_timestep, cur_pos, cur_quat_rpy, cur_vel, target_pos, target_rpy, target_vel):
- cur_rotation = np.array(p.getMatrixFromQuaternion(cur_quat_rpy)).reshape(3,3)
- pos_err = target_pos - cur_pos
- vel_err = target_vel - cur_vel
- self.integral_pos_e = self.integral_pos_e + pos_err*control_timestep
- self.integral_pos_e = np.clip(self.integral_pos_e, -2., 2.); self.integral_pos_e[2] = np.clip(self.integral_pos_e[2], -0.15, .15)
- target_thrust = np.multiply(self.P_COEFF_FOR, pos_err) + np.multiply(self.I_COEFF_FOR, self.integral_pos_e) + np.multiply(self.D_COEFF_FOR, vel_err) + np.array([0,0,self.GRAVITY])
- scalar_thrust = max(0., np.dot(target_thrust, cur_rotation[:,2]))
- base_thrust = (math.sqrt(scalar_thrust / (4*self.KF)) - self.PWM2RPM_CONST) / self.PWM2RPM_SCALE
- target_z_ax = target_thrust / np.linalg.norm(target_thrust)
- target_x_c = np.array([math.cos(target_rpy[2]), math.sin(target_rpy[2]), 0])
- target_y_ax = np.cross(target_z_ax, target_x_c) / np.linalg.norm(np.cross(target_z_ax, target_x_c))
- target_x_ax = np.cross(target_y_ax, target_z_ax)
- target_rotation = (np.vstack([target_x_ax, target_y_ax, target_z_ax])).transpose()
- target_euler = (Rotation.from_matrix(target_rotation)).as_euler('XYZ', degrees=False)
- if np.any(np.abs(target_euler) > math.pi): print("\n[ERROR] ctrl it:", self.control_counter, "in _dslPositionControl(), values outside range [-pi,pi]")
- return base_thrust, target_euler, pos_err
-
- ####################################################################################################
- #### DSL's CF2.x PID attitude control ##############################################################
- ####################################################################################################
- #### Arguments #####################################################################################
- #### - control_timestep (Float) timestep at which control is computed ######################
- #### - cur_quat_rpy (4-by-1 array) current orientation as a quaternion ########################
- #### - base_thrust (Float) desired thrust along the drone z-axis ######################
- #### - target_euler (3-by-1 array) computed target euler angles ###############################
- #### - cur_ang_vel (3-by-1 array) current angular velocity ###################################
- #### - target_ang_vel (3-by-1 array) desired angular velocity ###################################
- ####################################################################################################
- #### Returns #######################################################################################
- #### - rpm (4-by-1 array) RPM values to apply to the 4 motors ########################
- ####################################################################################################
- def _dslPIDAttitudeControl(self, control_timestep, cur_quat_rpy, base_thrust, target_euler, cur_ang_vel, target_ang_vel):
- cur_rotation = np.array(p.getMatrixFromQuaternion(cur_quat_rpy)).reshape(3,3)
- target_quat = (Rotation.from_euler('XYZ', target_euler, degrees=False)).as_quat()
- w,x,y,z = target_quat
- target_rotation = (Rotation.from_quat([w,x,y,z])).as_matrix()
- rot_matrix_err = np.dot((target_rotation.transpose()),cur_rotation) - np.dot(cur_rotation.transpose(),target_rotation)
- rot_err = np.array([rot_matrix_err[2,1], rot_matrix_err[0,2], rot_matrix_err[1,0]])
- ang_vel_err = target_ang_vel - cur_ang_vel
- self.integral_rpy_e = self.integral_rpy_e - rot_err*control_timestep
- self.integral_rpy_e = np.clip(self.integral_rpy_e, -1500., 1500.); self.integral_rpy_e[0:2] = np.clip(self.integral_rpy_e[0:2], -1., 1.)
- target_torques = - np.multiply(self.P_COEFF_TOR, rot_err) + np.multiply(self.D_COEFF_TOR, ang_vel_err) + np.multiply(self.I_COEFF_TOR, self.integral_rpy_e)
- target_torques = np.clip(target_torques, -3200, 3200)
- motor_pwm = base_thrust + np.dot(self.MIXER_MATRIX, target_torques)
- motor_pwm = np.clip(motor_pwm, self.MIN_PWM, self.MAX_PWM)
- return self.PWM2RPM_SCALE*motor_pwm + self.PWM2RPM_CONST
-
- ####################################################################################################
- #### Non-negative Least Squares (NNLS) RPM from desired thrust and torques ########################
- ####################################################################################################
- #### Arguments #####################################################################################
- #### - thrust (Float) desired thrust along the local z-axis ######################
- #### - x_torque (Float) desired x-axis torque ######################################
- #### - y_torque (Float) desired y-axis torque ######################################
- #### - z_torque (Float) desired z-axis torque ######################################
- ####################################################################################################
- #### Returns #######################################################################################
- #### - rpm (4-by-1 array) RPM values to apply to the 4 motors ########################
- ####################################################################################################
- def _physicsToRPM(self, thrust, x_torque, y_torque, z_torque):
- new_line = True
- if thrust < 0 or thrust > self.MAX_THRUST:
- if new_line: print(); new_line = False
- print("[WARNING] ctrl it:", self.control_counter, "in _physicsToRPM(), unfeasible THRUST {:.2f} outside range [0, {:.2f}]".format(thrust, self.MAX_THRUST))
- if np.abs(x_torque) > self.MAX_XY_TORQUE:
- if new_line: print(); new_line = False
- print("[WARNING] ctrl it:", self.control_counter, "in _physicsToRPM(), unfeasible ROLL torque {:.2f} outside range [{:.2f}, {:.2f}]".format(x_torque, -self.MAX_XY_TORQUE, self.MAX_XY_TORQUE))
- if np.abs(y_torque) > self.MAX_XY_TORQUE:
- if new_line: print(); new_line = False
- print("[WARNING] ctrl it:", self.control_counter, "in _physicsToRPM(), unfeasible PITCH torque {:.2f} outside range [{:.2f}, {:.2f}]".format(y_torque, -self.MAX_XY_TORQUE, self.MAX_XY_TORQUE))
- if np.abs(z_torque) > self.MAX_Z_TORQUE:
- if new_line: print(); new_line = False
- print("[WARNING] ctrl it:", self.control_counter, "in _physicsToRPM(), unfeasible YAW torque {:.2f} outside range [{:.2f}, {:.2f}]".format(z_torque, -self.MAX_Z_TORQUE, self.MAX_Z_TORQUE))
- B = np.multiply(np.array([thrust, x_torque, y_torque, z_torque]), self.B_COEFF)
- sq_rpm = np.dot(self.INV_A, B)
- ####################################################################################################
- #### Use NNLS if any of the desired angular velocities is negative #################################
- ####################################################################################################
- if np.min(sq_rpm) < 0:
- sol, res = nnls(self.A, B, maxiter=3*self.A.shape[1])
- if new_line: print(); new_line = False
- print("[WARNING] ctrl it:", self.control_counter, "in _physicsToRPM(), unfeasible SQ. ROTOR SPEEDS: using NNLS")
- print("Negative sq. rotor speeds:\t [{:.2f}, {:.2f}, {:.2f}, {:.2f}]".format(sq_rpm[0], sq_rpm[1], sq_rpm[2], sq_rpm[3]),
- "\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)))
- print("NNLS:\t\t\t\t [{:.2f}, {:.2f}, {:.2f}, {:.2f}]".format(sol[0], sol[1], sol[2], sol[3]),
- "\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)),
- "\t\tResidual: {:.2f}".format(res) )
- sq_rpm = sol
- return np.sqrt(sq_rpm)
-
diff --git a/gym_pybullet_drones/control/__init__.py b/gym_pybullet_drones/control/__init__.py
deleted file mode 100644
index 95f9a8f4c..000000000
--- a/gym_pybullet_drones/control/__init__.py
+++ /dev/null
@@ -1,2 +0,0 @@
-from gym_pybullet_drones.control.SingleDroneControl import SingleDroneControl
-from gym_pybullet_drones.control.MultiDroneControl import MultiDroneControl
\ No newline at end of file
diff --git a/gym_pybullet_drones/envs/Aviary.py b/gym_pybullet_drones/envs/Aviary.py
new file mode 100644
index 000000000..fa5a9d1a8
--- /dev/null
+++ b/gym_pybullet_drones/envs/Aviary.py
@@ -0,0 +1,489 @@
+import os
+import time
+import curses
+import pdb
+import math
+import numpy as np
+import xml.etree.ElementTree as etxml
+import pybullet as p
+import pybullet_data
+import gym
+from gym import error, spaces, utils
+from gym.utils import seeding
+from enum import Enum
+from datetime import datetime
+
+from gym_pybullet_drones.envs.ProblemSpecificFunctions import Problem, ProblemSpecificFunctions
+
+
+######################################################################################################################################################
+#### Drone models enumeration ########################################################################################################################
+######################################################################################################################################################
+class DroneModel(Enum):
+ CF2X = 0 # Bitcraze Craziflie 2.0 in the X configuration
+ CF2P = 1 # Bitcraze Craziflie 2.0 in the + configuration
+ HB = 2 # Generic quadrotor (with AscTec Hummingbird inertial properties)
+
+
+######################################################################################################################################################
+#### Physics implementations enumeration #############################################################################################################
+######################################################################################################################################################
+class Physics(Enum):
+ PYB = 0 # Base PyBullet physics update
+ DYN = 1 # Update with an explicit model of the dynamics
+ PYB_GND = 2 # PyBullet physics update with ground effect
+ PYB_DRAG = 3 # PyBullet physics update with drag
+ PYB_DW = 4 # PyBullet physics update with downwash
+ PYB_GND_DRAG_DW = 5 # PyBullet physics update with ground effect, drag, and downwash
+ PYB_PM = 6 # Simpler PyBullet physics update with point-mass models
+ PYB_KIN = 7 # Update with a desired kinmatics input
+
+
+######################################################################################################################################################
+#### Multi-drone environment class ###################################################################################################################
+######################################################################################################################################################
+class Aviary(gym.Env):
+ metadata = {'render.modes': ['human']}
+
+ ####################################################################################################
+ #### Initialize the environment ####################################################################
+ ####################################################################################################
+ #### Arguments #####################################################################################
+ #### - drone_model (DroneModel) desired drone type (associated to an .urdf file) ###########
+ #### - num_drones (int) desired number of drones in the aviary #####################
+ #### - visibility_radius (float) used to compute the drones' adjacency matrix, in meters ####
+ #### - initial_xyz ((3,1) array) initial XYZ position of the drones #########################
+ #### - initial_rpy ((3,1) array) initial orientations of the drones (radians) ###############
+ #### - physics (Physics) desired implementation of physics/dynamics #################
+ #### - normalized_spaces (bool) whether to use normalized OpenAI Gym spaces ################
+ #### - freq (int) the freqeuency (Hz) at which the simulation steps ##########
+ #### - gui (bool) whether to use PyBullet's GUI ##############################
+ #### - obstacles (bool) whether to add obstacles to the simulation #################
+ #### - record (bool) whether to save the simulation as an .mp4 video ############
+ #### - problem (Problem) used to select reward, done, and normalization functions ###
+ ####################################################################################################
+ def __init__(self, drone_model: DroneModel=DroneModel.CF2X, num_drones: int=1, \
+ visibility_radius: float=np.inf, initial_xyzs=None, initial_rpys=None, \
+ physics: Physics=Physics.PYB, normalized_spaces=True, freq: int=240, \
+ gui=False, obstacles=False, record=False, problem: Problem=Problem.DEFAULT):
+ super(Aviary, self).__init__()
+ #### Parameters ####################################################################################
+ self.DRONE_MODEL = drone_model; self.NUM_DRONES = num_drones; self.VISIBILITY_RADIUS = visibility_radius
+ self.PHYSICS = physics; self.NORM_SPACES = normalized_spaces
+ #### Constants #####################################################################################
+ self.G = 9.8; self.RAD2DEG = 180/np.pi; self.DEG2RAD = np.pi/180
+ self.SIM_FREQ = freq; self.TIMESTEP = 1./self.SIM_FREQ
+ self.GUI = gui; self.OBSTACLES = obstacles; self.RECORD = record; self.PROBLEM = problem
+ if self.DRONE_MODEL==DroneModel.CF2X: self.URDF = "cf2x.urdf"
+ elif self.DRONE_MODEL==DroneModel.CF2P: self.URDF = "cf2p.urdf"
+ elif self.DRONE_MODEL==DroneModel.HB: self.URDF = "hb.urdf"
+ #### Load the drone properties from the .urdf file #################################################
+ self.M, self.L, self.THRUST2WEIGHT_RATIO, self.J, self.J_INV, self.KF, self.KM, self.COLLISION_H, self.COLLISION_R, self.COLLISION_Z_OFFSET, self.MAX_SPEED_KMH, self.GND_EFF_COEFF, self.PROP_RADIUS, self.DRAG_COEFF = self._loadURDF()
+ print("[INFO] Aviary.__init__() loaded parameters from the drone's .urdf:\n[INFO] m {:f}, L {:f},\n[INFO] ixx {:f}, iyy {:f}, izz {:f},\n[INFO] kf {:f}, km {:f},\n[INFO] t2w {:f}, max_speed_kmh {:f},\n[INFO] gnd_eff_coeff {:f}, prop_radius {:f},\n[INFO] drag_xy_coeff {:f}, drag_z_coeff {:f}".format(\
+ self.M, self.L, self.J[0,0], self.J[1,1], self.J[2,2], self.KF, self.KM, self.THRUST2WEIGHT_RATIO, self.MAX_SPEED_KMH, self.GND_EFF_COEFF, self.PROP_RADIUS, self.DRAG_COEFF[0], self.DRAG_COEFF[2]) )
+ #### Compute constants #############################################################################
+ self.GRAVITY = self.G*self.M; self.HOVER_RPM = np.sqrt(self.GRAVITY/(4*self.KF))
+ self.MAX_RPM = np.sqrt((self.THRUST2WEIGHT_RATIO*self.GRAVITY)/(4*self.KF)); self.MAX_THRUST = (4*self.KF*self.MAX_RPM**2)
+ self.MAX_XY_TORQUE = (self.L*self.KF*self.MAX_RPM**2); self.MAX_Z_TORQUE = (2*self.KM*self.MAX_RPM**2)
+ self.MAX_A = self.MAX_THRUST/self.M; self.MAX_PYB_KIN_V = self.MAX_SPEED_KMH*(1000/60**2)
+ #### Connect to PyBullet ###########################################################################
+ if self.GUI:
+ self.CLIENT = p.connect(p.GUI)
+ for i in [p.COV_ENABLE_RGB_BUFFER_PREVIEW, p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW]: p.configureDebugVisualizer(i, 0, physicsClientId=self.CLIENT)
+ p.resetDebugVisualizerCamera(cameraDistance=3, cameraYaw=-30, cameraPitch=-30, cameraTargetPosition=[0,0,0], physicsClientId=self.CLIENT)
+ #### Add input sliders to the GUI ##################################################################
+ self.SLIDERS = -1*np.ones(4)
+ for i in range(4): self.SLIDERS[i] = p.addUserDebugParameter("Propeller "+str(i)+" RPM", 0, self.MAX_RPM, self.HOVER_RPM, physicsClientId=self.CLIENT)
+ self.INPUT_SWITCH = p.addUserDebugParameter("Use GUI RPM", 9999, -1, 0, physicsClientId=self.CLIENT)
+ else: self.CLIENT = p.connect(p.DIRECT)
+ #### Set initial poses #############################################################################
+ if initial_xyzs is None: self.INIT_XYZS = np.hstack([ np.array([x*4*self.L for x in range(self.NUM_DRONES)]), np.array([y*4*self.L for y in range(self.NUM_DRONES)]), np.ones(self.NUM_DRONES) * (self.COLLISION_H/2-self.COLLISION_Z_OFFSET+.1) ]).reshape(self.NUM_DRONES,3)
+ elif np.array(initial_xyzs).shape==(self.NUM_DRONES,3): self.INIT_XYZS = initial_xyzs
+ else: print("[ERROR] invalid initial_xyzs in Aviary.__init__(), try initial_xyzs.reshape(NUM_DRONES,3)")
+ if initial_rpys is None: self.INIT_RPYS = np.zeros((self.NUM_DRONES,3))
+ elif np.array(initial_rpys).shape==(self.NUM_DRONES,3): self.INIT_RPYS = initial_rpys
+ else: print("[ERROR] invalid initial_rpys in Aviary.__init__(), try initial_rpys.reshape(NUM_DRONES,3)")
+ #### Create action and observation spaces ##########################################################
+ if self.NORM_SPACES:
+ #### Set bounds for normalized spaces ##############################################################
+ act_lower_bound = np.array([-1, -1, -1, -1])
+ act_upper_bound = np.array([1, 1, 1, 1])
+ 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])
+ 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])
+ else:
+ ##### Set bounds for unnormalized spaces ############################################################
+ if self.PHYSICS in [Physics.PYB_PM, Physics.PYB_KIN]:
+ norm_upper_bound = self.MAX_A if self.PHYSICS==Physics.PYB_PM else self.MAX_PYB_KIN_V
+ #### Action vector A ###### X Y Z Norm
+ act_lower_bound = np.array([-1, -1, -1, 0.])
+ act_upper_bound = np.array([1, 1, 1, norm_upper_bound])
+ else:
+ #### Action vector B ###### P0 P1 P2 P3
+ act_lower_bound = np.array([0., 0., 0., 0.])
+ act_upper_bound = np.array([self.MAX_RPM, self.MAX_RPM, self.MAX_RPM, self.MAX_RPM])
+ #### Observation vector ### X Y Z Q1 Q2 Q3 Q4 R P Y VX VY VZ WR WP WY P0 P1 P2 P3
+ obs_lower_bound = np.array([-np.inf, -np.inf, 0., -1., -1., -1., -1., -np.pi, -np.pi, -np.pi, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, 0., 0., 0., 0.])
+ obs_upper_bound = np.array([np.inf, np.inf, np.inf, 1., 1., 1., 1., np.pi, np.pi, np.pi, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf, self.MAX_RPM, self.MAX_RPM, self.MAX_RPM, self.MAX_RPM])
+ if self.NUM_DRONES==1:
+ ##### Use simpler Box spaces for a single drone ####################################################
+ self.action_space = spaces.Box( low=act_lower_bound, high=act_upper_bound, dtype=np.float32 )
+ self.observation_space = spaces.Box( low=obs_lower_bound, high=obs_upper_bound, dtype=np.float32 )
+ else:
+ ##### Use nested Dict spaces for multiple drones ###################################################
+ self.action_space = spaces.Dict({ str(i): spaces.Box(low=act_lower_bound, high=act_upper_bound, dtype=np.float32) for i in range(self.NUM_DRONES) })
+ self.observation_space = spaces.Dict({ str(i): spaces.Dict ({"state": spaces.Box(low=obs_lower_bound, high=obs_upper_bound, dtype=np.float32), \
+ "neighbors": spaces.MultiBinary(self.NUM_DRONES) }) for i in range(self.NUM_DRONES) })
+
+ #### Housekeeping ##################################################################################
+ self._housekeeping()
+
+ ####################################################################################################
+ #### Reset the environment #########################################################################
+ ####################################################################################################
+ #### Returns #######################################################################################
+ #### - obs (..) initial observation, Dict() for multidrones, Box() for 1 ###
+ ####################################################################################################
+ def reset(self):
+ p.resetSimulation(physicsClientId=self.CLIENT)
+ #### Housekeeping ##################################################################################
+ self._housekeeping()
+ #### Start video recording #########################################################################
+ if self.RECORD: self.VIDEO_ID = p.startStateLogging(loggingType=p.STATE_LOGGING_VIDEO_MP4, fileName=os.path.dirname(os.path.abspath(__file__))+"/../../files/video-"+datetime.now().strftime("%m.%d.%Y_%H.%M.%S")+".mp4", physicsClientId=self.CLIENT)
+ #### Return the initial observation ################################################################
+ if self.NUM_DRONES==1: return self._getDroneState(0)
+ else:
+ adjacency_mat = self.getAdjacencyMatrix()
+ return {str(i): {"state": self._getDroneState(i), "neighbors": adjacency_mat[i,:] } for i in range(self.NUM_DRONES) }
+
+ ####################################################################################################
+ #### Advance the environment by one simulation step ################################################
+ ####################################################################################################
+ #### Arguments #####################################################################################
+ #### - action (..) motors' speed (or commanded a, v), as Dict() or Box() ######
+ ####################################################################################################
+ #### Returns #######################################################################################
+ #### - obs (..) initial observation, Dict() for multidrones, Box() for 1 ###
+ #### - reward (..) reward value, Dict() for multidrones, float for 1 ##########
+ #### - done (..) whether the current episode is over, Dict() or bool ########
+ #### - info (Dict) currently unused ###########################################
+ ####################################################################################################
+ def step(self, action):
+ self.step_counter += 1
+ #### Read the GUI's input parameters ###############################################################
+ if self.GUI:
+ current_input_switch = p.readUserDebugParameter(self.INPUT_SWITCH, physicsClientId=self.CLIENT)
+ if current_input_switch>self.last_input_switch:
+ self.last_input_switch = current_input_switch
+ self.USE_GUI_RPM = True if self.USE_GUI_RPM==False else False
+ if self.USE_GUI_RPM:
+ for i in range(4): self.gui_input[i] = p.readUserDebugParameter(int(self.SLIDERS[i]), physicsClientId=self.CLIENT)
+ clipped_action = np.tile(self.gui_input,(self.NUM_DRONES,1))
+ if self.step_counter%(self.SIM_FREQ/2)==0: self.GUI_INPUT_TEXT = [ p.addUserDebugText("Using GUI RPM", textPosition=[0,0,0], textColorRGB=[1,0,0], lifeTime=1, textSize=2, parentObjectUniqueId=self.DRONE_IDS[i], parentLinkIndex=-1, replaceItemUniqueId=int(self.GUI_INPUT_TEXT[i]), physicsClientId=self.CLIENT) for i in range(self.NUM_DRONES) ]
+ #### Denormalize (if necessary) and clip the action to the maximum RPM #############################
+ else:
+ #### Transform 1-drone action into the Dict format of multiple drones to simplify the code #########
+ if self.NUM_DRONES==1: action = {"0": action}
+ clipped_action = np.zeros((self.NUM_DRONES,4))
+ for k, v in action.items():
+ self.last_action[int(k),:] = v
+ #### Manage commanded acceleration #################################################################
+ if self.PHYSICS==Physics.PYB_PM:
+ acc = self._normActionToAcceleration(action) if self.NORM_SPACES else np.array(action)
+ clipped_action[int(k),:] = acc # TODO implement clipping
+ #### Manage commanded velocity #####################################################################
+ elif self.PHYSICS==Physics.PYB_KIN:
+ vel = self._normActionToVelocity(action) if self.NORM_SPACES else np.array(action)
+ clipped_action[int(k),:] = vel # TODO implement clipping
+ #### Manage commanded RPM ##########################################################################
+ else:
+ rpm = self._normActionToRPM(v) if self.NORM_SPACES else np.array(v)
+ clipped_action[int(k),:] = np.clip(np.array(rpm), 0, self.MAX_RPM)
+ #### Step the simulation using the desired physics update ##########################################
+ for i in range (self.NUM_DRONES):
+ self._showDroneFrame(i)
+ if self.PHYSICS==Physics.PYB: self._physics(clipped_action[i,:], i)
+ elif self.PHYSICS==Physics.DYN: self._dynamics(clipped_action[i,:], i)
+ elif self.PHYSICS==Physics.PYB_GND: self._physics(clipped_action[i,:], i); self._groundEffect(clipped_action[i,:], i)
+ elif self.PHYSICS==Physics.PYB_DRAG: self._physics(clipped_action[i,:], i); self._drag(self.last_clipped_action[i,:], i)
+ elif self.PHYSICS==Physics.PYB_DW: self._physics(clipped_action[i,:], i); self._downwash(i)
+ elif self.PHYSICS==Physics.PYB_GND_DRAG_DW: self._physics(clipped_action[i,:], i); self._groundEffect(clipped_action[i,:], i); self._drag(self.last_clipped_action[i,:], i); self._downwash(i)
+ elif self.PHYSICS==Physics.PYB_PM: self._pointMass(clipped_action[i,:], i)
+ elif self.PHYSICS==Physics.PYB_KIN: self._kinematics(clipped_action[i,:], i)
+ #### Let PyBullet compute the new state, unless using Physics.DYN ##################################
+ if self.PHYSICS!=Physics.DYN: p.stepSimulation(physicsClientId=self.CLIENT)
+ #### Save the last applied action to compute drag in the next step #################################
+ if self.PHYSICS in [Physics.PYB_DRAG, Physics.PYB_GND_DRAG_DW]: self.last_clipped_action = clipped_action
+ #### Prepare the return values #####################################################################
+ if self.NUM_DRONES==1:
+ obs = self._getDroneState(0)
+ reward = self.PROBLEM_SPECIFIC_FUNCTIONS.rewardFunction(obs)
+ done = self.PROBLEM_SPECIFIC_FUNCTIONS.doneFunction(obs, self.step_counter/self.SIM_FREQ)
+ else:
+ adjacency_mat = self.getAdjacencyMatrix()
+ obs = {str(i): {"state": self._getDroneState(i), "neighbors": adjacency_mat[i,:] } for i in range(self.NUM_DRONES) }
+ reward = 0. # TODO, self.PROBLEM_SPECIFIC_FUNCTIONS.rewardFunction(obs)
+ done = False # TODO, self.PROBLEM_SPECIFIC_FUNCTIONS.doneFunction(obs, self.step_counter/self.SIM_FREQ)
+ return obs, reward, done, {}
+
+ ####################################################################################################
+ #### Print a textual output of the environment #####################################################
+ ####################################################################################################
+ def render(self, mode='human', close=False):
+ if self.first_render_call and not self.GUI:
+ print("[WARNING] Aviary.render() is implemented as text-only, re-initialize the environment using Aviary(gui=True) to use PyBullet's graphical interface")
+ self.first_render_call = False
+ print("\n[INFO] Aviary.render() ——— it {:04d}".format(self.step_counter),
+ "——— wall-clock time {:.1f}s,".format(time.time()-self.RESET_TIME),
+ "simulation time {:.1f}s@{:d}Hz ({:.2f}x)".format(self.step_counter*self.TIMESTEP, self.SIM_FREQ, (self.step_counter*self.TIMESTEP)/(time.time()-self.RESET_TIME)))
+ for i in range (self.NUM_DRONES):
+ pos, quat = p.getBasePositionAndOrientation(self.DRONE_IDS[i], physicsClientId=self.CLIENT)
+ rpy = p.getEulerFromQuaternion(quat)
+ vel, ang_v = p.getBaseVelocity(self.DRONE_IDS[i], physicsClientId=self.CLIENT)
+ print("[INFO] Aviary.render() ——— drone {:d}".format(i),
+ "——— x {:+06.2f}, y {:+06.2f}, z {:+06.2f}".format(pos[0], pos[1], pos[2]),
+ "——— velocity {:+06.2f}, {:+06.2f}, {:+06.2f}".format(vel[0], vel[1], vel[2]),
+ "——— roll {:+06.2f}, pitch {:+06.2f}, yaw {:+06.2f}".format(rpy[0]*self.RAD2DEG, rpy[1]*self.RAD2DEG, rpy[2]*self.RAD2DEG),
+ "——— angular velocities {:+06.2f}, {:+06.2f}, {:+06.2f} ——— ".format(ang_v[0]*self.RAD2DEG, ang_v[1]*self.RAD2DEG, ang_v[2]*self.RAD2DEG))
+
+ ####################################################################################################
+ #### Close the environment #########################################################################
+ ####################################################################################################
+ def close(self):
+ if self.RECORD: p.stopStateLogging(self.VIDEO_ID, physicsClientId=self.CLIENT)
+ p.disconnect(physicsClientId=self.CLIENT)
+
+ ####################################################################################################
+ #### Return the PyBullet Client Id #################################################################
+ ####################################################################################################
+ def getPyBulletClient(self):
+ return self.CLIENT
+
+ ####################################################################################################
+ #### Return the Drone Id ###########################################################################
+ ####################################################################################################
+ def getDroneIds(self):
+ return self.DRONE_IDS
+
+ ####################################################################################################
+ #### Compute the adjacency matrix of a multidrone system using VISIBILITY_RADIUS ###################
+ ####################################################################################################
+ #### Returns #######################################################################################
+ #### - adj_mat ((NUM_DRONES,NUM_DRONES) array) adj_mat[i,j]=1 if i,j are neighbors, 0 otherwise #
+ ####################################################################################################
+ def getAdjacencyMatrix(self):
+ adjacency_mat = np.identity(self.NUM_DRONES)
+ for i in range(self.NUM_DRONES-1):
+ for j in range(self.NUM_DRONES-i-1):
+ pos_i, _ = p.getBasePositionAndOrientation(self.DRONE_IDS[i], physicsClientId=self.CLIENT)
+ pos_j, _ = p.getBasePositionAndOrientation(self.DRONE_IDS[j+i+1], physicsClientId=self.CLIENT)
+ if np.linalg.norm(np.array(pos_i)-np.array(pos_j))1: print("\n[ERROR] it", self.step_counter, "in Aviary._normActionToRPM(), out-of-bound action")
+ return np.where(action <= 0, (action+1)*self.HOVER_RPM, action*self.MAX_RPM)
+
+ ####################################################################################################
+ #### Normalize the [0, MAX RPM] range to the [-1,1] range ##########################################
+ ####################################################################################################
+ #### Arguments #####################################################################################
+ #### - rpm ((4,1) array) RPM values of the 4 motors #################################
+ ####################################################################################################
+ #### Returns #######################################################################################
+ #### - action ((4,1) array) normalized action to apply to the 4 motors #################
+ ####################################################################################################
+ def _rpmToNormAction(self, rpm):
+ if np.any(rpm)<0: print("\n[ERROR] it", self.step_counter, "in Aviary._rpmToNormAction(), negative RPM")
+ return np.where(rpm <= self.HOVER_RPM, (rpm/self.HOVER_RPM)-1, rpm/self.MAX_RPM)
+
+ ####################################################################################################
+ #### Load parameters from the .urdf file ###########################################################
+ ####################################################################################################
+ def _loadURDF(self):
+ URDF_TREE = etxml.parse(os.path.dirname(os.path.abspath(__file__))+"/../assets/"+self.URDF).getroot()
+ M = float(URDF_TREE[1][0][1].attrib['value']); L = float(URDF_TREE[0].attrib['arm']); THRUST2WEIGHT_RATIO = float(URDF_TREE[0].attrib['thrust2weight'])
+ IXX = float(URDF_TREE[1][0][2].attrib['ixx']); IYY = float(URDF_TREE[1][0][2].attrib['iyy']); IZZ = float(URDF_TREE[1][0][2].attrib['izz'])
+ J = np.diag([IXX, IYY, IZZ]); J_INV = np.linalg.inv(J); KF = float(URDF_TREE[0].attrib['kf']); KM = float(URDF_TREE[0].attrib['km'])
+ COLLISION_H = float(URDF_TREE[1][2][1][0].attrib['length']); COLLISION_R = float(URDF_TREE[1][2][1][0].attrib['radius'])
+ COLLISION_SHAPE_OFFSETS = [float(s) for s in URDF_TREE[1][2][0].attrib['xyz'].split(' ')]; COLLISION_Z_OFFSET = COLLISION_SHAPE_OFFSETS[2]
+ MAX_SPEED_KMH = float(URDF_TREE[0].attrib['max_speed_kmh']); GND_EFF_COEFF = float(URDF_TREE[0].attrib['gnd_eff_coeff']); PROP_RADIUS = float(URDF_TREE[0].attrib['prop_radius'])
+ DRAG_COEFF_XY = float(URDF_TREE[0].attrib['drag_coeff_xy']); DRAG_COEFF_Z = float(URDF_TREE[0].attrib['drag_coeff_z']); DRAG_COEFF = np.array([DRAG_COEFF_XY, DRAG_COEFF_XY, DRAG_COEFF_Z])
+ return M, L, THRUST2WEIGHT_RATIO, J, J_INV, KF, KM, COLLISION_H, COLLISION_R, COLLISION_Z_OFFSET, MAX_SPEED_KMH, GND_EFF_COEFF, PROP_RADIUS, DRAG_COEFF
+
+ ####################################################################################################
+ #### Housekeeping shared by the __init__() and reset() functions ###################################
+ ####################################################################################################
+ def _housekeeping(self):
+ #### Initialize/reset counters and zero-valued variables ###########################################
+ self.RESET_TIME = time.time(); self.step_counter = 0; self.first_render_call = True
+ self.X_AX = -1*np.ones(self.NUM_DRONES); self.Y_AX = -1*np.ones(self.NUM_DRONES); self.Z_AX = -1*np.ones(self.NUM_DRONES);
+ self.GUI_INPUT_TEXT = -1*np.ones(self.NUM_DRONES); self.USE_GUI_RPM=False; self.last_input_switch = 0
+ self.PROBLEM_SPECIFIC_FUNCTIONS = ProblemSpecificFunctions(self.CLIENT, self.GUI, self.PROBLEM)
+ self.last_action = -1*np.ones((self.NUM_DRONES,4)) if self.NORM_SPACES else np.zeros((self.NUM_DRONES,4))
+ self.last_clipped_action = np.zeros((self.NUM_DRONES,4)); self.gui_input = np.zeros(4)
+ self.no_pybullet_dyn_accs = np.zeros((self.NUM_DRONES,3));
+ #### Set PyBullet's parameters #####################################################################
+ p.setGravity(0, 0, -self.G, physicsClientId=self.CLIENT)
+ p.setRealTimeSimulation(0, physicsClientId=self.CLIENT)
+ p.setTimeStep(self.TIMESTEP, physicsClientId=self.CLIENT)
+ p.setAdditionalSearchPath(pybullet_data.getDataPath(), physicsClientId=self.CLIENT)
+ #### Load ground plane, drone and obstacles models #################################################
+ p.loadURDF("plane.urdf", physicsClientId=self.CLIENT)
+ self.DRONE_IDS = np.array([p.loadURDF(os.path.dirname(os.path.abspath(__file__))+"/../assets/"+self.URDF, self.INIT_XYZS[i,:], p.getQuaternionFromEuler(self.INIT_RPYS[i,:]), physicsClientId=self.CLIENT) for i in range(self.NUM_DRONES)])
+ if self.OBSTACLES: self.PROBLEM_SPECIFIC_FUNCTIONS.addObstacles()
+
+ ####################################################################################################
+ #### Return the state vector of the nth drone ######################################################
+ ####################################################################################################
+ #### Arguments #####################################################################################
+ #### - nth_drone (int) order position of the drone in list self.DRONE_IDS #########
+ ####################################################################################################
+ #### Returns #######################################################################################
+ #### - state (Box (20,)) the state vector of the nth drone ##########################
+ ####################################################################################################
+ def _getDroneState(self, nth_drone):
+ pos, quat = p.getBasePositionAndOrientation(self.DRONE_IDS[nth_drone], physicsClientId=self.CLIENT)
+ rpy = p.getEulerFromQuaternion(quat)
+ vel, ang_v = p.getBaseVelocity(self.DRONE_IDS[nth_drone], physicsClientId=self.CLIENT)
+ state = np.hstack([pos, quat, rpy, vel, ang_v, self.last_action[nth_drone,:]])
+ if self.NORM_SPACES: state = self.PROBLEM_SPECIFIC_FUNCTIONS.clipAndNormalizeState(state, self.step_counter)
+ return state.reshape(20,)
+
+ ####################################################################################################
+ #### Draw the local frame of the nth drone #########################################################
+ ####################################################################################################
+ #### Arguments #####################################################################################
+ #### - nth_drone (int) order position of the drone in list self.DRONE_IDS #########
+ ####################################################################################################
+ def _showDroneFrame(self, nth_drone):
+ AXIS_LENGTH = 2*self.L
+ self.X_AX[nth_drone] = p.addUserDebugLine(lineFromXYZ=[0,0,0], lineToXYZ=[AXIS_LENGTH,0,0], lineColorRGB=[1,0,0], parentObjectUniqueId=self.DRONE_IDS[nth_drone], parentLinkIndex=-1, replaceItemUniqueId=int(self.X_AX[nth_drone]), physicsClientId=self.CLIENT)
+ self.Y_AX[nth_drone] = p.addUserDebugLine(lineFromXYZ=[0,0,0], lineToXYZ=[0,AXIS_LENGTH,0], lineColorRGB=[0,1,0], parentObjectUniqueId=self.DRONE_IDS[nth_drone], parentLinkIndex=-1, replaceItemUniqueId=int(self.Y_AX[nth_drone]), physicsClientId=self.CLIENT)
+ self.Z_AX[nth_drone] = p.addUserDebugLine(lineFromXYZ=[0,0,0], lineToXYZ=[0,0,AXIS_LENGTH], lineColorRGB=[0,0,1], parentObjectUniqueId=self.DRONE_IDS[nth_drone], parentLinkIndex=-1, replaceItemUniqueId=int(self.Z_AX[nth_drone]), physicsClientId=self.CLIENT)
+
+ ####################################################################################################
+ #### Base PyBullet physics implementation ##########################################################
+ ####################################################################################################
+ #### Arguments #####################################################################################
+ #### - rpm ((4,1) array) RPM values of the 4 motors #################################
+ #### - nth_drone (int) order position of the drone in list self.DRONE_IDS #########
+ ####################################################################################################
+ def _physics(self, rpm, nth_drone):
+ forces = np.array(rpm**2)*self.KF
+ torques = np.array(rpm**2)*self.KM
+ z_torque = (-torques[0] + torques[1] - torques[2] + torques[3])
+ for i in range(4): p.applyExternalForce(self.DRONE_IDS[nth_drone], i, forceObj=[0,0,forces[i]], posObj=[0,0,0], flags=p.LINK_FRAME, physicsClientId=self.CLIENT)
+ p.applyExternalTorque(self.DRONE_IDS[nth_drone], 4, torqueObj=[0,0,z_torque], flags=p.LINK_FRAME, physicsClientId=self.CLIENT)
+
+ ####################################################################################################
+ #### PyBullet implementation of ground effect, from (Shi et al., 2019) #############################
+ ####################################################################################################
+ #### Arguments #####################################################################################
+ #### - rpm ((4,1) array) RPM values of the 4 motors #################################
+ #### - nth_drone (int) order position of the drone in list self.DRONE_IDS #########
+ ####################################################################################################
+ def _groundEffect(self, rpm, nth_drone):
+ #### Kinematic information of the base and all links (propellers and center of mass) ###############
+ base_pos, base_quat = p.getBasePositionAndOrientation(self.DRONE_IDS[nth_drone], physicsClientId=self.CLIENT)
+ base_rpy = p.getEulerFromQuaternion(base_quat)
+ link_states = np.array(p.getLinkStates(self.DRONE_IDS[nth_drone], linkIndices=[0,1,2,3,4], computeLinkVelocity=1, computeForwardKinematics=1, physicsClientId=self.CLIENT))
+ #### Simple, per-propeller ground effects ##########################################################
+ prop_heights = np.array([link_states[0,0][2], link_states[1,0][2], link_states[2,0][2], link_states[3,0][2]])
+ gnd_effects = np.array(rpm**2) * self.KF * self.GND_EFF_COEFF * (self.PROP_RADIUS/(4 * prop_heights))**2
+ gnd_effects = np.clip(gnd_effects, 0, self.MAX_THRUST/10)
+ if np.abs(base_rpy[0])math.pi): print("\n[ERROR] ctrl it", self.control_counter, "in Control._dslPIDPositionControl(), values outside range [-pi,pi]")
+ return thrust, target_euler, pos_e
+
+ ####################################################################################################
+ #### DSL's CF2.x PID attitude control ##############################################################
+ ####################################################################################################
+ #### Arguments #####################################################################################
+ #### - control_timestep (float) timestep at which control is computed ######################
+ #### - thrust (float) desired thrust along the drone z-axis ######################
+ #### - cur_quat ((4,1) array) current orientation as a quaternion ########################
+ #### - cur_ang_vel ((3,1) array) current angular velocity ###################################
+ #### - target_euler ((3,1) array) computed target euler angles ###############################
+ #### - target_ang_vel ((3,1) array) desired angular velocity ###################################
+ ####################################################################################################
+ #### Returns #######################################################################################
+ #### - rpm ((4,1) array) RPM values to apply to the 4 motors ########################
+ ####################################################################################################
+ def _dslPIDAttitudeControl(self, control_timestep, thrust, cur_quat, cur_ang_vel, target_euler, target_ang_vel):
+ cur_rotation = np.array(p.getMatrixFromQuaternion(cur_quat)).reshape(3,3)
+ target_quat = (Rotation.from_euler('XYZ', target_euler, degrees=False)).as_quat()
+ w,x,y,z = target_quat
+ target_rotation = (Rotation.from_quat([w,x,y,z])).as_matrix()
+ rot_matrix_e = np.dot((target_rotation.transpose()),cur_rotation) - np.dot(cur_rotation.transpose(),target_rotation)
+ rot_e = np.array([rot_matrix_e[2,1], rot_matrix_e[0,2], rot_matrix_e[1,0]])
+ ang_vel_e = target_ang_vel - cur_ang_vel
+ self.integral_rpy_e = self.integral_rpy_e - rot_e*control_timestep
+ self.integral_rpy_e = np.clip(self.integral_rpy_e, -1500., 1500.); self.integral_rpy_e[0:2] = np.clip(self.integral_rpy_e[0:2], -1., 1.)
+ #### PID target torques ############################################################################
+ target_torques = - np.multiply(self.P_COEFF_TOR, rot_e) + np.multiply(self.D_COEFF_TOR, ang_vel_e) + np.multiply(self.I_COEFF_TOR, self.integral_rpy_e)
+ target_torques = np.clip(target_torques, -3200, 3200)
+ pwm = thrust + np.dot(self.MIXER_MATRIX, target_torques)
+ pwm = np.clip(pwm, self.MIN_PWM, self.MAX_PWM)
+ return self.PWM2RPM_SCALE * pwm + self.PWM2RPM_CONST
+
+ ####################################################################################################
+ #### Generic PID position control (with yaw locked to 0.) ##########################################
+ ####################################################################################################
+ #### Arguments #####################################################################################
+ #### - control_timestep (float) timestep at which control is computed ######################
+ #### - cur_pos ((3,1) array) current position ###########################################
+ #### - cur_quat ((4,1) array) current orientation as a quaternion ########################
+ #### - target_pos ((3,1) array) desired position ###########################################
+ ####################################################################################################
+ #### Returns #######################################################################################
+ #### - thrust (float) thrust along the drone z-axis ##############################
+ #### - target_rpy ((3,1) array) computed target roll, pitch, and yaw #######################
+ #### - yaw_e (float) current yaw error ##########################################
+ ####################################################################################################
+ def _simplePIDPositionControl(self, control_timestep, cur_pos, cur_quat, target_pos):
+ pos_e = target_pos - np.array(cur_pos).reshape(3)
+ d_pos_e = (pos_e - self.last_pos_e) / control_timestep
+ self.last_pos_e = pos_e
+ self.integral_pos_e = self.integral_pos_e + pos_e*control_timestep
+ #### PID target thrust #############################################################################
+ target_force = np.array([0,0,self.GRAVITY]) + np.multiply(self.P_COEFF_FOR,pos_e) + np.multiply(self.I_COEFF_FOR,self.integral_pos_e) + np.multiply(self.D_COEFF_FOR,d_pos_e)
+ target_rpy = np.zeros(3)
+ sign_z = np.sign(target_force[2])
+ if sign_z==0: sign_z = 1
+ #### Target rotation ###############################################################################
+ target_rpy[0] = np.arcsin(-sign_z*target_force[1] / np.linalg.norm(target_force))
+ target_rpy[1] = np.arctan2(sign_z*target_force[0], sign_z*target_force[2])
+ target_rpy[2] = 0.
+ target_rpy[0] = np.clip(target_rpy[0], -self.MAX_ROLL_PITCH, self.MAX_ROLL_PITCH)
+ target_rpy[1] = np.clip(target_rpy[1], -self.MAX_ROLL_PITCH, self.MAX_ROLL_PITCH)
+ cur_rotation = np.array(p.getMatrixFromQuaternion(cur_quat)).reshape(3,3)
+ thrust = np.dot(cur_rotation, target_force)
+ return thrust[2], target_rpy, pos_e
+
+ ####################################################################################################
+ #### Generic PID attiutude control (with yaw locked to 0.) #########################################
+ ####################################################################################################
+ #### Arguments #####################################################################################
+ #### - control_timestep (float) timestep at which control is computed ######################
+ #### - thrust (float) desired thrust along the drone z-axis ######################
+ #### - cur_quat ((4,1) array) current orientation as a quaternion ########################
+ #### - target_rpy ((3,1) array) computed target roll, pitch, and yaw #######################
+ ####################################################################################################
+ #### Returns #######################################################################################
+ #### - rpm ((4,1) array) RPM values to apply to the 4 motors ########################
+ ####################################################################################################
+ def _simplePIDAttitudeControl(self, control_timestep, thrust, cur_quat, target_rpy):
+ cur_rpy = p.getEulerFromQuaternion(cur_quat)
+ rpy_e = target_rpy - np.array(cur_rpy).reshape(3,)
+ if rpy_e[2]>np.pi: rpy_e[2] = rpy_e[2] - 2*np.pi
+ if rpy_e[2]<-np.pi: rpy_e[2] = rpy_e[2] + 2*np.pi
+ d_rpy_e = (rpy_e - self.last_rpy_e) / control_timestep
+ self.last_rpy_e = rpy_e
+ self.integral_rpy_e = self.integral_rpy_e + rpy_e*control_timestep
+ #### PID target torques ############################################################################
+ target_torques = np.multiply(self.P_COEFF_TOR,rpy_e) + np.multiply(self.I_COEFF_TOR,self.integral_rpy_e) + np.multiply(self.D_COEFF_TOR,d_rpy_e)
+ return self._nnlsRPM(thrust, target_torques[0], target_torques[1], target_torques[2])
+
+ ####################################################################################################
+ #### Non-negative Least Squares (NNLS) RPM from desired thrust and torques ########################
+ ####################################################################################################
+ #### Arguments #####################################################################################
+ #### - thrust (float) desired thrust along the local z-axis ######################
+ #### - x_torque (float) desired x-axis torque ######################################
+ #### - y_torque (float) desired y-axis torque ######################################
+ #### - z_torque (float) desired z-axis torque ######################################
+ ####################################################################################################
+ #### Returns #######################################################################################
+ #### - rpm ((4,1) array) RPM values to apply to the 4 motors ########################
+ ####################################################################################################
+ def _nnlsRPM(self, thrust, x_torque, y_torque, z_torque):
+ new_line = True
+ #### Check the feasibility of thrust and torques ###################################################
+ if thrust<0 or thrust>self.MAX_THRUST:
+ if new_line: print(); new_line = False
+ print("[WARNING] ctrl it", self.control_counter, "in Control._nnlsRPM(), unfeasible thrust {:.2f} outside range [0, {:.2f}]".format(thrust, self.MAX_THRUST))
+ if np.abs(x_torque)>self.MAX_XY_TORQUE:
+ if new_line: print(); new_line = False
+ print("[WARNING] ctrl it", self.control_counter, "in Control._nnlsRPM(), unfeasible roll torque {:.2f} outside range [{:.2f}, {:.2f}]".format(x_torque, -self.MAX_XY_TORQUE, self.MAX_XY_TORQUE))
+ if np.abs(y_torque)>self.MAX_XY_TORQUE:
+ if new_line: print(); new_line = False
+ print("[WARNING] ctrl it", self.control_counter, "in Control._nnlsRPM(), unfeasible pitch torque {:.2f} outside range [{:.2f}, {:.2f}]".format(y_torque, -self.MAX_XY_TORQUE, self.MAX_XY_TORQUE))
+ if np.abs(z_torque)>self.MAX_Z_TORQUE:
+ if new_line: print(); new_line = False
+ print("[WARNING] ctrl it", self.control_counter, "in Control._nnlsRPM(), unfeasible yaw torque {:.2f} outside range [{:.2f}, {:.2f}]".format(z_torque, -self.MAX_Z_TORQUE, self.MAX_Z_TORQUE))
+ B = np.multiply(np.array([thrust, x_torque, y_torque, z_torque]), self.B_COEFF)
+ sq_rpm = np.dot(self.INV_A, B)
+ #### Use NNLS if any of the desired angular velocities is negative #################################
+ if np.min(sq_rpm)<0:
+ sol, res = nnls(self.A, B, maxiter=3*self.A.shape[1])
+ if new_line: print(); new_line = False
+ print("[WARNING] ctrl it", self.control_counter, "in Control._nnlsRPM(), unfeasible squared rotor speeds, using NNLS")
+ print("Negative sq. rotor speeds:\t [{:.2f}, {:.2f}, {:.2f}, {:.2f}]".format(sq_rpm[0], sq_rpm[1], sq_rpm[2], sq_rpm[3]),
+ "\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)))
+ print("NNLS:\t\t\t\t [{:.2f}, {:.2f}, {:.2f}, {:.2f}]".format(sol[0], sol[1], sol[2], sol[3]),
+ "\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)),
+ "\t\tResidual: {:.2f}".format(res) )
+ sq_rpm = sol
+ return np.sqrt(sq_rpm)
+
+
diff --git a/gym_pybullet_drones/envs/Logger.py b/gym_pybullet_drones/envs/Logger.py
new file mode 100644
index 000000000..eb410af35
--- /dev/null
+++ b/gym_pybullet_drones/envs/Logger.py
@@ -0,0 +1,86 @@
+import os
+import time
+from datetime import datetime
+from cycler import cycler
+import numpy as np
+import matplotlib.pyplot as plt
+
+
+######################################################################################################################################################
+#### Logger class ####################################################################################################################################
+######################################################################################################################################################
+class Logger(object):
+
+ ####################################################################################################
+ #### Initialize the logger #########################################################################
+ ####################################################################################################
+ #### Arguments #####################################################################################
+ #### - duration_sec (int) duration of the simulation in seconds ######################
+ #### - simulation_freq_hz (int) simulation frequency in Hz #################################
+ #### - num_drones (int) number of drones ###########################################
+ ####################################################################################################
+ def __init__(self, duration_sec: int, simulation_freq_hz: int, num_drones: int=1):
+ self.DURATION_SEC = duration_sec; self.SIMULATION_FREQ_HZ = simulation_freq_hz
+ self.NUM_DRONES = num_drones; self.NUM_STEPS = self.DURATION_SEC * self.SIMULATION_FREQ_HZ
+ self.counters = np.zeros(num_drones)
+ self.timestamps = np.zeros((num_drones, self.NUM_STEPS))
+ self.states = np.zeros((num_drones, self.NUM_STEPS, 16)) #### 16 states: pos_x, pos_y, pos_z,
+ # vel_x, vel_y, vel_z,
+ # roll, pitch, yaw,
+ # ang_vel_x, ang_vel_y, ang_vel_z,
+ # rpm0, rpm1, rpm2, rpm3
+ #### Note that this is not the same order returned in obs["i"]["state"] by Aviary.step() #######
+ self.controls = np.zeros((num_drones, self.NUM_STEPS, 12)) #### 12 control targets: pos_x, pos_y, pos_z,
+ # vel_x, vel_y, vel_z,
+ # roll, pitch, yaw,
+ # ang_vel_x, ang_vel_y, ang_vel_z
+
+ ####################################################################################################
+ #### Log entries for a single simulation step, of a single drone ###################################
+ ####################################################################################################
+ #### Arguments #####################################################################################
+ #### - drone (int) drone associated to the log entry ##########################
+ #### - timestamp (float) timestamp of the log in simulation clock ###################
+ #### - state ((20,1) array) drone's state ##############################################
+ #### - control ((12,1) array) drone's control target #####################################
+ ####################################################################################################
+ def log(self, drone: int, timestamp, state, control):
+ current_counter = int(self.counters[drone])
+ if drone<0 or drone>=self.NUM_DRONES or timestamp<0 or timestamp>self.DURATION_SEC \
+ or len(state)!=16 or len(control)!=12 or current_counter>=self.NUM_STEPS: print("[ERROR] in Logger.log(), invalid data")
+ self.timestamps[drone, current_counter] = timestamp
+ self.states[drone, current_counter,:] = np.hstack([ state[0:3], state[10:13], state[7:10], state[13:20] ])
+ self.controls[drone, current_counter,:] = control
+ self.counters[drone] += 1
+
+ ####################################################################################################
+ #### Save the logs to file #########################################################################
+ ####################################################################################################
+ def save(self):
+ with open(os.path.dirname(os.path.abspath(__file__))+"/../../files/save-flight-"+datetime.now().strftime("%m.%d.%Y_%H.%M.%S")+".npy", 'wb') as out_file:
+ np.save(out_file, self.timestamps); np.save(out_file, self.states); np.save(out_file, self.controls)
+
+ ####################################################################################################
+ #### Plot the logged (state) data ##################################################################
+ ####################################################################################################
+ #### Arguments #####################################################################################
+ #### - pwm (bool) if True, convert logged RPM into PWM values ################
+ ####################################################################################################
+ def plot(self, pwm=False):
+ #### Loop over colors and line styles ##############################################################
+ plt.rc('axes', prop_cycle=(cycler('color', ['r', 'g', 'b', 'y']) + cycler('linestyle', ['-', '--', ':', '-.'])))
+ fig, axs = plt.subplots(8,2)
+ t = np.arange(0, self.DURATION_SEC, 1/self.SIMULATION_FREQ_HZ)
+ labels = ['X', 'Y', 'Z', 'VX', 'VY', 'VZ', 'R', 'P', 'Yaw', 'WR', 'WP', 'WY', 'PWM0', 'PWM1', 'PWM2', 'PWM3'] if pwm else ['X', 'Y', 'Z', 'VX', 'VY', 'VZ', 'R', 'P', 'Yaw', 'WR', 'WP', 'WY', 'RPM0', 'RPM1', 'RPM2', 'RPM3']
+ for i in range(16):
+ for j in range(self.NUM_DRONES):
+ #### This line converts RPM into PWM for all drones except drone_0 (used in compare.py) ############
+ if pwm and i>11 and j>0: self.states[j,:,i] = (self.states[j,:,i] - 4070.3) / 0.2685
+ axs[i%8,i//8].plot(t, self.states[j,:,i], label="drone_"+str(j))
+ axs[i%8,i//8].set_xlabel('time')
+ axs[i%8,i//8].set_ylabel(labels[i])
+ axs[i%8,i//8].grid(True)
+ axs[i%8,i//8].legend(loc='upper right', frameon=True)
+ fig.subplots_adjust(left=0.06, bottom=0.05, right=0.99, top=0.98, wspace=0.15, hspace=0.0)
+ plt.show()
+
diff --git a/gym_pybullet_drones/envs/MultiDroneEnv.py b/gym_pybullet_drones/envs/MultiDroneEnv.py
deleted file mode 100644
index 50978a41b..000000000
--- a/gym_pybullet_drones/envs/MultiDroneEnv.py
+++ /dev/null
@@ -1,75 +0,0 @@
-import os
-import time
-from datetime import datetime
-import pdb
-import math
-import numpy as np
-import pybullet as p
-import pybullet_data
-import gym
-from gym import error, spaces, utils
-from gym.utils import seeding
-import xml.etree.ElementTree as etxml
-
-from gym_pybullet_drones.envs.SingleDroneEnv import DroneModel
-
-
-######################################################################################################################################################
-#### Multi drone environment class ###################################################################################################################
-######################################################################################################################################################
-class MultiDroneEnv(gym.Env):
- metadata = {'render.modes': ['human']}
-
- ####################################################################################################
- #### TBD ###########################################################################################
- ####################################################################################################
- def __init__(self, drone_model=DroneModel.CF2X, gui=False):
- super(MultiDroneEnv, self).__init__()
-
- ####################################################################################################
- #### TBD ###########################################################################################
- ####################################################################################################
- def reset(self):
- pass
-
- ####################################################################################################
- #### TBD ###########################################################################################
- ####################################################################################################
- def step(self, action):
- pass
-
- ####################################################################################################
- #### TBD ###########################################################################################
- ####################################################################################################
- def render(self, mode='human', close=False):
- pass
-
- ####################################################################################################
- #### TBD ###########################################################################################
- ####################################################################################################
- def close(self):
- pass
-
- ####################################################################################################
- #### TBD ###########################################################################################
- ####################################################################################################
- def getPyBulletClient(self):
- pass
-
- ####################################################################################################
- #### TBD ###########################################################################################
- ####################################################################################################
- def getDroneIds(self):
- pass
-
- ####################################################################################################
- #### TBD ###########################################################################################
- ####################################################################################################
- def _housekeeping(self):
- pass
-
- ####################################################################################################
- #### TBD ###########################################################################################
- ####################################################################################################
- def _physics(self, forces, z_torque):
- pass
diff --git a/gym_pybullet_drones/envs/MultiDroneUserDefinedFunctions.py b/gym_pybullet_drones/envs/MultiDroneUserDefinedFunctions.py
deleted file mode 100644
index 95a69ca07..000000000
--- a/gym_pybullet_drones/envs/MultiDroneUserDefinedFunctions.py
+++ /dev/null
@@ -1,21 +0,0 @@
-import os
-import time
-import pdb
-import math
-import numpy as np
-import pybullet as p
-import gym
-from gym import error, spaces, utils
-from gym.utils import seeding
-
-
-######################################################################################################################################################
-#### A class for user-defined "reward" and "done" functions, state/obs normalization, etc. ###########################################################
-######################################################################################################################################################
-class MultiDroneUserDefinedFunctions(object):
-
- ####################################################################################################
- #### Initialization with a username string #########################################################
- ####################################################################################################
- def __init__(self, client, gui=False, user: str="Default"):
- self.CLIENT = client; self.GUI = gui; self.USER = user
\ No newline at end of file
diff --git a/gym_pybullet_drones/envs/ProblemSpecificFunctions.py b/gym_pybullet_drones/envs/ProblemSpecificFunctions.py
new file mode 100644
index 000000000..d6de2a458
--- /dev/null
+++ b/gym_pybullet_drones/envs/ProblemSpecificFunctions.py
@@ -0,0 +1,129 @@
+import os
+import time
+import pdb
+import math
+import numpy as np
+import pybullet as p
+import gym
+from gym import error, spaces, utils
+from gym.utils import seeding
+from enum import Enum
+
+
+######################################################################################################################################################
+#### Enumeration of ProblemSpecificFunctions implementations #########################################################################################
+######################################################################################################################################################
+class Problem(Enum):
+ DEFAULT = 0 # Default "reward" and "done" functions, state normalization, obstacles (i.e. fly upwards from the origin)
+ CUSTOM = 1 # Placeholder for custom "reward" and "done" functions, state normalization, obstacles, etc.
+
+
+######################################################################################################################################################
+#### A class for problem-specific "reward" and "done" functions, state normalization, etc. ###########################################################
+######################################################################################################################################################
+class ProblemSpecificFunctions(object):
+
+ ####################################################################################################
+ #### Initialization with the desired problem #######################################################
+ ####################################################################################################
+ #### Arguments #####################################################################################
+ #### - client (PyBullet client id) id of the pybullet client to connect to ####################
+ #### - gui (bool) whether PyBullet's GUI is in use ###########################
+ #### - problem (Problem) to disambiguate between function implementations ###########
+ ####################################################################################################
+ def __init__(self, client, gui=False, problem: Problem=Problem.DEFAULT):
+ self.CLIENT = client; self.GUI = gui; self.PROBLEM = problem
+
+ ####################################################################################################
+ #### Compute the current state's reward ############################################################
+ ####################################################################################################
+ #### Arguments #####################################################################################
+ #### - normalized state ((20,1) array) clipped and normalized simulation state ####################
+ ####################################################################################################
+ #### Returns #######################################################################################
+ #### - reward (float) reward value ###############################################
+ ####################################################################################################
+ def rewardFunction(self, obs):
+ if self.PROBLEM==Problem.DEFAULT:
+ if obs[2] > 0.8: return -1
+ elif obs[2] > 0.5: return 2000
+ elif obs[2] > 0.3: return 1000
+ elif obs[2] > 0.2: return 500
+ elif obs[2] > 0.15: return 100
+ elif obs[2] > 0.1: return 10
+ else: return -1
+ elif self.PROBLEM==Problem.CUSTOM:
+ pass
+ else: print("[ERROR] in ProblemSpecificFunctions.rewardFunction(), unknown Problem")
+
+ ####################################################################################################
+ #### Evaluate the current state's stopping conditions ##############################################
+ ####################################################################################################
+ #### Arguments #####################################################################################
+ #### - normalized state ((20,1) array) clipped and normalized simulation state ####################
+ #### - sim_time (float) elapsed simulation time (in seconds) #######################
+ ####################################################################################################
+ #### Returns #######################################################################################
+ #### - done (bool) whether the stopping conditions of the episode are met #####
+ ####################################################################################################
+ def doneFunction(self, obs, sim_time):
+ if self.PROBLEM==Problem.DEFAULT:
+ if np.abs(obs[0])>=1 or np.abs(obs[1])>=1 or obs[2]>=1 \
+ or np.abs(obs[7])>=np.pi/3 or np.abs(obs[8])>=np.pi/3 \
+ or np.abs(obs[10])>=1 or np.abs(obs[11])>=1 or np.abs(obs[12])>=1 \
+ or np.abs(obs[13])>=10*np.pi or np.abs(obs[14])>=10*np.pi or np.abs(obs[15])>=20*np.pi \
+ or sim_time > 3: return True
+ else: return False
+ elif self.PROBLEM==Problem.CUSTOM:
+ pass
+ else: print("[ERROR] in ProblemSpecificFunctions.doneFunction(), unknown Problem")
+
+ ####################################################################################################
+ #### Normalize the 20 values in the simulation state to the [-1,1] range ###########################
+ ####################################################################################################
+ #### Arguments #####################################################################################
+ #### - state ((20,1) array) raw simulation stat data ##################################
+ #### - step_counter (int) current simulation step ####################################
+ ####################################################################################################
+ #### Returns #######################################################################################
+ #### - normalized state ((20,1) array) clipped and normalized simulation state ####################
+ ####################################################################################################
+ def clipAndNormalizeState(self, state, step_counter):
+ if self.PROBLEM==Problem.DEFAULT:
+ clipped_pos = np.clip(state[0:3], -1, 1)
+ clipped_rp = np.clip(state[7:9], -np.pi/3, np.pi/3)
+ clipped_vel = np.clip(state[10:13], -1, 1)
+ clipped_ang_vel_rp = np.clip(state[13:15], -10*np.pi, 10*np.pi)
+ clipped_ang_vel_y = np.clip(state[15], -20*np.pi, 20*np.pi)
+ if self.GUI:
+ if not(clipped_pos==np.array(state[0:3])).all(): print("[WARNING] it", step_counter, "in ProblemSpecificFunctions.clipAndNormalizeState(), out-of-bound position [{:.2f} {:.2f} {:.2f}], consider a more conservative implementation of ProblemSpecificFunctions.doneFunction()".format(state[0], state[1], state[2]))
+ if not(clipped_rp==np.array(state[7:9])).all(): print("[WARNING] it", step_counter, "in ProblemSpecificFunctions.clipAndNormalizeState(), out-of-bound roll/pitch [{:.2f} {:.2f}], consider a more conservative implementation of ProblemSpecificFunctions.doneFunction()".format(state[7], state[8]))
+ if not(clipped_vel==np.array(state[10:13])).all(): print("[WARNING] it", step_counter, "in ProblemSpecificFunctions.clipAndNormalizeState(), out-of-bound velocity [{:.2f} {:.2f} {:.2f}], consider a more conservative implementation of ProblemSpecificFunctions.doneFunction()".format(state[10], state[11], state[12]))
+ if not(clipped_ang_vel_rp==np.array(state[13:15])).all(): print("[WARNING] it", step_counter, "in ProblemSpecificFunctions.clipAndNormalizeState(), out-of-bound angular velocity [{:.2f} {:.2f} {:.2f}], consider a more conservative implementation of ProblemSpecificFunctions.doneFunction()".format(state[13], state[14], state[15]))
+ if not(clipped_ang_vel_y==np.array(state[15])): print("[WARNING] it", step_counter, "in ProblemSpecificFunctions.clipAndNormalizeState(), out-of-bound angular velocity [{:.2f} {:.2f} {:.2f}], consider a more conservative implementation of ProblemSpecificFunctions.doneFunction()".format(state[13], state[14], state[15]))
+ normalized_pos = clipped_pos
+ normalized_rp = clipped_rp/(np.pi/3)
+ normalized_y = state[9]/np.pi
+ normalized_vel = clipped_vel
+ normalized_ang_vel_rp = clipped_ang_vel_rp/(10*np.pi)
+ normalized_ang_vel_y = clipped_ang_vel_y/(20*np.pi)
+ return np.hstack([normalized_pos, state[3:7], normalized_rp, normalized_y, normalized_vel, normalized_ang_vel_rp, normalized_ang_vel_y, state[16:20] ]).reshape(20,)
+ elif self.PROBLEM==Problem.CUSTOM:
+ pass
+ else: print("[ERROR] in ProblemSpecificFunctions.clipAndNormalizeState(), unknown Problem")
+
+ ####################################################################################################
+ #### Add obstacles to the environment from .urdf files #############################################
+ ####################################################################################################
+ def addObstacles(self):
+ if self.PROBLEM==Problem.DEFAULT:
+ p.loadURDF("samurai.urdf", physicsClientId=self.CLIENT)
+ p.loadURDF("duck_vhacd.urdf", [-.5,-.5,.05], p.getQuaternionFromEuler([0,0,0]), physicsClientId=self.CLIENT)
+ p.loadURDF("cube_no_rotation.urdf", [-.5,-2.5,.5], p.getQuaternionFromEuler([0,0,0]), physicsClientId=self.CLIENT)
+ p.loadURDF("sphere2.urdf", [0,2,.5], p.getQuaternionFromEuler([0,0,0]), physicsClientId=self.CLIENT)
+ elif self.PROBLEM==Problem.CUSTOM:
+ pass
+ else: print("[ERROR] in ProblemSpecificFunctions.addObstacles(), unknown Problem")
+
+
+
diff --git a/gym_pybullet_drones/envs/SingleDroneEnv.py b/gym_pybullet_drones/envs/SingleDroneEnv.py
deleted file mode 100644
index 79f12a78d..000000000
--- a/gym_pybullet_drones/envs/SingleDroneEnv.py
+++ /dev/null
@@ -1,411 +0,0 @@
-import os
-import time
-import pdb
-import math
-import numpy as np
-import pybullet as p
-import pybullet_data
-import gym
-import xml.etree.ElementTree as etxml
-from gym import error, spaces, utils
-from gym.utils import seeding
-from enum import Enum
-from datetime import datetime
-
-from gym_pybullet_drones.envs.SingleDroneUserDefinedFunctions import SingleDroneUserDefinedFunctions
-
-
-######################################################################################################################################################
-#### Drone models enumeration ########################################################################################################################
-######################################################################################################################################################
-class DroneModel(Enum):
- HB = 0 # Generic quadrotor (with AscTec Hummingbird intertial properties)
- CF2X = 1 # Bitcraze Craziflie 2.0 in the X configuration
- CF2P = 2 # Bitcraze Craziflie 2.0 in the + configuration
-
-
-######################################################################################################################################################
-#### Single drone environment class ##################################################################################################################
-######################################################################################################################################################
-class SingleDroneEnv(gym.Env):
- metadata = {'render.modes': ['human']}
-
- ####################################################################################################
- #### Initialize the environment ####################################################################
- ####################################################################################################
- #### Arguments #####################################################################################
- #### - drone_model (DroneModel) the type of drone to use (associated to an .urdf file) #####
- #### - initial_xyz (3-by-1 list) initial XYZ position of the drone ##########################
- #### - initial_rpy (3-by-1 list) initial roll, pitch, and yaw of the drone (radians) ########
- #### - pybullet (Boolean) whether to use PyBullet's physics engine ###################
- #### - aero_effects (Boolean) simple drag and ground effect in PyBullet ##################
- #### - normalized_spaces (Boolean) whether to use normalized OpenAI Gym spaces ################
- #### - freq (Integer) the freqeuency (Hz) at which the simulation steps ##########
- #### - gui (Boolean) whether to use PyBullet's GUI ##############################
- #### - obstacles (Boolean) whether to add obstacles in the simulation #################
- #### - record (Boolean) whether to save the simulation as an .mp4 video ############
- #### - user (String) passed to __init__() of SingleDroneUserDefinedFunctions ####
- ####################################################################################################
- def __init__(self, drone_model: DroneModel=DroneModel.CF2X, initial_xyz=[0.,0.,.1], initial_rpy=[0.,0.,0.], pybullet=True, aero_effects=False, normalized_spaces=True, freq=240, gui=False, obstacles=False, record=False, user="Default"):
- super(SingleDroneEnv, self).__init__()
- self.G = 9.8; self.RAD2DEG = 180/np.pi; self.DEG2RAD = np.pi/180
- self.DRONE_MODEL = drone_model; self.INIT_XYZ = initial_xyz; self.INIT_RPY = initial_rpy
- self.PYBULLET = pybullet; self.AERO_EFFECTS = aero_effects; self.NORM_SPACES = normalized_spaces
- self.SIM_FREQ = freq; self.TIMESTEP = 1./self.SIM_FREQ; self.GUI=gui; self.OBSTACLES = obstacles; self.RECORD = record; self.USER = user
- if self.DRONE_MODEL == DroneModel.HB: self.URDF = "hb.urdf"
- elif self.DRONE_MODEL == DroneModel.CF2X: self.URDF = "cf2x.urdf"
- elif self.DRONE_MODEL == DroneModel.CF2P: self.URDF = "cf2p.urdf"
- if self.AERO_EFFECTS and not self.PYBULLET: print("[WARNING] SingleDroneEnv was initalized with pybullet=False, aerodynamic effects will not be computed")
- ####################################################################################################
- #### Connect to PyBullet ###########################################################################
- ####################################################################################################
- if self.GUI:
- self.CLIENT = p.connect(p.GUI)
- p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW,0, physicsClientId=self.CLIENT)
- p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW,0, physicsClientId=self.CLIENT)
- p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW,0, physicsClientId=self.CLIENT)
- p.resetDebugVisualizerCamera(cameraDistance=3, cameraYaw=-30, cameraPitch=-30, cameraTargetPosition=[0,0,0], physicsClientId=self.CLIENT)
- else: self.CLIENT = p.connect(p.DIRECT)
- ####################################################################################################
- #### Load the drone properties from the .urdf file #################################################
- ####################################################################################################
- URDF_TREE = etxml.parse(os.path.dirname(os.path.abspath(__file__))+"/../assets/"+self.URDF).getroot()
- self.M = float(URDF_TREE[1][0][1].attrib['value']); self.L = float(URDF_TREE[0].attrib['arm']); self.THRUST2WEIGHT_RATIO = float(URDF_TREE[0].attrib['thrust2weight'])
- self.IXX = float(URDF_TREE[1][0][2].attrib['ixx']); self.IYY = float(URDF_TREE[1][0][2].attrib['iyy']); self.IZZ = float(URDF_TREE[1][0][2].attrib['izz'])
- self.KF = float(URDF_TREE[0].attrib['kf']); self.KM = float(URDF_TREE[0].attrib['km'])
- self.COLLISION_H = float(URDF_TREE[1][2][1][0].attrib['length']); self.COLLISION_R = float(URDF_TREE[1][2][1][0].attrib['radius'])
- COLLISION_SHAPE_OFFSETS = [float(s) for s in URDF_TREE[1][2][0].attrib['xyz'].split(' ')]; self.COLLISION_Z_OFFSET = COLLISION_SHAPE_OFFSETS[2]
- self.GND_EFF_COEFF = float(URDF_TREE[0].attrib['gnd_eff_coeff']); self.PROP_RADIUS = float(URDF_TREE[0].attrib['prop_radius'])
- self.DRAG_COEFF_XY = float(URDF_TREE[0].attrib['drag_coeff_xy']); self.DRAG_COEFF_Z = float(URDF_TREE[0].attrib['drag_coeff_z'])
- self.DRAG_COEFF = np.array([self.DRAG_COEFF_XY, self.DRAG_COEFF_XY, self.DRAG_COEFF_Z])
- if not self.PYBULLET: self.J = np.diag([self.IXX, self.IYY, self.IZZ]); self.J_INV = np.linalg.inv(self.J)
- print("[INFO] SingleDroneEnv.__init__() loaded from the drone's .urdf file the following physical parameters (m, L, ixx, iyy, izz, kf, km, t2w):", self.M, self.L, self.IXX, self.IYY, self.IZZ, self.KF, self.KM, self.THRUST2WEIGHT_RATIO)
- if self.AERO_EFFECTS: print("[INFO] SingleDroneEnv.__init__() loaded from the drone's .urdf file the following aerodynamic parameters (gnd_eff_coeff, prop_radius, drag_xy_coeff, drag_z_coeff):", self.GND_EFF_COEFF, self.PROP_RADIUS, self.DRAG_COEFF_XY, self.DRAG_COEFF_Z)
- ####################################################################################################
- #### Compute constants #############################################################################
- ####################################################################################################
- self.GRAVITY = self.G*self.M
- self.HOVER_RPM = np.sqrt((self.M*self.G)/(4*self.KF))
- self.MAX_RPM = np.sqrt((self.THRUST2WEIGHT_RATIO*self.GRAVITY)/(4*self.KF))
- self.MAX_THRUST = (4*self.KF*self.MAX_RPM**2)
- self.MAX_XY_TORQUE = (self.L*self.KF*self.MAX_RPM**2)
- self.MAX_Z_TORQUE = (2*self.KM*self.MAX_RPM**2)
- ####################################################################################################
- #### Create action and observation spaces ##########################################################
- ####################################################################################################
- if self.NORM_SPACES:
- self.action_space = spaces.Box( low=np.array([-1,-1,-1,-1]), high=np.array([1,1,1,1]), dtype=np.float32)
- #### Observations ################################ X Y Z Q1 Q2 Q3 Q4 R P Y VX VY VZ WR WP WY P0 P1 P2 P3
- self.observation_space = spaces.Box(low=np.array([ -1, -1, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1]), \
- high=np.array([ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]), dtype=np.float32)
- else:
- self.action_space = spaces.Box( low=np.array([0.,0.,0.,0.]), high=np.array([self.MAX_RPM,self.MAX_RPM,self.MAX_RPM,self.MAX_RPM]), dtype=np.float32)
- #### Observations ################################ X Y Z Q1 Q2 Q3 Q4 R P Y VX VY VZ WR WP WY P0 P1 P2 P3
- self.observation_space = spaces.Box(low=np.array([ -np.inf,-np.inf,0., -1., -1., -1., -1., -np.pi, -np.pi, -np.pi, -np.inf,-np.inf,-np.inf,-np.inf,-np.inf,-np.inf, 0., 0., 0., 0.]), \
- high=np.array([ np.inf, np.inf, np.inf, 1., 1., 1., 1., np.pi, np.pi, np.pi, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf, self.MAX_RPM, self.MAX_RPM, self.MAX_RPM, self.MAX_RPM]), dtype=np.float32)
- ####################################################################################################
- #### Add input sliders to the GUI ##################################################################
- ####################################################################################################
- self.P0_SLIDER = p.addUserDebugParameter('Propeller 0 RPM', 0, self.MAX_RPM, self.HOVER_RPM, physicsClientId=self.CLIENT)
- self.P1_SLIDER = p.addUserDebugParameter('Propeller 1 RPM', 0, self.MAX_RPM, self.HOVER_RPM, physicsClientId=self.CLIENT)
- self.P2_SLIDER = p.addUserDebugParameter('Propeller 2 RPM', 0, self.MAX_RPM, self.HOVER_RPM, physicsClientId=self.CLIENT)
- self.P3_SLIDER = p.addUserDebugParameter('Propeller 3 RPM', 0, self.MAX_RPM, self.HOVER_RPM, physicsClientId=self.CLIENT)
- self.INPUT_SWITCH = p.addUserDebugParameter('Use GUI RPM', 9999., -1., 0, physicsClientId=self.CLIENT)
- ####################################################################################################
- #### Housekeeping ##################################################################################
- ####################################################################################################
- self._housekeeping()
-
- ####################################################################################################
- #### Reset the environment #########################################################################
- ####################################################################################################
- def reset(self):
- p.resetSimulation(physicsClientId=self.CLIENT)
- ####################################################################################################
- #### Housekeeping ##################################################################################
- ####################################################################################################
- self._housekeeping()
- if self.RECORD: self.VIDEO_ID = p.startStateLogging(loggingType=p.STATE_LOGGING_VIDEO_MP4, fileName=os.path.dirname(os.path.abspath(__file__))+"/../../files/saves/video-"+datetime.now().strftime("%m.%d.%Y_%H.%M.%S")+".mp4", physicsClientId=self.CLIENT)
- ####################################################################################################
- #### Return the initial observation ################################################################
- ####################################################################################################
- pos, quat = p.getBasePositionAndOrientation(self.DRONE_ID, physicsClientId=self.CLIENT)
- rpy = p.getEulerFromQuaternion(quat)
- vel, ang_v = p.getBaseVelocity(self.DRONE_ID, physicsClientId=self.CLIENT)
- state = np.hstack([pos, quat, rpy, vel, ang_v, self.last_action])
- if self.NORM_SPACES: state = self.USER_DEFINED_FUNCTIONS.clipAndNormalizeState(state, self.step_counter)
- return state.reshape(20,)
-
- ####################################################################################################
- #### Advance the environment by one simulation step ################################################
- ####################################################################################################
- #### Arguments #####################################################################################
- #### - action (Gym's Box(4,)) motors' speed (normalized or unnormalized) #################
- ####################################################################################################
- #### Returns #######################################################################################
- #### - state (Gym's Box(20,)) observation vector (normalized or unnormalized) ############
- #### - reward (Float) reward value of the current state ##########################
- #### - done (Boolean) whether the current episode is over/meets halt conditions ##
- #### - info (Dict) additional custom information ##############################
- ####################################################################################################
- def step(self, action):
- self.step_counter += 1
- ####################################################################################################
- #### Read the GUI's input parameters ###############################################################
- ####################################################################################################
- if self.GUI:
- current_input_switch = p.readUserDebugParameter(self.INPUT_SWITCH, physicsClientId=self.CLIENT)
- if current_input_switch > self.last_input_switch:
- self.last_input_switch = current_input_switch
- self.USE_GUI_RPM = True if self.USE_GUI_RPM==False else False
- if self.USE_GUI_RPM:
- p0, p1 = p.readUserDebugParameter(self.P0_SLIDER, physicsClientId=self.CLIENT), p.readUserDebugParameter(self.P1_SLIDER, physicsClientId=self.CLIENT)
- p2, p3 = p.readUserDebugParameter(self.P2_SLIDER, physicsClientId=self.CLIENT), p.readUserDebugParameter(self.P3_SLIDER, physicsClientId=self.CLIENT)
- clipped_rpm = np.array([p0,p1,p2,p3])
- if self.step_counter%(self.SIM_FREQ/2)==0:
- self.GUI_INPUT_TEXT = p.addUserDebugText("Using GUI RPM",textPosition=[0,0,0],textColorRGB=[1,0,0],lifeTime=1,textSize=2,parentObjectUniqueId=self.DRONE_ID,parentLinkIndex=-1,replaceItemUniqueId=self.GUI_INPUT_TEXT,physicsClientId=self.CLIENT)
- ####################################################################################################
- #### Denormalize (if necessary) and clip the action to the maximum RPM #############################
- ####################################################################################################
- else:
- rpm = self._normActionToRPM(action) if self.NORM_SPACES else np.array(action)
- clipped_rpm = np.clip(np.array(rpm), 0, self.MAX_RPM)
- if self.GUI and not(clipped_rpm==np.array(rpm)).all():
- print("\n[WARNING] it:", self.step_counter, "in step(), out-of-range rotor speeds [{:.0f} {:.0f} {:.0f} {:.0f}], max RPM: {:.0f}".format(rpm[0], rpm[1], rpm[2], rpm[3], self.MAX_RPM))
- self.last_action = np.array(action)
- ####################################################################################################
- #### Step the simulation using PyBullet's physics engine ###########################################
- ####################################################################################################
- if self.PYBULLET:
- self._physics(clipped_rpm)
- if self.AERO_EFFECTS: self._simpleAerodynamicEffects(clipped_rpm)
- p.stepSimulation(physicsClientId=self.CLIENT)
- ####################################################################################################
- #### Step the simulation with a custom dynamics implementation #####################################
- ####################################################################################################
- else: self._noPyBulletDynamics(clipped_rpm)
- ####################################################################################################
- #### Prepare the return values #####################################################################
- ####################################################################################################
- self._showDroneFrame()
- pos, quat = p.getBasePositionAndOrientation(self.DRONE_ID, physicsClientId=self.CLIENT)
- rpy = p.getEulerFromQuaternion(quat)
- vel, ang_v = p.getBaseVelocity(self.DRONE_ID, physicsClientId=self.CLIENT)
- state = np.hstack([pos, quat, rpy, vel, ang_v, self.last_action])
- if self.NORM_SPACES: state = self.USER_DEFINED_FUNCTIONS.clipAndNormalizeState(state, self.step_counter)
- reward = self.USER_DEFINED_FUNCTIONS.rewardFunction(state)
- done = self.USER_DEFINED_FUNCTIONS.doneFunction(state, self.step_counter/self.SIM_FREQ)
- info = {"answer": 42}
- return state.reshape(20,), reward, done, info
-
- ####################################################################################################
- #### Print a textual output of the environment #####################################################
- ####################################################################################################
- def render(self, mode='human', close=False):
- if self.first_render_call and not self.GUI:
- self.first_render_call = False
- print("[WARNING] render() is implemented as text-only, re-initialize the environment using singleDroneEnv(gui=True) to use PyBullet's graphical interface")
- pos, quat = p.getBasePositionAndOrientation(self.DRONE_ID, physicsClientId=self.CLIENT)
- rpy = p.getEulerFromQuaternion(quat)
- vel, ang_v = p.getBaseVelocity(self.DRONE_ID, physicsClientId=self.CLIENT)
- print("——— step number {:d}".format(self.step_counter),
- "——— wall-clock time {:.1f}".format(time.time()-self.RESET_TIME),
- "——— simulation time {:.1f}@{:d}Hz ({:.2f}x)".format(self.step_counter*self.TIMESTEP, self.SIM_FREQ, (self.step_counter*self.TIMESTEP)/(time.time()-self.RESET_TIME)),
- "——— X {:.2f} Y {:.2f} Z {:.2f} vel. {:.2f}".format(pos[0],pos[1],pos[2], np.linalg.norm(vel)),
- "——— roll {:.2f} pitch {:.2f} yaw {:.2f}".format(rpy[0]*self.RAD2DEG, rpy[1]*self.RAD2DEG, rpy[2]*self.RAD2DEG),
- "——— ang. vel. {:.2f} {:.2f} {:.2f} ——— ".format(ang_v[0]*self.RAD2DEG, ang_v[1]*self.RAD2DEG, ang_v[2]*self.RAD2DEG), end="\r")
-
- ####################################################################################################
- #### Close the environment #########################################################################
- ####################################################################################################
- def close(self):
- pos, quat = p.getBasePositionAndOrientation(self.DRONE_ID, physicsClientId=self.CLIENT)
- rpy = p.getEulerFromQuaternion(quat)
- vel, ang_v = p.getBaseVelocity(self.DRONE_ID, physicsClientId=self.CLIENT)
- print("——— step number {:d}".format(self.step_counter),
- "——— wall-clock time {:.1f}".format(time.time()-self.RESET_TIME),
- "——— simulation time {:.1f}@{:d}Hz ({:.2f}x)".format(self.step_counter*self.TIMESTEP, self.SIM_FREQ, (self.step_counter*self.TIMESTEP)/(time.time()-self.RESET_TIME)),
- "——— X {:.2f} Y {:.2f} Z {:.2f} vel. {:.2f}".format(pos[0],pos[1],pos[2], np.linalg.norm(vel)),
- "——— roll {:.2f} pitch {:.2f} yaw {:.2f}".format(rpy[0]*self.RAD2DEG, rpy[1]*self.RAD2DEG, rpy[2]*self.RAD2DEG),
- "——— ang. vel. {:.2f} {:.2f} {:.2f} ——— ".format(ang_v[0]*self.RAD2DEG, ang_v[1]*self.RAD2DEG, ang_v[2]*self.RAD2DEG))
- if self.RECORD: p.stopStateLogging(self.VIDEO_ID, physicsClientId=self.CLIENT)
- p.disconnect(physicsClientId=self.CLIENT)
-
- ####################################################################################################
- #### Return the PyBullet Client Id #################################################################
- ####################################################################################################
- def getPyBulletClient(self):
- return self.CLIENT
-
- ####################################################################################################
- #### Return the Drone Id ###########################################################################
- ####################################################################################################
- def getDroneId(self):
- return self.DRONE_ID
-
- ####################################################################################################
- #### Denormalize the [-1,1] range to the [0, MAX RPM] range ########################################
- ####################################################################################################
- #### Arguments #####################################################################################
- #### - action (4-by-1 array) normalized [-1,1] actions applied to the 4 motors ##########
- ####################################################################################################
- #### Returns #######################################################################################
- #### - rpm (4-by-1 array) RPM values to apply to the 4 motors ########################
- ####################################################################################################
- def _normActionToRPM(self, action):
- if np.any(np.abs(action)) > 1: print("\n[ERROR] it:", self.step_counter, "in _normActionToRPM(), out-of-bound action")
- return np.where(action <= 0, (action+1)*self.HOVER_RPM, action*self.MAX_RPM)
-
- ####################################################################################################
- #### Normalize the [0, MAX RPM] range to the [-1,1] range ##########################################
- ####################################################################################################
- #### Arguments #####################################################################################
- #### - rpm (4-by-1 array) RPM values of the 4 motors #################################
- ####################################################################################################
- #### Returns #######################################################################################
- #### - action (4-by-1 array) normalized action to apply to the 4 motors #################
- ####################################################################################################
- # def _rpmToNormAction(self, rpm):
- # if np.any(rpm) < 0: print("\n[ERROR] it:", self.step_counter, "in _rpmToNormAction(), negative RPM")
- # return np.where(rpm <= self.HOVER_RPM, (rpm/self.HOVER_RPM)-1, rpm/self.MAX_RPM)
-
- ####################################################################################################
- #### Housekeeping shared by the __init__() and reset() functions ###################################
- ####################################################################################################
- def _housekeeping(self):
- ####################################################################################################
- #### Initialize/reset counters and zero-valued variables ###########################################
- ####################################################################################################
- self.RESET_TIME = time.time(); self.step_counter = 0; self.first_render_call = True
- self.X_AX = -1; self.Y_AX = -1; self.Z_AX = -1;
- self.GUI_INPUT_TEXT = -1; self.USE_GUI_RPM=False; self.last_input_switch = 0
- self.USER_DEFINED_FUNCTIONS = SingleDroneUserDefinedFunctions(self.CLIENT, self.GUI, self.USER)
- self.last_action = -1*np.ones(4) if self.NORM_SPACES else np.zeros(4)
- if not self.PYBULLET: self.no_pybullet_acc = np.zeros(3)
- ####################################################################################################
- #### Set PyBullet's parameters #####################################################################
- ####################################################################################################
- p.setGravity(0, 0, -self.G, physicsClientId=self.CLIENT)
- p.setRealTimeSimulation(0, physicsClientId=self.CLIENT)
- p.setTimeStep(self.TIMESTEP, physicsClientId=self.CLIENT)
- p.setAdditionalSearchPath(pybullet_data.getDataPath(), physicsClientId=self.CLIENT)
- ####################################################################################################
- #### Load ground plane, drone and obstacles models #################################################
- ####################################################################################################
- p.loadURDF("plane.urdf", physicsClientId=self.CLIENT)
- self.DRONE_ID = p.loadURDF(os.path.dirname(os.path.abspath(__file__))+"/../assets/"+self.URDF,self.INIT_XYZ, p.getQuaternionFromEuler(self.INIT_RPY), physicsClientId=self.CLIENT)
- if self.OBSTACLES:
- self.USER_DEFINED_FUNCTIONS.addObstacles()
-
- ####################################################################################################
- #### Draw the local frame of the drone #############################################################
- ####################################################################################################
- def _showDroneFrame(self):
- AXIS_LENGTH = 2*self.L
- self.X_AX = p.addUserDebugLine(lineFromXYZ=[0,0,0],lineToXYZ=[AXIS_LENGTH,0,0],lineColorRGB=[1,0,0],parentObjectUniqueId=self.DRONE_ID,parentLinkIndex=-1,replaceItemUniqueId=self.X_AX,physicsClientId=self.CLIENT)
- self.Y_AX = p.addUserDebugLine(lineFromXYZ=[0,0,0],lineToXYZ=[0,AXIS_LENGTH,0],lineColorRGB=[0,1,0],parentObjectUniqueId=self.DRONE_ID,parentLinkIndex=-1,replaceItemUniqueId=self.Y_AX,physicsClientId=self.CLIENT)
- self.Z_AX = p.addUserDebugLine(lineFromXYZ=[0,0,0],lineToXYZ=[0,0,AXIS_LENGTH],lineColorRGB=[0,0,1],parentObjectUniqueId=self.DRONE_ID,parentLinkIndex=-1,replaceItemUniqueId=self.Z_AX,physicsClientId=self.CLIENT)
-
- ####################################################################################################
- #### PyBullet physics implementation ###############################################################
- ####################################################################################################
- #### Arguments #####################################################################################
- #### - rpm (4-by-1 array) RPM values of the 4 motors #################################
- ####################################################################################################
- def _physics(self, rpm):
- forces = np.array(rpm**2)*self.KF
- torques = np.array(rpm**2)*self.KM
- z_torque = (-torques[0] + torques[1] - torques[2] + torques[3])
- for i in range(4): p.applyExternalForce(self.DRONE_ID, i, forceObj=[0,0,forces[i]], posObj=[0,0,0], flags=p.LINK_FRAME, physicsClientId=self.CLIENT)
- p.applyExternalTorque(self.DRONE_ID, 4, torqueObj=[0,0,z_torque], flags=p.LINK_FRAME, physicsClientId=self.CLIENT)
-
- ####################################################################################################
- #### PyBullet implementation of drag and ground effect #############################################
- ####################################################################################################
- #### Arguments #####################################################################################
- #### - rpm (4-by-1 array) RPM values of the 4 motors #################################
- ####################################################################################################
- def _simpleAerodynamicEffects(self, rpm):
- ####################################################################################################
- #### Kinematic information of the base and all links (propellers and center of mass) ###############
- ####################################################################################################
- base_pos, base_quat = p.getBasePositionAndOrientation(self.DRONE_ID, physicsClientId=self.CLIENT)
- base_rpy = p.getEulerFromQuaternion(base_quat)
- base_rot = np.array(p.getMatrixFromQuaternion(base_quat)).reshape(3,3)
- base_vel, base_ang_v = p.getBaseVelocity(self.DRONE_ID, physicsClientId=self.CLIENT)
- link_states = np.array(p.getLinkStates(self.DRONE_ID, linkIndices=[0,1,2,3,4], computeLinkVelocity=1, computeForwardKinematics=1, physicsClientId=self.CLIENT))
- ####################################################################################################
- #### Simple, per-propeller ground effects, from (Shi et al., 2019) #################################
- ####################################################################################################
- prop_heights = np.array([link_states[0,0][2], link_states[1,0][2], link_states[2,0][2], link_states[3,0][2]])
- gnd_effects = np.array(rpm**2) * self.KF * self.GND_EFF_COEFF * (self.PROP_RADIUS/(4 * prop_heights))**2
- gnd_effects = np.clip(gnd_effects, 0, self.MAX_THRUST/10)
- if np.abs(base_rpy[0]) 0.8: return -1
- elif state[2] > 0.5: return 2000
- elif state[2] > 0.3: return 1000
- elif state[2] > 0.2: return 500
- elif state[2] > 0.15: return 100
- elif state[2] > 0.1: return 10
- else: return -1
- elif self.USER == "Custom": # Use with: >>> env = SingleDroneEnv(user="Custom")
- pass
- else: print("[ERROR] in rewardFunction(), unknown user")
-
- ####################################################################################################
- #### Evaluate the current state's halting conditions ###############################################
- ####################################################################################################
- #### Arguments #####################################################################################
- #### - norm. state (20-by-1 array) clipped and normalized simulation state ####################
- #### - sim_time (Float) elapsed simulation time (in seconds) #######################
- ####################################################################################################
- #### Returns #######################################################################################
- #### - done (Boolean) whether the halting conditions of the episode are met ######
- ####################################################################################################
- def doneFunction(self, state, sim_time):
- if self.USER == "Default":
- if np.abs(state[0])>=1 or np.abs(state[1])>=1 or state[2]>=1 \
- or np.abs(state[7])>=np.pi/3 or np.abs(state[8])>=np.pi/3 \
- or np.abs(state[10])>=1 or np.abs(state[11])>=1 or np.abs(state[12])>=1 \
- or np.abs(state[13])>=10*np.pi or np.abs(state[14])>=10*np.pi or np.abs(state[15])>=20*np.pi \
- or sim_time > 3:
- done = True
- else:
- done = False
- return done
- elif self.USER == "Custom": # Use with: >>> env = SingleDroneEnv(user="Custom")
- pass
- else: print("[ERROR] in doneFunction(), unknown user")
-
- ####################################################################################################
- #### Normalize the 20 values in the simulation state to the [-1,1] range ###########################
- ####################################################################################################
- #### Arguments #####################################################################################
- #### - state (20-by-1 array) raw simulation state #######################################
- #### - step_counter (Integer) current simulation step ####################################
- ####################################################################################################
- #### Returns #######################################################################################
- #### - norm. state (20-by-1 array) clipped and normalized simulation state ####################
- ####################################################################################################
- def clipAndNormalizeState(self, state, step_counter):
- clipped_pos = np.clip(state[0:3], -1, 1)
- clipped_rp = np.clip(state[7:9], -np.pi/3, np.pi/3)
- clipped_vel = np.clip(state[10:13], -1, 1)
- clipped_ang_vel_rp = np.clip(state[13:15], -10*np.pi, 10*np.pi)
- clipped_ang_vel_y = np.clip(state[15], -20*np.pi, 20*np.pi)
- if self.GUI:
- if not(clipped_pos==np.array(state[0:3])).all(): print("[WARNING] it:", step_counter, "in clipAndNormalizeState(), out-of-bound position [{:.2f} {:.2f} {:.2f}], consider a more conservative implementation of doneFunction()".format(state[0], state[1], state[2]))
- if not(clipped_rp==np.array(state[7:9])).all(): print("[WARNING] it:", step_counter, "in clipAndNormalizeState(), out-of-bound roll/pitch [{:.2f} {:.2f}], consider a more conservative implementation of doneFunction()".format(state[7], state[8]))
- if not(clipped_vel==np.array(state[10:13])).all(): print("[WARNING] it:", step_counter, "in clipAndNormalizeState(), out-of-bound velocity [{:.2f} {:.2f} {:.2f}], consider a more conservative implementation of doneFunction()".format(state[10], state[11], state[12]))
- if not(clipped_ang_vel_rp==np.array(state[13:15])).all(): print("[WARNING] it:", step_counter, "in clipAndNormalizeState(), out-of-bound angular velocity [{:.2f} {:.2f} {:.2f}], consider a more conservative implementation of doneFunction()".format(state[13], state[14], state[15]))
- if not(clipped_ang_vel_y==np.array(state[15])): print("[WARNING] it:", step_counter, "in clipAndNormalizeState(), out-of-bound angular velocity [{:.2f} {:.2f} {:.2f}], consider a more conservative implementation of doneFunction()".format(state[13], state[14], state[15]))
- normalized_pos = clipped_pos
- normalized_rp = clipped_rp/(np.pi/3)
- normalized_y = state[9]/np.pi
- normalized_vel = clipped_vel
- normalized_ang_vel_rp = clipped_ang_vel_rp/(10*np.pi)
- normalized_ang_vel_y = clipped_ang_vel_y/(20*np.pi)
- return np.hstack([normalized_pos, state[3:7], normalized_rp, normalized_y, normalized_vel, normalized_ang_vel_rp, normalized_ang_vel_y, state[16:20] ]).reshape(20,)
-
- ####################################################################################################
- #### Add obstacles to the environment from .urdf files #############################################
- ####################################################################################################
- def addObstacles(self):
- if self.USER == "Default":
- p.loadURDF("samurai.urdf", physicsClientId=self.CLIENT)
- p.loadURDF("duck_vhacd.urdf", [-.5,-.5,.05], p.getQuaternionFromEuler([0,0,0]), physicsClientId=self.CLIENT)
- p.loadURDF("cube_no_rotation.urdf", [-.5,-2.5,.5], p.getQuaternionFromEuler([0,0,0]), physicsClientId=self.CLIENT)
- p.loadURDF("sphere2.urdf", [0,2,.5], p.getQuaternionFromEuler([0,0,0]), physicsClientId=self.CLIENT)
- elif self.USER == "Custom": # Use with: >>> env = SingleDroneEnv(user="Custom")
- pass
- else: print("[ERROR] in addObstacles(), unknown user")
-
-
-
diff --git a/gym_pybullet_drones/envs/__init__.py b/gym_pybullet_drones/envs/__init__.py
index 77f6213a7..cbc26b6b0 100644
--- a/gym_pybullet_drones/envs/__init__.py
+++ b/gym_pybullet_drones/envs/__init__.py
@@ -1,4 +1,3 @@
-from gym_pybullet_drones.envs.SingleDroneEnv import SingleDroneEnv
-from gym_pybullet_drones.envs.MultiDroneEnv import MultiDroneEnv
+from gym_pybullet_drones.envs.Aviary import Aviary
diff --git a/setup.py b/setup.py
index 1ad1f5c3a..693232cf2 100644
--- a/setup.py
+++ b/setup.py
@@ -1,6 +1,6 @@
from setuptools import setup
setup(name='gym_pybullet_drones',
- version='0.1.0',
+ version='1.0.0',
install_requires=['gym', 'pybullet', 'stable_baselines3', 'ray[rllib]']
)