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 -alt text alt text +alt text alt text - 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) +alt text alt text -- `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 ``` -alt text alt text +> Tip: use the GUI's sliders and button `Use GUI RPM` to override the control with interactive inputs +alt text alt text + +alt text alt text -- `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 - -alt text alt text +alt text alt text +alt text alt text -- `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 ``` -alt text alt text -alt text alt text - - +> 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]'] )