From 73a47d543163d8516a3eadf267801470cb35627d Mon Sep 17 00:00:00 2001 From: Jacopo Date: Sat, 2 Mar 2024 14:39:50 +0400 Subject: [PATCH] Initial commit --- README.md | 29 ++ gym_pybullet_drones/examples/cff-bit.py | 264 ++++++++++++++++++ .../examples/{cf.py => cff-dsl.py} | 0 3 files changed, 293 insertions(+) create mode 100644 gym_pybullet_drones/examples/cff-bit.py rename gym_pybullet_drones/examples/{cf.py => cff-dsl.py} (100%) diff --git a/README.md b/README.md index 81033c692..bc0619dad 100644 --- a/README.md +++ b/README.md @@ -49,6 +49,35 @@ python learn.py --multiagent true # task: 2-drone hover at z == 1.2 and 0.7 rl example marl example +### 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 diff --git a/gym_pybullet_drones/examples/cff-bit.py b/gym_pybullet_drones/examples/cff-bit.py new file mode 100644 index 000000000..1ee48aaa3 --- /dev/null +++ b/gym_pybullet_drones/examples/cff-bit.py @@ -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)) diff --git a/gym_pybullet_drones/examples/cf.py b/gym_pybullet_drones/examples/cff-dsl.py similarity index 100% rename from gym_pybullet_drones/examples/cf.py rename to gym_pybullet_drones/examples/cff-dsl.py