Skip to content

Commit

Permalink
Initial commit
Browse files Browse the repository at this point in the history
  • Loading branch information
@JacopoPan
JacopoPan committed Mar 2, 2024
1 parent 6a79071 commit 73a47d5
Show file tree
Hide file tree
Showing 3 changed files with 293 additions and 0 deletions.
29 changes: 29 additions & 0 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,35 @@ python learn.py --multiagent true # task: 2-drone hover at z == 1.2 and 0.7

<img src="gym_pybullet_drones/assets/rl.gif" alt="rl example" width="375"> <img src="gym_pybullet_drones/assets/marl.gif" alt="marl example" width="375">

### utiasDSL `pycffirmware` Python Bindings example (multiplatform, single-drone)

Install [`pycffirmware`](https://github.com/utiasDSL/pycffirmware?tab=readme-ov-file#installation) for Ubuntu, macOS, or Windows

```sh
cd gym_pybullet_drones/examples/
python3 cff-dsl.py
```

### Bitcraze Python bindings example (Ubuntu only, multi-drone)

```sh
sudo apt update
sudo apt -y install swig
sudo apt-get install make gcc-arm-none-eabi
git clone --recursive https://github.com/bitcraze/crazyflie-firmware.git
cd crazyflie-firmware/
make cf2_defconfig
make -j 12
make bindings_python
cd build/
python3 setup.py install --user
```

```sh
cd gym_pybullet_drones/examples/
python3 cff-bit.py
```

### Betaflight SITL example (Ubuntu only)

```sh
Expand Down
264 changes: 264 additions & 0 deletions gym_pybullet_drones/examples/cff-bit.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,264 @@
"""Script demonstrating the use of crazyflie-firmware's Python bindings.
Example
-------
In a terminal, run as:
$ python cff.py
"""
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

import cffirmware

from gym_pybullet_drones.utils.enums import DroneModel, Physics
from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary
from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl
from gym_pybullet_drones.utils.Logger import Logger
from gym_pybullet_drones.utils.utils import sync, str2bool

DEFAULT_DRONES = DroneModel("cf2x")
DEFAULT_NUM_DRONES = 1
DEFAULT_PHYSICS = Physics("pyb")
DEFAULT_GUI = True
DEFAULT_RECORD_VISION = False
DEFAULT_PLOT = True
DEFAULT_USER_DEBUG_GUI = False
DEFAULT_OBSTACLES = True
DEFAULT_SIMULATION_FREQ_HZ = 240
DEFAULT_CONTROL_FREQ_HZ = 48
DEFAULT_DURATION_SEC = 12
DEFAULT_OUTPUT_FOLDER = 'results'
DEFAULT_COLAB = False

def run(
drone=DEFAULT_DRONES,
num_drones=DEFAULT_NUM_DRONES,
physics=DEFAULT_PHYSICS,
gui=DEFAULT_GUI,
record_video=DEFAULT_RECORD_VISION,
plot=DEFAULT_PLOT,
user_debug_gui=DEFAULT_USER_DEBUG_GUI,
obstacles=DEFAULT_OBSTACLES,
simulation_freq_hz=DEFAULT_SIMULATION_FREQ_HZ,
control_freq_hz=DEFAULT_CONTROL_FREQ_HZ,
duration_sec=DEFAULT_DURATION_SEC,
output_folder=DEFAULT_OUTPUT_FOLDER,
colab=DEFAULT_COLAB
):
#### Initialize the simulation #############################
H = .1
H_STEP = .05
R = .3
#INIT_XYZS = np.array([[R*np.cos((i/6)*2*np.pi+np.pi/2), R*np.sin((i/6)*2*np.pi+np.pi/2)-R, H+i*H_STEP] for i in range(num_drones)])
INIT_XYZS = np.array([[.5*i, .5*i, .1] for i in range(num_drones)])
INIT_RPYS = np.array([[0, 0, i * (np.pi/2)/num_drones] for i in range(num_drones)])

#### 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], 0
wp_counters = np.array([int((i*NUM_WP/6)%NUM_WP) for i in range(num_drones)])

delta = 75 # 3s @ 25hz control loop
trajectory = [[0, 0, 0] for i in range(delta)] + \
[[0, 0, i/delta] for i in range(delta)] + \
[[i/delta, 0, 1] for i in range(delta)] + \
[[1, i/delta, 1] for i in range(delta)] + \
[[1-i/delta, 1, 1] for i in range(delta)] + \
[[0, 1-i/delta, 1] for i in range(delta)] + \
[[0, 0, 1-i/delta] for i in range(delta)]


#### Debug trajectory ######################################
#### Uncomment alt. target_pos in .computeControlFromState()
# INIT_XYZS = np.array([[.3 * i, 0, .1] for i in range(num_drones)])
# INIT_RPYS = np.array([[0, 0, i * (np.pi/3)/num_drones] for i in range(num_drones)])
# NUM_WP = control_freq_hz*15
# TARGET_POS = np.zeros((NUM_WP,3))
# for i in range(NUM_WP):
# if i < NUM_WP/6:
# TARGET_POS[i, :] = (i*6)/NUM_WP, 0, 0.5*(i*6)/NUM_WP
# elif i < 2 * NUM_WP/6:
# TARGET_POS[i, :] = 1 - ((i-NUM_WP/6)*6)/NUM_WP, 0, 0.5 - 0.5*((i-NUM_WP/6)*6)/NUM_WP
# elif i < 3 * NUM_WP/6:
# TARGET_POS[i, :] = 0, ((i-2*NUM_WP/6)*6)/NUM_WP, 0.5*((i-2*NUM_WP/6)*6)/NUM_WP
# elif i < 4 * NUM_WP/6:
# TARGET_POS[i, :] = 0, 1 - ((i-3*NUM_WP/6)*6)/NUM_WP, 0.5 - 0.5*((i-3*NUM_WP/6)*6)/NUM_WP
# elif i < 5 * NUM_WP/6:
# TARGET_POS[i, :] = ((i-4*NUM_WP/6)*6)/NUM_WP, ((i-4*NUM_WP/6)*6)/NUM_WP, 0.5*((i-4*NUM_WP/6)*6)/NUM_WP
# elif i < 6 * NUM_WP/6:
# TARGET_POS[i, :] = 1 - ((i-5*NUM_WP/6)*6)/NUM_WP, 1 - ((i-5*NUM_WP/6)*6)/NUM_WP, 0.5 - 0.5*((i-5*NUM_WP/6)*6)/NUM_WP
# wp_counters = np.array([0 for i in range(num_drones)])

#### Create the environment ################################
env = CtrlAviary(drone_model=drone,
num_drones=num_drones,
initial_xyzs=INIT_XYZS,
initial_rpys=INIT_RPYS,
physics=physics,
neighbourhood_radius=10,
pyb_freq=simulation_freq_hz,
ctrl_freq=control_freq_hz,
gui=gui,
record=record_video,
obstacles=obstacles,
user_debug_gui=user_debug_gui
)

#### Obtain the PyBullet Client ID from the environment ####
PYB_CLIENT = env.getPyBulletClient()

#### Initialize the logger #################################
logger = Logger(logging_freq_hz=control_freq_hz,
num_drones=num_drones,
output_folder=output_folder,
colab=colab
)

#### Initialize the controllers ############################
if drone in [DroneModel.CF2X, DroneModel.CF2P]:
ctrl = [DSLPIDControl(drone_model=drone) for i in range(num_drones)]

cff_controller = cffirmware.controllerMellinger_t()
cffirmware.controllerMellingerInit(cff_controller)

#### Run the simulation ####################################
action = np.zeros((num_drones,4))
START = time.time()
for i in range(0, int(duration_sec*env.CTRL_FREQ)):

#### Make it rain rubber ducks #############################
# if i/env.SIM_FREQ>5 and i%10==0 and i/env.SIM_FREQ<10: p.loadURDF("duck_vhacd.urdf", [0+random.gauss(0, 0.3),-0.5+random.gauss(0, 0.3),3], p.getQuaternionFromEuler([random.randint(0,360),random.randint(0,360),random.randint(0,360)]), physicsClientId=PYB_CLIENT)

#### Step the simulation ###################################
obs, reward, terminated, truncated, info = env.step(action)

#### Compute control for the current way point #############
for j in range(num_drones):

try:
target = trajectory[i]
pos = target+[INIT_XYZS[j][0], INIT_XYZS[j][1], 0]
vel = np.zeros(3)
acc = np.zeros(3)
yaw = i*np.pi/delta/2
rpy_rate = np.zeros(3)
print(pos)
except:
break

action[j, :], _, _ = ctrl[j].computeControlFromState(control_timestep=env.CTRL_TIMESTEP,
state=obs[j],
target_pos=[pos[0], pos[1], pos[2]],
# target_pos=np.hstack([TARGET_POS[wp_counters[j], 0:2], INIT_XYZS[j, 2]]),
# target_pos=INIT_XYZS[j, :] + TARGET_POS[wp_counters[j], :],
target_rpy=INIT_RPYS[j, :]
)

state = cffirmware.state_t()
state.attitude.roll = 0
state.attitude.pitch = -0 # WARNING: This needs to be negated
state.attitude.yaw = 0
state.position.x = obs[j][0]
state.position.y = obs[j][1]
state.position.z = obs[j][2]
state.velocity.x = obs[j][10]
state.velocity.y = obs[j][11]
state.velocity.z = obs[j][12]

sensors = cffirmware.sensorData_t()
sensors.gyro.x = obs[j][7]
sensors.gyro.y = obs[j][8]
sensors.gyro.z = obs[j][9]

setpoint = cffirmware.setpoint_t()
setpoint.mode.z = cffirmware.modeAbs
setpoint.position.z = INIT_XYZS[j, 2]
setpoint.mode.x = cffirmware.modeAbs
setpoint.velocity.x = TARGET_POS[wp_counters[j], 0]
setpoint.mode.y = cffirmware.modeAbs
setpoint.velocity.y = TARGET_POS[wp_counters[j], 1]
setpoint.mode.yaw = cffirmware.modeVelocity
setpoint.attitudeRate.yaw = 0

control = cffirmware.control_t()
step = i
cffirmware.controllerMellinger(cff_controller, control, setpoint, sensors, state, step)
# assert control.controlMode == cffirmware.controlModeLegacy
# print(control.thrust, control.roll, control.pitch, control.yaw)

actual = cffirmware.motors_thrust_uncapped_t()
cffirmware.powerDistribution(control, actual)
# print(actual.motors.m1, actual.motors.m2, actual.motors.m3, actual.motors.m4)
PWM2RPM_SCALE = 0.2685
PWM2RPM_CONST = 4070.3
MIN_PWM = 20000
MAX_PWM = 65535
new_action = PWM2RPM_SCALE * np.clip(np.array([actual.motors.m1, actual.motors.m2, actual.motors.m3, actual.motors.m4]), MIN_PWM, MAX_PWM) + PWM2RPM_CONST
print(i, new_action)

# action[j, :] = [actual.motors.m1, actual.motors.m2, actual.motors.m3, actual.motors.m4]
# action[j, :] = new_action

#### Go to the next way point 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.CTRL_FREQ,
state=obs[j],
control=np.hstack([TARGET_POS[wp_counters[j], 0:2], INIT_XYZS[j, 2], INIT_RPYS[j, :], np.zeros(6)])
# control=np.hstack([INIT_XYZS[j, :]+TARGET_POS[wp_counters[j], :], INIT_RPYS[j, :], np.zeros(6)])
)

#### Printout ##############################################
# env.render()

#### Sync the simulation ###################################
if gui:
sync(i, START, env.CTRL_TIMESTEP)

#### Close the environment #################################
env.close()

#### Save the simulation results ###########################
logger.save()
logger.save_as_csv("pid") # Optional CSV save

#### Plot the simulation results ###########################
if plot:
logger.plot()

if __name__ == "__main__":
#### Define and parse (optional) arguments for the script ##
parser = argparse.ArgumentParser(description='Helix flight script using CtrlAviary and DSLPIDControl')
parser.add_argument('--drone', default=DEFAULT_DRONES, type=DroneModel, help='Drone model (default: CF2X)', metavar='', choices=DroneModel)
parser.add_argument('--num_drones', default=DEFAULT_NUM_DRONES, type=int, help='Number of drones (default: 3)', metavar='')
parser.add_argument('--physics', default=DEFAULT_PHYSICS, type=Physics, help='Physics updates (default: PYB)', metavar='', choices=Physics)
parser.add_argument('--gui', default=DEFAULT_GUI, type=str2bool, help='Whether to use PyBullet GUI (default: True)', metavar='')
parser.add_argument('--record_video', default=DEFAULT_RECORD_VISION, type=str2bool, help='Whether to record a video (default: False)', metavar='')
parser.add_argument('--plot', default=DEFAULT_PLOT, type=str2bool, help='Whether to plot the simulation results (default: True)', metavar='')
parser.add_argument('--user_debug_gui', default=DEFAULT_USER_DEBUG_GUI, type=str2bool, help='Whether to add debug lines and parameters to the GUI (default: False)', metavar='')
parser.add_argument('--obstacles', default=DEFAULT_OBSTACLES, type=str2bool, help='Whether to add obstacles to the environment (default: True)', metavar='')
parser.add_argument('--simulation_freq_hz', default=DEFAULT_SIMULATION_FREQ_HZ, type=int, help='Simulation frequency in Hz (default: 240)', metavar='')
parser.add_argument('--control_freq_hz', default=DEFAULT_CONTROL_FREQ_HZ, type=int, help='Control frequency in Hz (default: 48)', metavar='')
parser.add_argument('--duration_sec', default=DEFAULT_DURATION_SEC, type=int, help='Duration of the simulation in seconds (default: 5)', metavar='')
parser.add_argument('--output_folder', default=DEFAULT_OUTPUT_FOLDER, type=str, help='Folder where to save logs (default: "results")', metavar='')
parser.add_argument('--colab', default=DEFAULT_COLAB, type=bool, help='Whether example is being run by a notebook (default: "False")', metavar='')
ARGS = parser.parse_args()

run(**vars(ARGS))
0 gym_pybullet_drones/examples/cf.py → gym_pybullet_drones/examples/cff-dsl.py
View file
File renamed without changes.

0 comments on commit 73a47d5

Please sign in to comment.