diff --git a/README.md b/README.md index 8d8a1eef5..e013a98cf 100644 --- a/README.md +++ b/README.md @@ -1,421 +1,23 @@ -# gym-pybullet-drones (v26 migration) +# gym-pybullet-drones -[Simple](https://en.wikipedia.org/wiki/KISS_principle) OpenAI [Gym environment](https://gym.openai.com/envs/#classic_control) based on [PyBullet](https://github.com/bulletphysics/bullet3) for multi-agent reinforcement learning with quadrotors +2023 remastering -- This repository's `master` branch is actively developed, please `git pull` frequently and feel free to open new [issues](https://github.com/utiasDSL/gym-pybullet-drones/issues) for any undesired, unexpected, or (presumably) incorrect behavior. Thanks 🙏 -- If you are interested in safe control and the companion code of ["Safe Learning in Robotics"](https://www.annualreviews.org/doi/abs/10.1146/annurev-control-042920-020211) and ["Safe Control Gym"](https://ieeexplore.ieee.org/abstract/document/9849119/), check out [`safe-control-gym`](https://github.com/utiasDSL/safe-control-gym) +## Installation -formation flight control info +TBD -- 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) - -- Everything after a `$` is entered on a terminal, everything after `>>>` is passed to a Python interpreter - -- To better understand how the PyBullet back-end works, refer to its [Quickstart Guide](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.2ye70wns7io3) - -- Suggestions and corrections are very welcome in the form of [issues](https://github.com/utiasDSL/gym-pybullet-drones/issues) and [pull requests](https://github.com/utiasDSL/gym-pybullet-drones/pulls), respectively - -> ## Why Reinforcement Learning of Quadrotor Control - -> A lot of recent RL research for continuous actions has focused on [policy gradient algorithms and actor-critic architectures](https://lilianweng.github.io/lil-log/2018/04/08/policy-gradient-algorithms.html). A quadrotor is (i) an easy-to-understand mobile robot platform whose (ii) control can be framed as a continuous states and actions problem but, beyond 1-dimension, (iii) it adds the complexity that many candidate policies lead to unrecoverable states, violating the assumption of the existence of a stationary state distribution on the entailed Markov chain. - -## Overview - -| | `gym-pybullet-drones` | [AirSim](https://github.com/microsoft/AirSim) | [Flightmare](https://github.com/uzh-rpg/flightmare) | -|---------------------------------: | :-------------------: | :-------------------------------------------: | :-------------------------------------------------: | -| *Physics* | PyBullet | FastPhysicsEngine/PhysX | *Ad hoc*/Gazebo | -| *Rendering* | PyBullet | Unreal Engine 4 | Unity | -| *Language* | Python | C++/C# | C++/Python | -| *RGB/Depth/Segm. views* | **Yes** | **Yes** | **Yes** | -| *Multi-agent control* | **Yes** | **Yes** | **Yes** | -| *ROS interface* | ROS2/Python | ROS/C++ | ROS/C++ | -| *Hardware-In-The-Loop* | No | **Yes** | No | -| *Fully steppable physics* | **Yes** | No | **Yes** | -| *Aerodynamic effects* | Drag, downwash, ground| Drag | Drag | -| *OpenAI [`Gym`](https://github.com/openai/gym/blob/master/gym/core.py) interface* | **Yes** | **[Yes](https://github.com/microsoft/AirSim/pull/3215)** | **Yes** | -| *RLlib [`MultiAgentEnv`](https://github.com/ray-project/ray/blob/master/rllib/env/multi_agent_env.py) interface* | **Yes** | No | No | - -## Performance - -Simulation **speed-up with respect to the wall-clock** when using - -- *240Hz* (in simulation clock) PyBullet physics for **EACH** drone -- **AND** *48Hz* (in simulation clock) PID control of **EACH** drone -- **AND** nearby *obstacles* **AND** a mildly complex *background* (see GIFs) -- **AND** *24FPS* (in sim. clock), *64x48 pixel* capture of *6 channels* (RGBA, depth, segm.) on **EACH** drone - -| | Lenovo P52 (i7-8850H/Quadro P2000) | 2020 MacBook Pro (i7-1068NG7) | -| --------------------------------: | :--------------------------------: | :---------------------------: | -| Rendering | OpenGL | CPU-based TinyRenderer | -| Single drone, **no** vision | 15.5x | 16.8x | -| Single drone **with** vision | 10.8x | 1.3x | -| Multi-drone (10), **no** vision | 2.1x | 2.3x | -| Multi-drone (5) **with** vision | 2.5x | 0.2x | -| 80 drones in 4 env, **no** vision | 0.8x | 0.95x | - -> **Note: use `gui=False` and `aggregate_phy_steps=int(SIM_HZ/CTRL_HZ)` for better performance** - -> While it is easy to—consciously or not—[cherry pick](https://en.wikipedia.org/wiki/Cherry_picking) statistics, \~5kHz PyBullet physics (CPU-only) is faster than [AirSim (1kHz)](https://arxiv.org/pdf/1705.05065.pdf) and more accurate than [Flightmare's 35kHz simple single quadcopter dynamics](https://arxiv.org/pdf/2009.00563.pdf) - -> Exploiting parallel computation—i.e., multiple (80) drones in multiple (4) environments (see script [`parallelism.sh`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/experiments/performance/parallelism.sh))—achieves PyBullet physics updates at \~20kHz - -> Multi-agent 6-ch. video capture at \~750kB/s with CPU rendering (`(64*48)*(4+4+2)*24*5*0.2`) is comparable to [Flightmare's 240 RGB frames/s](https://arxiv.org/pdf/2009.00563.pdf) (`(32*32)*3*240`)—although in more complex [Unity environments](https://arxiv.org/pdf/2009.00563.pdf)—and up to an order of magnitude faster on Ubuntu, with OpenGL rendering - -## Requirements and Installation - -The repo was written using *Python 3.7* with [`conda`](https://github.com/JacopoPan/a-minimalist-guide#install-conda) on *macOS 10.15* and tested with *Python 3.8* on *macOS 12*, *Ubuntu 20.04* - -### On *macOS* and *Ubuntu* - -Major dependencies are [`gym`](https://gym.openai.com/docs/), [`pybullet`](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#), -[`stable-baselines3`](https://stable-baselines3.readthedocs.io/en/master/guide/quickstart.html), and [`rllib`](https://docs.ray.io/en/master/rllib.html) - -Video recording requires to have [`ffmpeg`](https://ffmpeg.org) installed, on *macOS* - -```bash -$ brew install ffmpeg -``` - -On *Ubuntu* - -```bash -$ sudo apt install ffmpeg -``` - - - -The repo is structured as a [Gym Environment](https://github.com/openai/gym/blob/master/docs/creating-environments.md) -and can be installed with `pip install --editable` - -```bash -$ conda create -n drones python=3.8 -$ conda activate drones -$ pip3 install --upgrade pip -$ git clone https://github.com/utiasDSL/gym-pybullet-drones.git -$ cd gym-pybullet-drones/ -$ pip3 install -e . -``` - - -### On *Windows* - -Check these step-by-step [instructions](https://github.com/utiasDSL/gym-pybullet-drones/tree/master/assignments#on-windows) written by Dr. Karime Pereida for *Windows 10* - -### On *Colab* - -Try the example scritps: -[`fly.py`](https://colab.research.google.com/drive/1hJlJElUuveD4U_GDGuNsX8NqDcl3jUGz?usp=sharing), -[`learn.py`](https://colab.research.google.com/drive/1lLGAET4xx-7gGznanfGe0bQy4H7O9ScL?usp=sharing), -[`downwash.py`](https://colab.research.google.com/drive/1Oj_RzJ5M_g4KrKFRJvcAhh62GJo78m9F?usp=sharing), -[`compare.py`](https://colab.research.google.com/drive/1RzY6jG5F7ddknuyssI486TdMnOfq9Cjf?usp=sharing), -[`ground_effect`](https://colab.research.google.com/drive/1BpLqPXnfk6lKiQ6YSNW74UQJ2MB4KwYJ?usp=sharing), and [`velocity`](https://colab.research.google.com/drive/1KN-fgwF3qjOCSIexHyQKBZ-rirtpt6ng?usp=sharing) contributed by [Spencer Teetaert](https://github.com/spencerteetaert) - -## Examples - -There are 2 basic template scripts in [`gym_pybullet_drones/examples/`](https://github.com/utiasDSL/gym-pybullet-drones/tree/master/gym_pybullet_drones/examples): `fly.py` and `learn.py` - -- `fly.py` [[try it on Colab](https://colab.research.google.com/drive/1hJlJElUuveD4U_GDGuNsX8NqDcl3jUGz?usp=sharing)] runs an independent flight **using PID control** implemented in class [`DSLPIDControl`](https://github.com/utiasDSL/gym-pybullet-drones/tree/master/gym_pybullet_drones/control/DSLPIDControl.py) - -```bash -$ cd gym-pybullet-drones/gym_pybullet_drones/examples/ -$ python3 fly.py # Try 'python3 fly.py -h' to show the script's customizable parameters -``` - -> Tip: use the GUI's sliders and button `Use GUI RPM` to override the control with interactive inputs - -sparse way points flight control info - -yaw saturation control info - -- `learn.py` [[try it on Colab](https://colab.research.google.com/drive/1lLGAET4xx-7gGznanfGe0bQy4H7O9ScL?usp=sharing)] is an **RL example** to 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) - -```bash -$ cd gym-pybullet-drones/gym_pybullet_drones/examples/ -$ python3 learn.py # Try 'python3 learn.py -h' to show the script's customizable parameters +```sh +tbd ``` -learning 1 learning 2 -learning 3 learning 4 - -Other scripts in folder [`gym_pybullet_drones/examples/`](https://github.com/utiasDSL/gym-pybullet-drones/tree/master/gym_pybullet_drones/examples) are +## Use -- `downwash.py` [[try it on Colab](https://colab.research.google.com/drive/1Oj_RzJ5M_g4KrKFRJvcAhh62GJo78m9F?usp=sharing)] is a flight script with only 2 drones, to test the downwash model +TBD -```bash -$ cd gym-pybullet-drones/gym_pybullet_drones/examples/ -$ python3 downwash.py # Try 'python3 downwash.py -h' to show the script's customizable parameters +```sh +tbd ``` -downwash example control info - -- `compare.py` [[try it on Colab](https://colab.research.google.com/drive/1RzY6jG5F7ddknuyssI486TdMnOfq9Cjf?usp=sharing)] which replays and compare to a trace saved in [`example_trace.pkl`](https://github.com/utiasDSL/gym-pybullet-drones/tree/master/files/example_trace.pkl) - -```bash -$ cd gym-pybullet-drones/gym_pybullet_drones/examples/ -$ python3 compare.py # Try 'python3 compare.py -h' to show the script's customizable parameters -``` - -pid flight on sine trajectroy control info - -## Experiments - -Folder [`experiments/learning`](https://github.com/utiasDSL/gym-pybullet-drones/tree/master/experiments/learning) contains scripts with template learning pipelines - -For single agent RL problems, using [`stable-baselines3`](https://stable-baselines3.readthedocs.io/en/master/guide/quickstart.html), run the [training script](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/experiments/learning/singleagent.py) as - -```bash -$ cd gym-pybullet-drones/experiments/learning/ -$ python3 singleagent.py --env --algo --obs --act --cpu -``` - -Run the [replay script](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/experiments/learning/test_singleagent.py) to visualize the best trained agent(s) as - -```bash -$ python3 test_singleagent.py --exp ./results/save----- -``` - -For multi-agent RL problems, using [`rllib`](https://docs.ray.io/en/master/rllib.html) run the [train script](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/experiments/learning/multiagent.py) as - -```bash -$ cd gym-pybullet-drones/experiments/learning/ -$ python3 multiagent.py --num_drones --env --obs --act --algo --num_workers -``` - -Run the [replay script](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/experiments/learning/test_multiagent.py) to visualize the best trained agent(s) as - -```bash -$ python3 test_multiagent.py --exp ./results/save------ -``` - -## Class `BaseAviary` - -A flight arena for one (ore more) quadrotor can be created as a subclass of `BaseAviary()` - -```python ->>> env = BaseAviary( ->>> drone_model=DroneModel.CF2X, # See DroneModel Enum class for other quadcopter models ->>> num_drones=1, # Number of drones ->>> neighbourhood_radius=np.inf, # Distance at which drones are considered neighbors, only used for multiple drones ->>> initial_xyzs=None, # Initial XYZ positions of the drones ->>> initial_rpys=None, # Initial roll, pitch, and yaw of the drones in radians ->>> physics: Physics=Physics.PYB, # Choice of (PyBullet) physics implementation ->>> freq=240, # Stepping frequency of the simulation ->>> aggregate_phy_steps=1, # Number of physics updates within each call to BaseAviary.step() ->>> gui=True, # Whether to display PyBullet's GUI, only use this for debbuging ->>> record=False, # Whether to save a .mp4 video (if gui=True) or .png frames (if gui=False) in gym-pybullet-drones/files/, see script /files/videos/ffmpeg_png2mp4.sh for encoding ->>> obstacles=False, # Whether to add obstacles to the environment ->>> user_debug_gui=True) # Whether to use addUserDebugLine and addUserDebugParameter calls (it can slow down the GUI) -```` - -And instantiated with `gym.make()`—see [`learn.py`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/examples/learn.py) for an example - -```python ->>> env = gym.make('rl-takeoff-aviary-v0') # See learn.py -``` - -Then, the environment can be stepped with - -```python ->>> obs = env.reset() ->>> for _ in range(10*240): ->>> obs, reward, done, info = env.step(env.action_space.sample()) ->>> env.render() ->>> if done: obs = env.reset() ->>> env.close() -``` - -### Creating New Aviaries - -A new RL problem can be created as a subclass of [`BaseAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/BaseAviary.py) (i.e. `class NewAviary(BaseAviary): ...`) and implementing the following 7 abstract methods - -```python ->>> #### 1 ->>> def _actionSpace(self): ->>> # e.g. return spaces.Box(low=np.zeros(4), high=np.ones(4), dtype=np.float32) ->>> #### 2 ->>> def _observationSpace(self): ->>> # e.g. return spaces.Box(low=np.zeros(20), high=np.ones(20), dtype=np.float32) ->>> #### 3 ->>> def _computeObs(self): ->>> # e.g. return self._getDroneStateVector(0) ->>> #### 4 ->>> def _preprocessAction(self, action): ->>> # e.g. return np.clip(action, 0, 1) ->>> #### 5 ->>> def _computeReward(self): ->>> # e.g. return -1 ->>> #### 6 ->>> def _computeDone(self): ->>> # e.g. return False ->>> #### 7 ->>> def _computeInfo(self): ->>> # e.g. return {"answer": 42} # Calculated by the Deep Thought supercomputer in 7.5M years -``` - -See [`CtrlAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/CtrlAviary.py), [`VisionAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/VisionAviary.py), [`HoverAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/single_agent_rl/HoverAviary.py), and [`FlockAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/multi_agent_rl/FlockAviary.py) for examples - -### Action Spaces Examples - -The action space's definition of an environment must be implemented in each subclass of [`BaseAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/BaseAviary.py) by function - -```python ->>> def _actionSpace(self): ->>> ... -``` - -In [`CtrlAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/CtrlAviary.py) and [`VisionAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/VisionAviary.py), it 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) containing the drones' commanded RPMs - -The dictionary's keys are `"0"`, `"1"`, .., `"n"`—where `n` is the number of drones - -Each subclass of [`BaseAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/BaseAviary.py) also needs to implement a preprocessing step translating actions into RPMs - -```python ->>> def _preprocessAction(self, action): ->>> ... -``` - -[`CtrlAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/CtrlAviary.py), [`VisionAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/VisionAviary.py), [`HoverAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/single_agent_rl/HoverAviary.py), and [`FlockAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/multi_agent_rl/FlockAviary.py) all simply clip the inputs to `MAX_RPM` - -[`DynAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/DynAviary.py)'s `action` input to `DynAviary.step()` 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) containing - -- The desired thrust along the drone's z-axis -- The desired torque around the drone's x-axis -- The desired torque around the drone's y-axis -- The desired torque around the drone's z-axis - -From these, desired RPMs are computed by [`DynAviary._preprocessAction()`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/DynAviary.py) - -### Observation Spaces Examples - -The observation space's definition of an environment must be implemented by every subclass of [`BaseAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/BaseAviary.py) - -```python ->>> def _observationSpace(self): ->>> ... -``` - -In [`CtrlAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/CtrlAviary.py), it is a [`Dict()`](https://github.com/openai/gym/blob/master/gym/spaces/dict.py) of pairs `{"state": Box(20,), "neighbors": MultiBinary(num_drones)}` - -The dictionary's 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` (in meters, 3 values) -- Quaternion orientation in `WORLD_FRAME` (4 values) -- Roll, pitch and yaw angles in `WORLD_FRAME` (in radians, 3 values) -- The velocity vector in `WORLD_FRAME` (in m/s, 3 values) -- Angular velocity in `WORLD_FRAME` (3 values) -- Motors' speeds (in RPMs, 4 values) - -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](https://en.wikipedia.org/wiki/Adjacency_matrix) - -The observation space of [`VisionAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/VisionAviary.py) is the same as[`CtrlAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/CtrlAviary.py) but also includes keys `rgb`, `dep`, and `seg` (in each drone's dictionary) for the matrices containing the drone's RGB, depth, and segmentation views - -To fill/customize the content of `obs`, every subclass of [`BaseAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/BaseAviary.py) needs to implement - -```python ->>> def _computeObs(self, action): ->>> ... -``` - -See [`BaseAviary._exportImage()`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/BaseAviary.py)) and its use in [`VisionAviary._computeObs()`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/VisionAviary.py) to save frames as PNGs - -### Obstacles - -Objects can be added to an environment using [`loadURDF` (or `loadSDF`, `loadMJCF`)](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.sbnykoneq1me) in method `_addObstacles()` - -```python ->>> def _addObstacles(self): ->>> ... ->>> p.loadURDF("sphere2.urdf", [0,0,0], p.getQuaternionFromEuler([0,0,0]), physicsClientId=self.CLIENT) -``` - -### Drag, Ground Effect, and Downwash Models - -Simple drag, ground effect, and downwash models can be included in the simulation initializing `BaseAviary()` 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) (Eq. 4.2), the analytical model used as a baseline for comparison by [Shi et al. (2019)](https://arxiv.org/pdf/1811.08027.pdf) (Eq. 15), and [DSL](https://www.dynsyslab.org/vision-news/)'s experimental work - -Check the implementations of `_drag()`, `_groundEffect()`, and `_downwash()` in [`BaseAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/BaseAviary.py) for more detail - -## RGB, Depth, and Segmentation Views - -rgb view depth view segmentation view - -## PID Control - -Folder [`control`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/control/) contains the implementations of 2 PID controllers - -[`DSLPIDControl`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/control/DSLPIDControl.py) (for `DroneModel.CF2X/P`) and [`SimplePIDControl`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/control/SimplePIDControl.py) (for `DroneModel.HB`) can be used as - -```python ->>> ctrl = [DSLPIDControl(drone_model=DroneModel.CF2X) 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(i)]["state"], ->>> target_pos=TARGET_POS) -``` - -For high-level coordination—using a *velocity input*—[`VelocityAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/VelocityAviary.py) integrates PID control within a `gym.Env`. - -Method [`setPIDCoefficients`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/control/BaseControl.py) can be used to change the coefficients of one of the given PID controllers—and, for example, implement learning problems whose goal is parameter tuning (see [`TuneAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/single_agent_rl/TuneAviary.py)). - -## Logger - -Class [`Logger`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/utils/Logger.py) contains helper functions to save and plot simulation data, as in this example - -```python ->>> logger = Logger(logging_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 -``` - -## ROS2 Python Wrapper - -Workspace [`ros2`](https://github.com/utiasDSL/gym-pybullet-drones/tree/master/ros2) contains two [ROS2 Foxy Fitzroy](https://index.ros.org/doc/ros2/Installation/Foxy/) Python nodes - -- [`AviaryWrapper`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/aviary_wrapper.py) is a wrapper node for a single-drone [`CtrlAviary`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/gym_pybullet_drones/envs/CtrlAviary.py) environment -- [`RandomControl`](https://github.com/utiasDSL/gym-pybullet-drones/blob/master/ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/random_control.py) reads `AviaryWrapper`'s `obs` topic and publishes random RPMs on topic `action` - -With ROS2 installed (on either macOS or Ubuntu, edit `ros2_and_pkg_setups.(zsh/bash)` accordingly), run - -```bash -$ cd gym-pybullet-drones/ros2/ -$ source ros2_and_pkg_setups.zsh # On macOS, on Ubuntu use $ source ros2_and_pkg_setups.bash - # Note that the second line in the script will throw an error (until you run calcon) that you can ignore -$ colcon build --packages-select ros2_gym_pybullet_drones -$ source ros2_and_pkg_setups.zsh # On macOS, on Ubuntu use $ source ros2_and_pkg_setups.bash -$ ros2 run ros2_gym_pybullet_drones aviary_wrapper -``` - -In a new terminal terminal, run - -```bash -$ cd gym-pybullet-drones/ros2/ -$ source ros2_and_pkg_setups.zsh # On macOS, on Ubuntu use $ source ros2_and_pkg_setups.bash -$ ros2 run ros2_gym_pybullet_drones random_control -``` - -## TODOs (January 2022) - -- Test and update ROS 2 instrucitons for [Humble Hawksbill](https://docs.ros.org/en/foxy/Releases/Release-Humble-Hawksbill.html) - ## Citation If you wish, please cite our work [(link)](https://arxiv.org/abs/2103.02142) as @@ -448,9 +50,5 @@ If you wish, please cite our work [(link)](https://arxiv.org/abs/2103.02142) as - C. Karen Liu and Dan Negrut (2020) [*The Role of Physics-Based Simulators in Robotics*](https://www.annualreviews.org/doi/pdf/10.1146/annurev-control-072220-093055) - Yunlong Song, Selim Naji, Elia Kaufmann, Antonio Loquercio, and Davide Scaramuzza (2020) [*Flightmare: A Flexible Quadrotor Simulator*](https://arxiv.org/pdf/2009.00563.pdf) -Bonus GIF for scrolling this far - -formation flight control info - ----- -> University of Toronto's [Dynamic Systems Lab](https://github.com/utiasDSL) / [Vector Institute](https://github.com/VectorInstitute) / University of Cambridge's [Prorok Lab](https://github.com/proroklab) / [Mitacs](https://www.mitacs.ca/en/projects/multi-agent-reinforcement-learning-decentralized-uavugv-cooperative-exploration) +> University of Toronto's [Dynamic Systems Lab](https://github.com/utiasDSL) / [Vector Institute](https://github.com/VectorInstitute) / University of Cambridge's [Prorok Lab](https://github.com/proroklab) diff --git a/assignments/README.md b/assignments/README.md deleted file mode 100644 index cee3b814d..000000000 --- a/assignments/README.md +++ /dev/null @@ -1,104 +0,0 @@ -# AER1216 Fall 2020 -Quadcopter control exercises for University of Toronto's Fall 2020 course *AER1216: Fundamentals of Unmanned Aerial Vehicles* - -## On Ubuntu or macOS - -The repo was written using *Python 3.7* with [`conda`](https://github.com/JacopoPan/a-minimalist-guide#install-conda) on *macOS 10.15* and tested on *macOS 11*, *Ubuntu 18.04* - -### Requirements -In a terminal, type -``` -pip3 install --upgrade numpy Pillow matplotlib cycler -pip3 install --upgrade gym pybullet stable_baselines3 'ray[rllib]' -``` -On *macOS* -``` -$ brew install ffmpeg -``` -On *Ubuntu* -``` -$ sudo apt install ffmpeg -``` - -### Installation -In a terminal, type -``` -$ wget https://github.com/utiasDSL/gym-pybullet-drones/archive/v0.5.2.zip -$ unzip v0.5.2.zip -$ cd gym-pybullet-drones-0.5.2/ -$ pip3 install -e . -``` - -### Use -In a terminal, type -``` -$ cd assignments/ -$ python3 aer1216_fall2020_hw1_sim.py # e.g., for hw1 -``` - -### Troubleshooting -On Ubuntu and macOS, contact `jacopo.panerati {at} utoronto.ca` - -## On Windows - -### Requirements - -Download Visual Studio and [C++ 14.0](https://visualstudio.microsoft.com/downloads/) -- We recommend the free Community version -- Select "Desktop development with C++" - -Download [Python 3](https://www.python.org/downloads/release/python-390/) -- Note: we used the [Windows x86-64 installer](https://www.python.org/ftp/python/3.9.0/python-3.9.0-amd64.exe) on Windows 10 Home - -Download a Python IDE -- We recommend [PyCharm Community](https://www.jetbrains.com/pycharm/download/#section=windows) -- Select all the options in the installer and reboot - -### Installation - -Download the code -- Go to https://github.com/utiasDSL/gym-pybullet-drones/releases -- Under "Releases" -> `v0.5.2`, click on "Assets" and download the source code (zip or tar.gz) - -source code - -Unzip and open folder `gym-pybullet-drones-v0.5.2` in PyCharm - -Open `aer1216_fall2020_hw1_sim.py` (or `aer1216_fall2020_hw2_sim.py`) in PyCharm - -To run code you may need to configure PyCharm. -- Go to `File->Settings` and Select `Project:gym-pybullet-drones.py->Python Interpreter` - -interpreter - -- Select the `+` - -> Type `numpy` and click "Install package". - -> Type `matplotlib` and click "Install package". - -> Type `pybullet` and click "Install package". - -> Type `gym` and click "Install package". - -> Type `Pillow` and click "Install package". - -> Type `Cycler` and click "Install package". - -### Use - -If (as) you did not install the `gym_pybullet_drones` module, uncomment lines 17-18 -``` ->>> import sys ->>> sys.path.append('../') -``` -in file `aer1216_fall2020_hw1_sim.py` (or `aer1216_fall2020_hw2_sim.py`) - -Go to the "Run" drop down menu and select "Run" - -run - -- Select `aer1216_fall2020_hw1_sim.py` (or `aer1216_fall2020_hw2_sim.py`) to start the simulation - -### Troubleshooting -On Windows, contact `karime.pereida {at} robotics.utias.utoronto.ca` diff --git a/assignments/aer1216_fall2020_hw1_ctrl.py b/assignments/aer1216_fall2020_hw1_ctrl.py deleted file mode 100644 index 2a5b2d2df..000000000 --- a/assignments/aer1216_fall2020_hw1_ctrl.py +++ /dev/null @@ -1,137 +0,0 @@ -"""Control implementation for assignment 1. - -The controller used the simulation in file `aer1216_fall2020_hw1_sim.py`. - -Example -------- -To run the simulation, type in a terminal: - - $ python aer1216_fall2020_hw1_sim.py - -Notes ------ -Tune the PD coefficients in `HW1Control.__init__()`. - -""" -import numpy as np -from gym_pybullet_drones.envs.BaseAviary import BaseAviary - -class HW1Control(): - """Control class for assignment 1.""" - - ################################################################################ - - def __init__(self, - env: BaseAviary - ): - """ Initialization of class HW1Control. - - Parameters - ---------- - env : BaseAviary - The PyBullet-based simulation environment. - - """ - self.g = env.G - """float: Gravity acceleration, in meters per second squared.""" - self.mass = env.M - """float: The mass of quad from environment.""" - self.timestep = env.TIMESTEP - """float: Simulation and control timestep.""" - self.kf_coeff = env.KF - """float: RPMs to force coefficient.""" - self.km_coeff = env.KM - """float: RPMs to torque coefficient.""" - - ############################################################ - ############################################################ - #### HOMEWORK CODE (START) ################################# - ############################################################ - ############################################################ - self.p_coeff_position = 0.7 * 0.7 - """float: Proportional coefficient for position control.""" - self.d_coeff_position = 2 * 0.7 * 0.7 - """float: Derivative coefficient for position control.""" - ############################################################ - ############################################################ - #### HOMEWORK CODE (END) ################################### - ############################################################ - ############################################################ - - self.reset() - - ################################################################################ - - def reset(self): - """ Resets the controller counter.""" - self.control_counter = 0 - - ################################################################################ - - def compute_control(self, - current_position, - current_velocity, - target_position, - target_velocity=np.zeros(3), - target_acceleration=np.zeros(3), - ): - """Compute the propellers' RPMs for the target state, given the current state. - - Parameters - ---------- - current_position : ndarray - (3,)-shaped array of floats containing global x, y, z, in meters. - current_velocity : ndarray - (3,)-shaped array of floats containing global vx, vy, vz, in m/s. - target_position : ndarray - (3,)-shaped array of float containing global x, y, z, in meters. - target_velocity : ndarray, optional - (3,)-shaped array of floats containing global, in m/s. - target_acceleration : ndarray, optional - (3,)-shaped array of floats containing global, in m/s^2. - - Returns - ------- - ndarray - (4,)-shaped array of ints containing the desired RPMs of each propeller. - """ - self.control_counter += 1 - - ############################################################ - ############################################################ - #### HOMEWORK CODE (START) ################################# - ############################################################ - ############################################################ - - ##### Calculate position and velocity errors ############### - current_pos_error = target_position[2] - current_position[2] - current_vel_error = target_velocity[2] - current_velocity[2] - - #### Calculate input with a PD controller ################## - # u = desired_acceleration + Kv * velocity_error + Kp * position_error - u = target_acceleration[2] \ - + self.d_coeff_position * current_vel_error \ - + self.p_coeff_position * current_pos_error - - ##### Calculate propeller turn rates given the PD input #### - # turn_rate = sqrt( (m*u + m*g) / (4*Kf) ) - propellers_rpm = np.sqrt((u*self.mass + self.g*self.mass) / (4 * self.kf_coeff)) - - # For up-down motion, assign the same turn rates to all motors - propellers_0_and_3_rpm, propellers_1_and_2_rpm = propellers_rpm, propellers_rpm - ############################################################ - ############################################################ - #### HOMEWORK CODE (END) ################################### - ############################################################ - ############################################################ - - #### Print relevant output ################################# - if self.control_counter%(1/self.timestep) == 0: - print("current_position", current_position) - print("current_velocity", current_velocity) - print("target_position", target_position) - print("target_velocity", target_velocity) - print("target_acceleration", target_acceleration) - - return np.array([propellers_0_and_3_rpm, propellers_1_and_2_rpm, - propellers_1_and_2_rpm, propellers_0_and_3_rpm]) diff --git a/assignments/aer1216_fall2020_hw1_sim.py b/assignments/aer1216_fall2020_hw1_sim.py deleted file mode 100644 index 79947877c..000000000 --- a/assignments/aer1216_fall2020_hw1_sim.py +++ /dev/null @@ -1,102 +0,0 @@ -"""Simulation script for assignment 1. - -The script uses the control defined in file `aer1216_fall2020_hw1_ctrl.py`. - -Example -------- -To run the simulation, type in a terminal: - - $ python aer1216_fall2020_hw1_sim.py - -""" -import time -import random -import numpy as np -import pybullet as p - -#### Uncomment the following 2 lines if "module gym_pybullet_drones cannot be found" -# import sys -# sys.path.append('../') - -from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary -from gym_pybullet_drones.utils.Logger import Logger -from gym_pybullet_drones.utils.utils import sync -from aer1216_fall2020_hw1_ctrl import HW1Control - -DURATION = 10 -"""int: The duration of the simulation in seconds.""" -GUI = True -"""bool: Whether to use PyBullet graphical interface.""" -RECORD = False -"""bool: Whether to save a video under /files/videos. Requires ffmpeg""" - -if __name__ == "__main__": - - #### Create the ENVironment ################################ - ENV = CtrlAviary(gui=GUI, record=RECORD) - PYB_CLIENT = ENV.getPyBulletClient() - - #### Initialize the LOGGER ################################# - LOGGER = Logger(logging_freq_hz=ENV.SIM_FREQ) - - #### Initialize the controller ############################# - CTRL = HW1Control(ENV) - - #### Initialize the ACTION ################################# - ACTION = {} - OBS = ENV.reset() - STATE = OBS["0"]["state"] - ACTION["0"] = CTRL.compute_control(current_position=STATE[0:3], - current_velocity=STATE[10:13], - target_position=STATE[0:3], - target_velocity=np.zeros(3), - target_acceleration=np.zeros(3) - ) - - #### Initialize target trajectory ########################## - TARGET_POSITION = np.array([[0, 0, 1.0] for i in range(DURATION*ENV.SIM_FREQ)]) - TARGET_VELOCITY = np.zeros([DURATION * ENV.SIM_FREQ, 3]) - TARGET_ACCELERATION = np.zeros([DURATION * ENV.SIM_FREQ, 3]) - - #### Derive the target trajectory to obtain target velocities and accelerations - TARGET_VELOCITY[1:, :] = (TARGET_POSITION[1:, :] - TARGET_POSITION[0:-1, :]) / ENV.SIM_FREQ - TARGET_ACCELERATION[1:, :] = (TARGET_VELOCITY[1:, :] - TARGET_VELOCITY[0:-1, :]) / ENV.SIM_FREQ - - #### Run the simulation #################################### - START = time.time() - for i in range(0, DURATION*ENV.SIM_FREQ): - - ### Secret control performance booster ##################### - # if i/ENV.SIM_FREQ>3 and i%30==0 and i/ENV.SIM_FREQ<10: p.loadURDF("duck_vhacd.urdf", [random.gauss(0, 0.3), random.gauss(0, 0.3), 3], p.getQuaternionFromEuler([random.randint(0, 360),random.randint(0, 360),random.randint(0, 360)]), physicsClientId=PYB_CLIENT) - - #### Step the simulation ################################### - OBS, _, _, _ = ENV.step(ACTION) - - #### Compute control ####################################### - STATE = OBS["0"]["state"] - ACTION["0"] = CTRL.compute_control(current_position=STATE[0:3], - current_velocity=STATE[10:13], - target_position=TARGET_POSITION[i, :], - target_velocity=TARGET_VELOCITY[i, :], - target_acceleration=TARGET_ACCELERATION[i, :] - ) - - #### Log the simulation #################################### - LOGGER.log(drone=0, timestamp=i/ENV.SIM_FREQ, state=STATE) - - #### Printout ############################################## - if i%ENV.SIM_FREQ == 0: - 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/assignments/aer1216_fall2020_hw2_ctrl.py b/assignments/aer1216_fall2020_hw2_ctrl.py deleted file mode 100644 index ffcba1908..000000000 --- a/assignments/aer1216_fall2020_hw2_ctrl.py +++ /dev/null @@ -1,274 +0,0 @@ -"""Control implementation for assignment 2. - -The controller used the simulation in file `aer1216_fall2020_hw2_sim.py`. - -Example -------- -To run the simulation, type in a terminal: - - $ python aer1216_fall2020_hw2_sim.py - -Notes ------ -To-dos - Search for word "Objective" this file (there are 4 occurrences) - Fill appropriate values in the 3 by 3 matrix self.matrix_u2rpm. - Compute u_1 for the linear controller and the second nonlinear one - Compute u_2 - -""" -import numpy as np -from gym_pybullet_drones.envs.BaseAviary import BaseAviary - -class HW2Control(): - """Control class for assignment 2.""" - - ################################################################################ - - def __init__(self, - env: BaseAviary, - control_type: int=0 - ): - """ Initialization of class HW2Control. - - Parameters - ---------- - env : BaseAviary - The PyBullet-based simulation environment. - control_type : int, optional - Choose between implementation of the u1 computation. - - """ - self.g = env.G - """float: Gravity acceleration, in meters per second squared.""" - self.mass = env.M - """float: The mass of quad from environment.""" - self.inertia_xx = env.J[0][0] - """float: The inertia of quad around x axis.""" - self.arm_length = env.L - """float: The inertia of quad around x axis.""" - self.timestep = env.TIMESTEP - """float: Simulation and control timestep.""" - self.last_rpy = np.zeros(3) - """ndarray: Store the last roll, pitch, and yaw.""" - self.kf_coeff = env.KF - """float: RPMs to force coefficient.""" - self.km_coeff = env.KM - """float: RPMs to torque coefficient.""" - self.CTRL_TYPE = control_type - """int: Flag switching beween implementations of u1.""" - self.p_coeff_position = {} - """dict[str, float]: Proportional coefficient(s) for position control.""" - self.d_coeff_position = {} - """dict[str, float]: Derivative coefficient(s) for position control.""" - - ############################################################ - ############################################################ - #### HOMEWORK CODE (START) ################################# - ############################################################ - ############################################################ - - # Objective 1 of 4: fill appropriate values in the 3 by 3 matrix - self.matrix_u2rpm = np.array([ [2, 1, 1], - [0, 1, -1], - [2, -1, -1] - ]) - """ndarray: (3, 3)-shaped array of ints to determine motor rpm from force and torque.""" - - ############################################################ - ############################################################ - #### HOMEWORK CODE (END) ################################### - ############################################################ - ############################################################ - - self.matrix_u2rpm_inv = np.linalg.inv(self.matrix_u2rpm) - - self.p_coeff_position["z"] = 0.7 * 0.7 - self.d_coeff_position["z"] = 2 * 0.5 * 0.7 - # - self.p_coeff_position["y"] = 0.7 * 0.7 - self.d_coeff_position["y"] = 2 * 0.5 * 0.7 - # - self.p_coeff_position["r"] = 0.7 * 0.7 - self.d_coeff_position["r"] = 2 * 2.5 * 0.7 - - self.reset() - - ################################################################################ - - def reset(self): - """ Resets the controller counter.""" - self.control_counter = 0 - - ################################################################################ - - def compute_control(self, - current_position, - current_velocity, - current_rpy, - target_position, - target_velocity=np.zeros(3), - target_acceleration=np.zeros(3), - ): - """Computes the propellers' RPMs for the target state, given the current state. - - Parameters - ---------- - current_position : ndarray - (3,)-shaped array of floats containing global x, y, z, in meters. - current_velocity : ndarray - (3,)-shaped array of floats containing global vx, vy, vz, in m/s. - current_rpy : ndarray - (3,)-shaped array of floats containing roll, pitch, yaw, in rad. - target_position : ndarray - (3,)-shaped array of float containing global x, y, z, in meters. - target_velocity : ndarray, optional - (3,)-shaped array of floats containing global, in m/s. - target_acceleration : ndarray, optional - (3,)-shaped array of floats containing global, in m/s^2. - - Returns - ------- - ndarray - (4,)-shaped array of ints containing the desired RPMs of each propeller. - """ - self.control_counter += 1 - - #### Compute roll, pitch, and yaw rates #################### - current_rpy_dot = (current_rpy - self.last_rpy) / self.timestep - - ##### Calculate PD control in y, z ######################### - y_ddot = self.pd_control(target_position[1], - current_position[1], - target_velocity[1], - current_velocity[1], - target_acceleration[1], - "y" - ) - z_ddot = self.pd_control(target_position[2], - current_position[2], - target_velocity[2], - current_velocity[2], - target_acceleration[2], - "z" - ) - - ##### Calculate desired roll and rates given by PD ######### - desired_roll = -y_ddot / self.g - desired_roll_dot = (desired_roll - current_rpy[0]) / 0.004 - self.old_roll = desired_roll - self.old_roll_dot = desired_roll_dot - roll_ddot = self.pd_control(desired_roll, - current_rpy[0], - desired_roll_dot, - current_rpy_dot[0], - 0, - "r" - ) - - ############################################################ - ############################################################ - #### HOMEWORK CODE (START) ################################# - ############################################################ - ############################################################ - - # Variables that you might use - # self.g - # self.mass - # self.inertia_xx - # y_ddot - # z_ddot - # roll_ddot - # current_rpy[0], current_rpy[1], current_rpy[2] - # Basic math and NumPy - # sine(x) -> np.sin(x) - # cosine(x) -> np.cos(x) - # x squared -> x**2 - # square root of x -> np.sqrt(x) - - ##### Calculate thrust and moment given the PD input ####### - if self.CTRL_TYPE == 0: - #### Linear Control ######################################## - # Objective 2 of 4: compute u_1 for the linear controller - u_1 = self.mass * (self.g + z_ddot) - - elif self.CTRL_TYPE == 1: - #### Nonlinear Control 1 ################################### - u_1 = self.mass * (self.g + z_ddot) / np.cos(current_rpy[0]) - - elif self.CTRL_TYPE == 2: - #### Nonlinear Control 2 ################################### - # Objective 3 of 4: compute u_1 for the second nonlinear controller - u_1 = self.mass * np.sqrt(y_ddot**2+(self.g + z_ddot)**2) - - # Objective 4 of 4: compute u_2 - u_2 = self.inertia_xx * roll_ddot - - ############################################################ - ############################################################ - #### HOMEWORK CODE (END) ################################### - ############################################################ - ############################################################ - - ##### Calculate RPMs ####################################### - u = np.array([ [u_1 / self.kf_coeff], - [u_2 / (self.arm_length*self.kf_coeff)], - [0] ]) - propellers_rpm = np.dot(self.matrix_u2rpm_inv, u) - - #### Command the turn rates of propellers 1 and 3 ########## - propellers_1_rpm = np.sqrt(propellers_rpm[1, 0]) - propellers_3_rpm = np.sqrt(propellers_rpm[2, 0]) - - #### For motion in the Y-Z plane, assign the same turn rates to prop. 0 and 2 - propellers_0_and_2_rpm = np.sqrt(propellers_rpm[0, 0]) - - #### Print relevant output ################################# - if self.control_counter%(1/self.timestep) == 0: - print("current_position", current_position) - print("current_velocity", current_velocity) - print("target_position", target_position) - print("target_velocity", target_velocity) - print("target_acceleration", target_acceleration) - - #### Store the last step's roll, pitch, and yaw ############ - self.last_rpy = current_rpy - - return np.array([propellers_0_and_2_rpm, propellers_1_rpm, - propellers_0_and_2_rpm, propellers_3_rpm]) - - ################################################################################ - - def pd_control(self, - desired_position, - current_position, - desired_velocity, - current_velocity, - desired_acceleration, - opt - ): - """Computes PD control for the acceleration minimizing position error. - - Parameters - ---------- - desired_position : - float: Desired global position. - current_position : - float: Current global position. - desired_velocity : - float: Desired global velocity. - current_velocity : - float: Current global velocity. - desired_acceleration : - float: Desired global acceleration. - - Returns - ------- - float - The commanded acceleration. - """ - u = desired_acceleration + \ - self.d_coeff_position[opt] * (desired_velocity - current_velocity) + \ - self.p_coeff_position[opt] * (desired_position - current_position) - - return u diff --git a/assignments/aer1216_fall2020_hw2_sim.py b/assignments/aer1216_fall2020_hw2_sim.py deleted file mode 100644 index e28ada76b..000000000 --- a/assignments/aer1216_fall2020_hw2_sim.py +++ /dev/null @@ -1,159 +0,0 @@ -"""Simulation script for assignment 2. - -The script uses the control defined in file `aer1216_fall2020_hw2_ctrl.py`. - -Example -------- -To run the simulation, type in a terminal: - - $ python aer1216_fall2020_hw2_sim.py - -""" -import time -import random -import numpy as np -import pybullet as p - -#### Uncomment the following 2 lines if "module gym_pybullet_drones cannot be found" -# import sys -# sys.path.append('../') - -from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary -from gym_pybullet_drones.utils.Logger import Logger -from gym_pybullet_drones.utils.utils import sync -from gym_pybullet_drones.utils.enums import DroneModel -from aer1216_fall2020_hw2_ctrl import HW2Control - -DURATION = 10 -"""int: The duration of the simulation in seconds.""" -GUI = True -"""bool: Whether to use PyBullet graphical interface.""" -RECORD = False -"""bool: Whether to save a video under /files/videos. Requires ffmpeg""" - -if __name__ == "__main__": - - #### Create the ENVironment ################################ - ENV = CtrlAviary(num_drones=3, - drone_model=DroneModel.CF2P, - initial_xyzs=np.array([ [.0, .0, .15], [-.3, .0, .15], [.3, .0, .15] ]), - gui=GUI, - record=RECORD - ) - PYB_CLIENT = ENV.getPyBulletClient() - - #### Initialize the LOGGER ################################# - LOGGER = Logger(logging_freq_hz=ENV.SIM_FREQ, - num_drones=3, - ) - - #### Initialize the CONTROLLERS ############################ - CTRL_0 = HW2Control(env=ENV, - control_type=0 - ) - CTRL_1 = HW2Control(env=ENV, - control_type=1 - ) - CTRL_2 = HW2Control(env=ENV, - control_type=2 - ) - - #### Initialize the ACTION ################################# - ACTION = {} - OBS = ENV.reset() - STATE = OBS["0"]["state"] - ACTION["0"] = CTRL_0.compute_control(current_position=STATE[0:3], - current_velocity=STATE[10:13], - current_rpy=STATE[7:10], - target_position=STATE[0:3], - target_velocity=np.zeros(3), - target_acceleration=np.zeros(3) - ) - STATE = OBS["1"]["state"] - ACTION["1"] = CTRL_1.compute_control(current_position=STATE[0:3], - current_velocity=STATE[10:13], - current_rpy=STATE[7:10], - target_position=STATE[0:3], - target_velocity=np.zeros(3), - target_acceleration=np.zeros(3) - ) - STATE = OBS["2"]["state"] - ACTION["2"] = CTRL_2.compute_control(current_position=STATE[0:3], - current_velocity=STATE[10:13], - current_rpy=STATE[7:10], - target_position=STATE[0:3], - target_velocity=np.zeros(3), - target_acceleration=np.zeros(3) - ) - - #### Initialize the target trajectory ###################### - TARGET_POSITION = np.array([[0, 4.0*np.cos(0.006*i), 1.0] for i in range(DURATION*ENV.SIM_FREQ)]) - TARGET_VELOCITY = np.zeros([DURATION * ENV.SIM_FREQ, 3]) - TARGET_ACCELERATION = np.zeros([DURATION * ENV.SIM_FREQ, 3]) - - #### Derive the target trajectory to obtain target velocities and accelerations - TARGET_VELOCITY[1:, :] = (TARGET_POSITION[1:, :] - TARGET_POSITION[0:-1, :]) / ENV.SIM_FREQ - TARGET_ACCELERATION[1:, :] = (TARGET_VELOCITY[1:, :] - TARGET_VELOCITY[0:-1, :]) / ENV.SIM_FREQ - - #### Run the simulation #################################### - START = time.time() - for i in range(0, DURATION*ENV.SIM_FREQ): - - ### Secret control performance booster ##################### - # if i/ENV.SIM_FREQ>3 and i%30==0 and i/ENV.SIM_FREQ<10: p.loadURDF("duck_vhacd.urdf", [random.gauss(0, 0.3), random.gauss(0, 0.3), 3], p.getQuaternionFromEuler([random.randint(0, 360),random.randint(0, 360),random.randint(0, 360)]), physicsClientId=PYB_CLIENT) - - #### Step the simulation ################################### - OBS, _, _, _ = ENV.step(ACTION) - - #### Compute control for drone 0 ########################### - STATE = OBS["0"]["state"] - ACTION["0"] = CTRL_0.compute_control(current_position=STATE[0:3], - current_velocity=STATE[10:13], - current_rpy=STATE[7:10], - target_position=TARGET_POSITION[i, :], - target_velocity=TARGET_VELOCITY[i, :], - target_acceleration=TARGET_ACCELERATION[i, :] - ) - #### Log drone 0 ########################################### - LOGGER.log(drone=0, timestamp=i/ENV.SIM_FREQ, state=STATE) - - #### Compute control for drone 1 ########################### - STATE = OBS["1"]["state"] - ACTION["1"] = CTRL_1.compute_control(current_position=STATE[0:3], - current_velocity=STATE[10:13], - current_rpy=STATE[7:10], - target_position=TARGET_POSITION[i, :] + np.array([-.3, .0, .0]), - target_velocity=TARGET_VELOCITY[i, :], - target_acceleration=TARGET_ACCELERATION[i, :] - ) - #### Log drone 1 ########################################### - LOGGER.log(drone=1, timestamp=i/ENV.SIM_FREQ, state=STATE) - - #### Compute control for drone 2 ########################### - STATE = OBS["2"]["state"] - ACTION["2"] = CTRL_2.compute_control(current_position=STATE[0:3], - current_velocity=STATE[10:13], - current_rpy=STATE[7:10], - target_position=TARGET_POSITION[i, :] + np.array([.3, .0, .0]), - target_velocity=TARGET_VELOCITY[i, :], - target_acceleration=TARGET_ACCELERATION[i, :] - ) - #### Log drone 2 ########################################### - LOGGER.log(drone=2, timestamp=i/ENV.SIM_FREQ, state=STATE) - - #### Printout ############################################## - if i%ENV.SIM_FREQ == 0: - 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/assignments/pybullet_example.py b/assignments/pybullet_example.py deleted file mode 100644 index 8a43c4853..000000000 --- a/assignments/pybullet_example.py +++ /dev/null @@ -1,20 +0,0 @@ -"""Minimal standalone PyBullet example. -""" -import pybullet as p -import pybullet_data -from time import sleep - -def main(): - PYB_CLIENT = p.connect(p.GUI) - p.setGravity(0, 0, -9.8, physicsClientId=PYB_CLIENT) - p.setRealTimeSimulation(0, physicsClientId=PYB_CLIENT) - p.setTimeStep(1/240, physicsClientId=PYB_CLIENT) - p.setAdditionalSearchPath(pybullet_data.getDataPath(), physicsClientId=PYB_CLIENT) - p.loadURDF("plane.urdf", physicsClientId=PYB_CLIENT) - p.loadURDF("duck_vhacd.urdf", [0, 0, 1], physicsClientId=PYB_CLIENT) - for _ in range(240*10): - p.stepSimulation(physicsClientId=PYB_CLIENT) - sleep(1/(240*10)) - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/experiments/performance/parallelism.sh b/experiments/performance/parallelism.sh deleted file mode 100755 index 42b96b1ae..000000000 --- a/experiments/performance/parallelism.sh +++ /dev/null @@ -1,19 +0,0 @@ -#!/bin/sh - -# USE -# 1. make the script executable: $ chmod +x ./parallelism.sh -# 2. run the script: $ ./parallelism.sh - -for i in {1..5}; do # for 5 rounds - echo -e "\nROUND $i\n" - for j in {1..4}; do # run script.py 4 times - python script.py --num_drones 20 --gui False --aggregate True --plot False --duration_sec 30 & - done - wait # wait for the 4 parallel simulations to complete -done 2>/dev/null # redirect STDERR to /dev/null - - - - - - diff --git a/experiments/performance/script.py b/experiments/performance/script.py deleted file mode 100644 index ade75fd3f..000000000 --- a/experiments/performance/script.py +++ /dev/null @@ -1,147 +0,0 @@ -"""Only used to spawn multiple simulations and evaluate performance. - -This script is similar to `examples/fly.py` and used by `parallelism.sh`. - -""" -import os -import time -import argparse -from datetime import datetime -import pdb -import math -import random -import numpy as np -import pybullet as p -import matplotlib.pyplot as plt - -from gym_pybullet_drones.utils.enums import DroneModel, Physics -from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary -from gym_pybullet_drones.envs.VisionAviary import VisionAviary -from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl -from gym_pybullet_drones.utils.Logger import Logger -from gym_pybullet_drones.utils.utils import sync, str2bool - -if __name__ == "__main__": - - #### Define and parse (optional) arguments for the script ## - parser = argparse.ArgumentParser(description='Helix flight script using CtrlAviary or VisionAviary and DSLPIDControl') - parser.add_argument('--drone', default="cf2x", type=DroneModel, help='Drone model (default: CF2X)', metavar='', choices=DroneModel) - parser.add_argument('--num_drones', default=3, type=int, help='Number of drones (default: 3)', metavar='') - parser.add_argument('--physics', default="pyb", type=Physics, help='Physics updates (default: PYB)', metavar='', choices=Physics) - parser.add_argument('--vision', default=False, type=str2bool, help='Whether to use VisionAviary (default: False)', metavar='') - parser.add_argument('--gui', default=True, type=str2bool, help='Whether to use PyBullet GUI (default: True)', metavar='') - parser.add_argument('--record_video', default=False, type=str2bool, help='Whether to record a video (default: False)', metavar='') - parser.add_argument('--plot', default=True, type=str2bool, help='Whether to plot the simulation results (default: True)', metavar='') - parser.add_argument('--user_debug_gui', default=False, type=str2bool, help='Whether to add debug lines and parameters to the GUI (default: False)', metavar='') - parser.add_argument('--aggregate', default=False, type=str2bool, help='Whether to aggregate physics steps (default: False)', metavar='') - parser.add_argument('--obstacles', default=True, type=str2bool, help='Whether to add obstacles to the environment (default: True)', metavar='') - parser.add_argument('--simulation_freq_hz', default=240, type=int, help='Simulation frequency in Hz (default: 240)', metavar='') - parser.add_argument('--control_freq_hz', default=48, type=int, help='Control frequency in Hz (default: 48)', metavar='') - parser.add_argument('--duration_sec', default=5, type=int, help='Duration of the simulation in seconds (default: 5)', metavar='') - ARGS = parser.parse_args() - - #### Initialize the simulation ############################# - H = .1 - H_STEP = .05 - R = .3 - INIT_XYZS = np.array([[R*np.cos((i/6)*2*np.pi+np.pi/2), R*np.sin((i/6)*2*np.pi+np.pi/2)-R, H+i*H_STEP] for i in range(ARGS.num_drones)]) - AGGR_PHY_STEPS = int(ARGS.simulation_freq_hz/ARGS.control_freq_hz) if ARGS.aggregate else 1 - - #### Create the environment with or without video capture ## - if ARGS.vision: - env = VisionAviary(drone_model=ARGS.drone, - num_drones=ARGS.num_drones, - initial_xyzs=INIT_XYZS, - physics=ARGS.physics, - neighbourhood_radius=10, - freq=ARGS.simulation_freq_hz, - aggregate_phy_steps=AGGR_PHY_STEPS, - gui=ARGS.gui, - record=ARGS.record_video, - obstacles=ARGS.obstacles - ) - else: - env = CtrlAviary(drone_model=ARGS.drone, - num_drones=ARGS.num_drones, - initial_xyzs=INIT_XYZS, - physics=ARGS.physics, - neighbourhood_radius=10, - freq=ARGS.simulation_freq_hz, - aggregate_phy_steps=AGGR_PHY_STEPS, - gui=ARGS.gui, - record=ARGS.record_video, - obstacles=ARGS.obstacles, - user_debug_gui=ARGS.user_debug_gui - ) - - #### Initialize a circular trajectory ###################### - PERIOD = 10 - NUM_WP = ARGS.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 = np.array([int((i*NUM_WP/6)%NUM_WP) for i in range(ARGS.num_drones)]) - - #### Initialize the logger ################################# - logger = Logger(logging_freq_hz=int(ARGS.simulation_freq_hz/AGGR_PHY_STEPS), - num_drones=ARGS.num_drones - ) - - #### Initialize the controllers ############################ - ctrl = [DSLPIDControl(drone_model=ARGS.drone) for i in range(ARGS.num_drones)] - - #### Run the simulation #################################### - CTRL_EVERY_N_STEPS= int(np.floor(env.SIM_FREQ/ARGS.control_freq_hz)) - action = {str(i): np.array([0, 0, 0, 0]) for i in range(ARGS.num_drones)} - START = time.time() - for i in range(0, int(ARGS.duration_sec*env.SIM_FREQ), AGGR_PHY_STEPS): - - #### Step the simulation ################################### - obs, reward, done, info = env.step(action) - - #### Compute control at the desired frequency @@@@@######### - if i%CTRL_EVERY_N_STEPS == 0: - - #### Compute control for the current way point ############# - for j in range(ARGS.num_drones): - action[str(j)], _, _ = ctrl[j].computeControlFromState(control_timestep=CTRL_EVERY_N_STEPS*env.TIMESTEP, - state=obs[str(j)]["state"], - target_pos=np.hstack([TARGET_POS[wp_counters[j], 0:2], H+j*H_STEP]) - ) - - #### Go to the next way point and loop ##################### - for j in range(ARGS.num_drones): - wp_counters[j] = wp_counters[j] + 1 if wp_counters[j] < (NUM_WP-1) else 0 - - #### Log the simulation #################################### - for j in range(ARGS.num_drones): - logger.log(drone=j, - timestamp=i/env.SIM_FREQ, - state= obs[str(j)]["state"], - control=np.hstack([TARGET_POS[wp_counters[j], 0:2], H+j*H_STEP, np.zeros(9)]) - ) - - #### Printout ############################################## - if i%env.SIM_FREQ == 0: - env.render() - #### Print matrices with the images captured by each drone # - if ARGS.vision: - for j in range(ARGS.num_drones): - print(obs[str(j)]["rgb"].shape, np.average(obs[str(j)]["rgb"]), - obs[str(j)]["dep"].shape, np.average(obs[str(j)]["dep"]), - obs[str(j)]["seg"].shape, np.average(obs[str(j)]["seg"]) - ) - - #### Sync the simulation ################################### - if ARGS.gui: - sync(i, START, env.TIMESTEP) - - #### Close the environment ################################# - env.close() - - #### Save the simulation results ########################### - logger.save() - - #### Plot the simulation results ########################### - if ARGS.plot: - logger.plot() diff --git a/ros2/.gitignore b/ros2/.gitignore deleted file mode 100644 index e1e89cce9..000000000 --- a/ros2/.gitignore +++ /dev/null @@ -1,56 +0,0 @@ -# More ROS 2 ignore -install/ -log/ - -# ROS -devel/ -logs/ -build/ -bin/ -lib/ -msg_gen/ -srv_gen/ -msg/*Action.msg -msg/*ActionFeedback.msg -msg/*ActionGoal.msg -msg/*ActionResult.msg -msg/*Feedback.msg -msg/*Goal.msg -msg/*Result.msg -msg/_*.py -build_isolated/ -devel_isolated/ - -# Generated by dynamic reconfigure -*.cfgc -/cfg/cpp/ -/cfg/*.py - -# Ignore generated docs -*.dox -*.wikidoc - -# eclipse stuff -.project -.cproject - -# qcreator stuff -CMakeLists.txt.user - -srv/_*.py -*.pcd -*.pyc -qtcreator-* -*.user - -/planning/cfg -/planning/docs -/planning/src - -*~ - -# Emacs -.#* - -# Catkin custom files -CATKIN_IGNORE \ No newline at end of file diff --git a/ros2/ros2_and_pkg_setups.bash b/ros2/ros2_and_pkg_setups.bash deleted file mode 100644 index 46283b7d5..000000000 --- a/ros2/ros2_and_pkg_setups.bash +++ /dev/null @@ -1,2 +0,0 @@ -source ~/ros2_foxy/ros2-osx/setup.bash # change to the location of your ROS2 installation, if necessary -source ./install/setup.bash # the relative path requires to source from folder /gym_pybullet_drones/ros2/ \ No newline at end of file diff --git a/ros2/ros2_and_pkg_setups.zsh b/ros2/ros2_and_pkg_setups.zsh deleted file mode 100755 index 88e506772..000000000 --- a/ros2/ros2_and_pkg_setups.zsh +++ /dev/null @@ -1,2 +0,0 @@ -source ~/ros2_foxy/ros2-osx/setup.zsh # change to the location of your ROS2 installation, if necessary -source ./install/setup.zsh # the relative path requires to source from folder /gym_pybullet_drones/ros2/ \ No newline at end of file diff --git a/ros2/src/ros2_gym_pybullet_drones/package.xml b/ros2/src/ros2_gym_pybullet_drones/package.xml deleted file mode 100644 index e07809872..000000000 --- a/ros2/src/ros2_gym_pybullet_drones/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - ros2_gym_pybullet_drones - 0.0.1 - A ROS2 Python wrapper for gym-pybullet-drones - Jacopo Panerati - MIT License - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - rclpy - std_msgs - - diff --git a/ros2/src/ros2_gym_pybullet_drones/resource/ros2_gym_pybullet_drones b/ros2/src/ros2_gym_pybullet_drones/resource/ros2_gym_pybullet_drones deleted file mode 100644 index e69de29bb..000000000 diff --git a/ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/__init__.py b/ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/aviary_wrapper.py b/ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/aviary_wrapper.py deleted file mode 100644 index a2aa6eb26..000000000 --- a/ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/aviary_wrapper.py +++ /dev/null @@ -1,85 +0,0 @@ -"""ROS2 Python wrapper node for class CtrlAviary. - -It creates an environment CtrlAviary and continually calls CtrlAviary.step(). -It publishes on topic 'obs' and reads from topic 'action'. -""" -import sys, os # See: https://github.com/utiasDSL/gym-pybullet-drones/issues/89 -import getpass -sys.path.append(sys.path[0].replace("ros2/install/ros2_gym_pybullet_drones/lib/ros2_gym_pybullet_drones", "")) -if sys.platform == 'darwin': # macOS - sys.path.append("/Users/"+os.getlogin()+"/opt/anaconda3/envs/drones/lib/python3.8/site-packages") -elif sys.platform == 'linux': # Ubuntu - sys.path.append("/home/"+getpass.getuser()+"/anaconda3/envs/drones/lib/python3.8/site-packages") - -import rclpy -import numpy as np -from rclpy.node import Node -from std_msgs.msg import Float32MultiArray - -from gym_pybullet_drones.utils.enums import DroneModel, Physics -from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary - -class AviaryWrapper(Node): - - #### Initialize the node ################################### - def __init__(self): - super().__init__('aviary_wrapper') - self.step_cb_count = 0 - self.get_action_cb_count = 0 - timer_freq_hz = 240 - timer_period_sec = 1/timer_freq_hz - #### Create the CtrlAviary environment wrapped by the node # - self.env = CtrlAviary(drone_model=DroneModel.CF2X, - num_drones=1, - neighbourhood_radius=np.inf, - initial_xyzs=None, - initial_rpys=None, - physics=Physics.PYB, - freq=timer_freq_hz, - aggregate_phy_steps=1, - gui=True, - record=False, - obstacles=False, - user_debug_gui=True - ) - #### Initialize an action with the RPMs at hover ########### - self.action = np.ones(4)*self.env.HOVER_RPM - #### Declare publishing on 'obs' and create a timer to call - #### action_callback every timer_period_sec ################ - self.publisher_ = self.create_publisher(Float32MultiArray, 'obs', 1) - self.timer = self.create_timer(timer_period_sec, self.step_callback) - #### Subscribe to topic 'action' ########################### - self.action_subscription = self.create_subscription(Float32MultiArray, 'action', self.get_action_callback, 1) - self.action_subscription # prevent unused variable warning - - #### Step the env and publish drone0's state on topic 'obs' - def step_callback(self): - self.step_cb_count += 1 - obs, reward, done, info = self.env.step({"0": self.action}) - msg = Float32MultiArray() - msg.data = obs["0"]["state"].tolist() - self.publisher_.publish(msg) - if self.step_cb_count%10 == 0: - self.get_logger().info('Publishing obs: "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f"' \ - % (msg.data[0], msg.data[1], msg.data[2], msg.data[3], msg.data[4], - msg.data[5], msg.data[6], msg.data[7], msg.data[8], msg.data[9], - msg.data[10], msg.data[11], msg.data[12], msg.data[13], msg.data[14], - msg.data[15], msg.data[16], msg.data[17], msg.data[18], msg.data[19] - ) - ) - - #### Read the action to apply to the env from topic 'action' - def get_action_callback(self, msg): - self.get_action_cb_count += 1 - self.action = np.array([msg.data[0], msg.data[1], msg.data[2], msg.data[3]]) - if self.get_action_cb_count%10 == 0: - self.get_logger().info('I received action: "%f" "%f" "%f" "%f"' % (msg.data[0], msg.data[1], msg.data[2], msg.data[3])) - -############################################################ -def main(args=None): - rclpy.init(args=args) - aviary_wrapper = AviaryWrapper() - rclpy.spin(aviary_wrapper) - -if __name__ == '__main__': - main() diff --git a/ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/random_control.py b/ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/random_control.py deleted file mode 100644 index 98d9f5775..000000000 --- a/ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/random_control.py +++ /dev/null @@ -1,72 +0,0 @@ -"""ROS2 Python node for random control. - -It subscribes to aviary_wrapper's 'obs' topic (but ignores it). -It publishes random RPMs on topic 'action'. -""" -import sys, os # See: https://github.com/utiasDSL/gym-pybullet-drones/issues/89 -import getpass -sys.path.append(sys.path[0].replace("ros2/install/ros2_gym_pybullet_drones/lib/ros2_gym_pybullet_drones", "")) -if sys.platform == 'darwin': # macOS - sys.path.append("/Users/"+os.getlogin()+"/opt/anaconda3/envs/drones/lib/python3.8/site-packages") -elif sys.platform == 'linux': # Ubuntu - sys.path.append("/home/"+getpass.getuser()+"/anaconda3/envs/drones/lib/python3.8/site-packages") - -import rclpy -import random -import numpy as np -from rclpy.node import Node -from std_msgs.msg import Float32MultiArray - -from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary - -class RandomControl(Node): - - #### Initialize the node ################################### - def __init__(self): - super().__init__('random_control') - self.action_cb_count = 0 - self.get_obs_cb_count = 0 - #### Set the frequency used to publish actions ############# - timer_freq_hz = 50 - timer_period_sec = 1/timer_freq_hz - #### Dummy CtrlAviary to obtain the HOVER_RPM constant ##### - self.env = CtrlAviary() - #### Declare publishing on 'action' and create a timer to ## - #### call action_callback every timer_period_sec ########### - self.publisher_ = self.create_publisher(Float32MultiArray, 'action', 1) - self.timer = self.create_timer(timer_period_sec, self.action_callback) - #### Subscribe to topic 'obs' ############################## - self.obs_subscription = self.create_subscription(Float32MultiArray, 'obs', self.get_obs_callback, 1) - self.obs_subscription # prevent unused variable warning - - #### Publish random RPMs on topic 'action' ################# - def action_callback(self): - self.action_cb_count += 1 - random_rpm13 = random.uniform(.9, 1.1)*self.env.HOVER_RPM - random_rpm24 = random.uniform(.9, 1.1)*self.env.HOVER_RPM - msg = Float32MultiArray() - msg.data = [random_rpm13,random_rpm24,random_rpm13,random_rpm24] - self.publisher_.publish(msg) - if self.action_cb_count%10 == 0: - self.get_logger().info('Publishing action: "%f" "%f" "%f" "%f"' % (msg.data[0], msg.data[1], msg.data[2], msg.data[3])) - - #### Read the state of drone 0 from topic 'obs' ############ - def get_obs_callback(self, msg): - self.get_obs_cb_count += 1 - if self.get_obs_cb_count%10 == 0: - self.get_logger().info('I received obs: "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f"' \ - % (msg.data[0], msg.data[1], msg.data[2], msg.data[3], msg.data[4], - msg.data[5], msg.data[6], msg.data[7], msg.data[8], msg.data[9], - msg.data[10], msg.data[11], msg.data[12], msg.data[13], msg.data[14], - msg.data[15], msg.data[16], msg.data[17], msg.data[18], msg.data[19] - ) - ) - -############################################################ -def main(args=None): - rclpy.init(args=args) - random_control = RandomControl() - rclpy.spin(random_control) - -if __name__ == '__main__': - main() diff --git a/ros2/src/ros2_gym_pybullet_drones/setup.cfg b/ros2/src/ros2_gym_pybullet_drones/setup.cfg deleted file mode 100644 index da95b1b5d..000000000 --- a/ros2/src/ros2_gym_pybullet_drones/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/ros2_gym_pybullet_drones -[install] -install_scripts=$base/lib/ros2_gym_pybullet_drones diff --git a/ros2/src/ros2_gym_pybullet_drones/setup.py b/ros2/src/ros2_gym_pybullet_drones/setup.py deleted file mode 100644 index af76c6f8e..000000000 --- a/ros2/src/ros2_gym_pybullet_drones/setup.py +++ /dev/null @@ -1,27 +0,0 @@ -from setuptools import setup - -package_name = 'ros2_gym_pybullet_drones' - -setup( - name=package_name, - version='0.0.1', - packages=[package_name], - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='Jacopo Panerati', - maintainer_email='jacopo.panerati@utoronto.ca', - description='A ROS2 Python wrapper for gym-pybullet-drones', - license='MIT License', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'aviary_wrapper = ros2_gym_pybullet_drones.aviary_wrapper:main', - 'random_control = ros2_gym_pybullet_drones.random_control:main', - ], - }, -) diff --git a/ros2/src/ros2_gym_pybullet_drones/test/test_copyright.py b/ros2/src/ros2_gym_pybullet_drones/test/test_copyright.py deleted file mode 100644 index cc8ff03f7..000000000 --- a/ros2/src/ros2_gym_pybullet_drones/test/test_copyright.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/ros2/src/ros2_gym_pybullet_drones/test/test_flake8.py b/ros2/src/ros2_gym_pybullet_drones/test/test_flake8.py deleted file mode 100644 index 27ee1078f..000000000 --- a/ros2/src/ros2_gym_pybullet_drones/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/ros2/src/ros2_gym_pybullet_drones/test/test_pep257.py b/ros2/src/ros2_gym_pybullet_drones/test/test_pep257.py deleted file mode 100644 index b234a3840..000000000 --- a/ros2/src/ros2_gym_pybullet_drones/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings'