From 01f5c27c9d564e82d4c0f23b33ff96e161a61145 Mon Sep 17 00:00:00 2001 From: Spencer Teetaert Date: Fri, 25 Aug 2023 19:34:05 -0400 Subject: [PATCH 01/15] Restructured BetaFlight controller into BetaAviary and CTBRControl. Provided beta2 example script. known issue: slight variations exist between flight results of beta.py and beta2.py --- gym_pybullet_drones/control/CTBRControl.py | 250 +++++++++++++++++ gym_pybullet_drones/envs/BetaAviary.py | 309 +++++++++++++++++++++ gym_pybullet_drones/examples/beta2.py | 176 ++++++++++++ 3 files changed, 735 insertions(+) create mode 100644 gym_pybullet_drones/control/CTBRControl.py create mode 100644 gym_pybullet_drones/envs/BetaAviary.py create mode 100644 gym_pybullet_drones/examples/beta2.py diff --git a/gym_pybullet_drones/control/CTBRControl.py b/gym_pybullet_drones/control/CTBRControl.py new file mode 100644 index 000000000..d1b4fb7af --- /dev/null +++ b/gym_pybullet_drones/control/CTBRControl.py @@ -0,0 +1,250 @@ +import os +import numpy as np +import xml.etree.ElementTree as etxml +import pkg_resources +import socket +import struct + +from transforms3d.quaternions import rotate_vector, qconjugate, mat2quat, qmult +from transforms3d.utils import normalized_vector + +from gym_pybullet_drones.utils.enums import DroneModel + +class CTBRControl(object): + """Base class for control. + + Implements `__init__()`, `reset(), and interface `computeControlFromState()`, + the main method `computeControl()` should be implemented by its subclasses. + + """ + + ################################################################################ + + def __init__(self, + drone_model: DroneModel, + g: float=9.8 + ): + """Common control classes __init__ method. + + Parameters + ---------- + drone_model : DroneModel + The type of drone to control (detailed in an .urdf file in folder `assets`). + g : float, optional + The gravitational acceleration in m/s^2. + + """ + #### Set general use constants ############################# + self.DRONE_MODEL = drone_model + """DroneModel: The type of drone to control.""" + self.GRAVITY = g*self._getURDFParameter('m') + """float: The gravitational force (M*g) acting on each drone.""" + self.KF = self._getURDFParameter('kf') + """float: The coefficient converting RPMs into thrust.""" + self.KM = self._getURDFParameter('km') + """float: The coefficient converting RPMs into torque.""" + + self.reset() + + ################################################################################ + + def reset(self): + """Reset the control classes. + + A general use counter is set to zero. + + """ + self.control_counter = 0 + + ################################################################################ + + def computeControlFromState(self, + control_timestep, + state, + target_pos, + target_rpy=np.zeros(3), + target_vel=np.zeros(3), + target_rpy_rates=np.zeros(3) + ): + """Interface method using `computeControl`. + + It can be used to compute a control action directly from the value of key "state" + in the `obs` returned by a call to BaseAviary.step(). + + Parameters + ---------- + control_timestep : float + The time step at which control is computed. + state : ndarray + (20,)-shaped array of floats containing the current state of the drone. + target_pos : ndarray + (3,1)-shaped array of floats containing the desired position. + target_rpy : ndarray, optional + (3,1)-shaped array of floats containing the desired orientation as roll, pitch, yaw. + target_vel : ndarray, optional + (3,1)-shaped array of floats containing the desired velocity. + target_rpy_rates : ndarray, optional + (3,1)-shaped array of floats containing the desired roll, pitch, and yaw rates. + """ + + return self.computeControl(control_timestep=control_timestep, + cur_pos=state[0:3], + cur_quat=np.array([state[6], state[3], state[4], state[5]]), + cur_vel=state[10:13], + cur_ang_vel=state[13:16], + target_pos=target_pos, + target_rpy=target_rpy, + target_vel=target_vel, + target_rpy_rates=target_rpy_rates + ) + + ################################################################################ + + def computeControl(self, + control_timestep, + cur_pos, + cur_quat, + cur_vel, + cur_ang_vel, + target_pos, + target_rpy=np.zeros(3), + target_vel=np.zeros(3), + target_rpy_rates=np.zeros(3) + ): + """Abstract method to compute the control action for a single drone. + + It must be implemented by each subclass of `BaseControl`. + + Parameters + ---------- + control_timestep : float + The time step at which control is computed. + cur_pos : ndarray + (3,1)-shaped array of floats containing the current position. + cur_quat : ndarray + (4,1)-shaped array of floats containing the current orientation as a quaternion. + cur_vel : ndarray + (3,1)-shaped array of floats containing the current velocity. + cur_ang_vel : ndarray + (3,1)-shaped array of floats containing the current angular velocity. + target_pos : ndarray + (3,1)-shaped array of floats containing the desired position. + target_rpy : ndarray, optional + (3,1)-shaped array of floats containing the desired orientation as roll, pitch, yaw. + target_vel : ndarray, optional + (3,1)-shaped array of floats containing the desired velocity. + target_rpy_rates : ndarray, optional + (3,1)-shaped array of floats containing the desired roll, pitch, and yaw rates. + + """ + assert(cur_pos.shape == (3,)), f"cur_pos {cur_pos.shape}" + assert(cur_quat.shape == (4,)), f"cur_quat {cur_quat.shape}" + assert(cur_vel.shape == (3,)), f"cur_vel {cur_vel.shape}" + assert(cur_ang_vel.shape == (3,)), f"cur_ang_vel {cur_ang_vel.shape}" + assert(target_pos.shape == (3,)), f"target_pos {target_pos.shape}" + assert(target_rpy.shape == (3,)), f"target_rpy {target_rpy.shape}" + assert(target_vel.shape == (3,)), f"target_vel {target_vel.shape}" + assert(target_rpy_rates.shape == (3,)), f"target_rpy_rates {target_rpy_rates.shape}" + + G = np.array([.0, .0, -9.8]) + K_P = np.array([3., 3., 8.]) + K_D = np.array([2.5, 2.5, 5.]) + K_RATES = np.array([5., 5., 1.]) + P = target_pos - cur_pos + D = target_vel - cur_vel + tar_acc = K_P * P + K_D * D - G + norm_thrust = np.dot(tar_acc, rotate_vector([.0, .0, 1.], cur_quat)) + # Calculate target attitude + z_body = normalized_vector(tar_acc) + x_body = normalized_vector(np.cross(np.array([.0, 1., .0]), z_body)) + y_body = normalized_vector(np.cross(z_body, x_body)) + tar_att = mat2quat(np.vstack([x_body, y_body, z_body]).T) + # Calculate body rates + q_error = qmult(qconjugate(cur_quat), tar_att) + body_rates = 2 * K_RATES * q_error[1:] + if q_error[0] < 0: + body_rates = -body_rates + + return norm_thrust, *body_rates + +################################################################################ + + def setPIDCoefficients(self, + p_coeff_pos=None, + i_coeff_pos=None, + d_coeff_pos=None, + p_coeff_att=None, + i_coeff_att=None, + d_coeff_att=None + ): + """Sets the coefficients of a PID controller. + + This method throws an error message and exist is the coefficients + were not initialized (e.g. when the controller is not a PID one). + + Parameters + ---------- + p_coeff_pos : ndarray, optional + (3,1)-shaped array of floats containing the position control proportional coefficients. + i_coeff_pos : ndarray, optional + (3,1)-shaped array of floats containing the position control integral coefficients. + d_coeff_pos : ndarray, optional + (3,1)-shaped array of floats containing the position control derivative coefficients. + p_coeff_att : ndarray, optional + (3,1)-shaped array of floats containing the attitude control proportional coefficients. + i_coeff_att : ndarray, optional + (3,1)-shaped array of floats containing the attitude control integral coefficients. + d_coeff_att : ndarray, optional + (3,1)-shaped array of floats containing the attitude control derivative coefficients. + + """ + ATTR_LIST = ['P_COEFF_FOR', 'I_COEFF_FOR', 'D_COEFF_FOR', 'P_COEFF_TOR', 'I_COEFF_TOR', 'D_COEFF_TOR'] + if not all(hasattr(self, attr) for attr in ATTR_LIST): + print("[ERROR] in BaseControl.setPIDCoefficients(), not all PID coefficients exist as attributes in the instantiated control class.") + exit() + else: + self.P_COEFF_FOR = self.P_COEFF_FOR if p_coeff_pos is None else p_coeff_pos + self.I_COEFF_FOR = self.I_COEFF_FOR if i_coeff_pos is None else i_coeff_pos + self.D_COEFF_FOR = self.D_COEFF_FOR if d_coeff_pos is None else d_coeff_pos + self.P_COEFF_TOR = self.P_COEFF_TOR if p_coeff_att is None else p_coeff_att + self.I_COEFF_TOR = self.I_COEFF_TOR if i_coeff_att is None else i_coeff_att + self.D_COEFF_TOR = self.D_COEFF_TOR if d_coeff_att is None else d_coeff_att + + ################################################################################ + + def _getURDFParameter(self, + parameter_name: str + ): + """Reads a parameter from a drone's URDF file. + + This method is nothing more than a custom XML parser for the .urdf + files in folder `assets/`. + + Parameters + ---------- + parameter_name : str + The name of the parameter to read. + + Returns + ------- + float + The value of the parameter. + + """ + #### Get the XML tree of the drone model to control ######## + URDF = self.DRONE_MODEL.value + ".urdf" + path = pkg_resources.resource_filename('gym_pybullet_drones', 'assets/'+URDF) + URDF_TREE = etxml.parse(path).getroot() + #### Find and return the desired parameter ################# + if parameter_name == 'm': + return float(URDF_TREE[1][0][1].attrib['value']) + elif parameter_name in ['ixx', 'iyy', 'izz']: + return float(URDF_TREE[1][0][2].attrib[parameter_name]) + elif parameter_name in ['arm', 'thrust2weight', 'kf', 'km', 'max_speed_kmh', 'gnd_eff_coeff' 'prop_radius', \ + 'drag_coeff_xy', 'drag_coeff_z', 'dw_coeff_1', 'dw_coeff_2', 'dw_coeff_3']: + return float(URDF_TREE[0].attrib[parameter_name]) + elif parameter_name in ['length', 'radius']: + return float(URDF_TREE[1][2][1][0].attrib[parameter_name]) + elif parameter_name == 'collision_z_offset': + COLLISION_SHAPE_OFFSETS = [float(s) for s in URDF_TREE[1][2][0].attrib['xyz'].split(' ')] + return COLLISION_SHAPE_OFFSETS[2] diff --git a/gym_pybullet_drones/envs/BetaAviary.py b/gym_pybullet_drones/envs/BetaAviary.py new file mode 100644 index 000000000..39c449636 --- /dev/null +++ b/gym_pybullet_drones/envs/BetaAviary.py @@ -0,0 +1,309 @@ +import numpy as np +from gymnasium import spaces +import socket +import struct + +from transforms3d.quaternions import rotate_vector, qconjugate + +from gym_pybullet_drones.envs.BaseAviary import BaseAviary +from gym_pybullet_drones.utils.enums import DroneModel, Physics + +class BetaAviary(BaseAviary): + """Multi-drone environment class for use of BetaFlight controller.""" + + ################################################################################ + + def __init__(self, + drone_model: DroneModel=DroneModel.CF2X, + num_drones: int=1, + neighbourhood_radius: float=np.inf, + initial_xyzs=None, + initial_rpys=None, + physics: Physics=Physics.PYB, + pyb_freq: int = 240, + ctrl_freq: int = 240, + gui=False, + record=False, + obstacles=False, + user_debug_gui=True, + output_folder='results', + udp_ip="127.0.0.1" + ): + """Initialization of an aviary environment for use of BetaFlight controller. + + Parameters + ---------- + drone_model : DroneModel, optional + The desired drone type (detailed in an .urdf file in folder `assets`). + num_drones : int, optional + The desired number of drones in the aviary. + neighbourhood_radius : float, optional + Radius used to compute the drones' adjacency matrix, in meters. + initial_xyzs: ndarray | None, optional + (NUM_DRONES, 3)-shaped array containing the initial XYZ position of the drones. + initial_rpys: ndarray | None, optional + (NUM_DRONES, 3)-shaped array containing the initial orientations of the drones (in radians). + physics : Physics, optional + The desired implementation of PyBullet physics/custom dynamics. + pyb_freq : int, optional + The frequency at which PyBullet steps (a multiple of ctrl_freq). + ctrl_freq : int, optional + The frequency at which the environment steps. + gui : bool, optional + Whether to use PyBullet's GUI. + record : bool, optional + Whether to save a video of the simulation in folder `files/videos/`. + obstacles : bool, optional + Whether to add obstacles to the simulation. + user_debug_gui : bool, optional + Whether to draw the drones' axes and the GUI RPMs sliders. + udp_ip : base ip for betaflight controller emulator + + """ + super().__init__(drone_model=drone_model, + num_drones=num_drones, + neighbourhood_radius=neighbourhood_radius, + initial_xyzs=initial_xyzs, + initial_rpys=initial_rpys, + physics=physics, + pyb_freq=pyb_freq, + ctrl_freq=ctrl_freq, + gui=gui, + record=record, + obstacles=obstacles, + user_debug_gui=user_debug_gui, + output_folder=output_folder + ) + + # Initialize connection to BetaFlight Controller + self.UDP_IP = udp_ip + self.ARM_TIME = 1 + self.TRAJ_TIME = 1.5 + + self.sock = socket.socket(socket.AF_INET, # Internet + socket.SOCK_DGRAM) # UDP + + self.sock_pwm = socket.socket(socket.AF_INET, # Internet + socket.SOCK_DGRAM) # UDP + self.sock_pwm.bind((self.UDP_IP, 9002)) + self.sock_pwm.settimeout(0.0) + + self.last_action = np.zeros((self.NUM_DRONES, 4)) + + ################################################################################ + + def step(self, action, i): + # Action is (thro, roll, pitch, yaw) + t = i/self.CTRL_FREQ + + thro = 1000 # Positive up + yaw = 1500 # Positive CCW + pitch = 1500 # Positive forward in x + roll = 1500 # Positive right/forward in y + + if t > self.TRAJ_TIME: + thro, roll, pitch, yaw = self.ctbr2beta(*action[0]) + + #### RC message to Betaflight ############################## + aux1 = 1000 if t < self.ARM_TIME else 1500 # Arm + rc_packet = struct.pack( + '@dHHHHHHHHHHHHHHHH', + t, # datetime.now().timestamp(), # double timestamp; // in seconds + round(roll), round(pitch), round(thro), round(yaw), # roll, pitch, throttle, yaw + aux1, 1000, 1000, 1000, # aux 1, .. + 1000, 1000, 1000, 1000, + 1000, 1000, 1000, 1000 + ) + # print("rc", struct.unpack('@dHHHHHHHHHHHHHHHH', rc_packet)) + self.sock.sendto(rc_packet, (self.UDP_IP, 9004)) + + #### PWMs message from Betaflight ########################## + try: + data, addr = self.sock_pwm.recvfrom(16) # buffer size is 100 bytes (servo_packet size 16) + except socket.error as msg: + _action = self.last_action + pass + else: + # print("received message: ", data) + _action = np.array(struct.unpack('@ffff', data)).reshape((1,4)) + + obs, reward, terminated, truncated, info = super().step(_action) + self.last_action = _action + + #### State message to Betaflight ########################### + o = obs[0] # p, q, euler, v, w, rpm (all in world frame) + p = o[:3] + q = np.array([o[6], o[3], o[4], o[5]]) # w, x, y, z + v = o[10:13] + w = o[13:16] # world frame + w_body = rotate_vector(w, qconjugate(q)) # local frame + + fdm_packet = struct.pack( + '@dddddddddddddddddd', # t, w, a, q, v, p, pressure + t, # double timestamp in seconds + # minus signs due to ENU to NED conversion + w_body[0], -w_body[1], -w_body[2], # double imu_angular_velocity_rpy[3] + 0, 0, 0, # double imu_linear_acceleration_xyz[3] + 1., .0, .0, 0., # double imu_orientation_quat[4] w, x, y, z + 0, 0, 0, # double velocity_xyz[3] + 0, 0, 0, # double position_xyz[3] + 1.0 # double pressure; + ) + self.sock.sendto(fdm_packet, (self.UDP_IP, 9003)) + + return obs, reward, terminated, truncated, info + + ################################################################################ + + def ctbr2beta(self, thrust, roll, pitch, yaw): + MIN_CHANNEL = 1000 + MAX_CHANNEL = 2000 + MAX_RATE = 360 + MAX_THRUST = 40.9 + mid = (MAX_CHANNEL + MIN_CHANNEL) / 2 + d = (MAX_CHANNEL - MIN_CHANNEL) / 2 + thrust = thrust / MAX_THRUST * d * 2 + MIN_CHANNEL + rates = np.array([roll, pitch, -yaw]) + rates = rates / np.pi * 180 / MAX_RATE * d + mid + thrust = np.clip(thrust, MIN_CHANNEL, MAX_CHANNEL) + rates = np.clip(rates, MIN_CHANNEL, MAX_CHANNEL) + return thrust, *rates + + ################################################################################ + + def _actionSpace(self): + """Returns the action space of the environment. + + Returns + ------- + spaces.Box + An ndarray of shape (NUM_DRONES, 4) for the commanded RPMs. + + """ + #### Action vector ######## P0 P1 P2 P3 + act_lower_bound = np.array([[0., 0., 0., 0.] for i in range(self.NUM_DRONES)]) + act_upper_bound = np.array([[self.MAX_RPM, self.MAX_RPM, self.MAX_RPM, self.MAX_RPM] for i in range(self.NUM_DRONES)]) + return spaces.Box(low=act_lower_bound, high=act_upper_bound, dtype=np.float32) + + ################################################################################ + + def _observationSpace(self): + """Returns the observation space of the environment. + + Returns + ------- + spaces.Box + The observation space, i.e., an ndarray of shape (NUM_DRONES, 20). + + """ + #### Observation vector ### X Y Z Q1 Q2 Q3 Q4 R P Y VX VY VZ WX WY WZ P0 P1 P2 P3 + obs_lower_bound = np.array([[-np.inf, -np.inf, 0., -1., -1., -1., -1., -np.pi, -np.pi, -np.pi, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, 0., 0., 0., 0.] for i in range(self.NUM_DRONES)]) + obs_upper_bound = np.array([[np.inf, np.inf, np.inf, 1., 1., 1., 1., np.pi, np.pi, np.pi, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf, self.MAX_RPM, self.MAX_RPM, self.MAX_RPM, self.MAX_RPM] for i in range(self.NUM_DRONES)]) + return spaces.Box(low=obs_lower_bound, high=obs_upper_bound, dtype=np.float32) + + ################################################################################ + + def _computeObs(self): + """Returns the current observation of the environment. + + For the value of the state, see the implementation of `_getDroneStateVector()`. + + Returns + ------- + ndarray + An ndarray of shape (NUM_DRONES, 20) with the state of each drone. + + """ + return np.array([self._getDroneStateVector(i) for i in range(self.NUM_DRONES)]) + + ################################################################################ + + def _preprocessAction(self, + action + ): + """Pre-processes the action passed to `.step()` into motors' RPMs. + + Clips and converts a dictionary into a 2D array. + + Parameters + ---------- + action : ndarray + The (unbounded) input action for each drone, to be translated into feasible RPMs. + + Returns + ------- + ndarray + (NUM_DRONES, 4)-shaped array of ints containing to clipped RPMs + commanded to the 4 motors of each drone. + + """ + remapped_input = np.array([[ + action[i][2], # 0 + action[i][1], # 1 + action[i][3], # 2 + action[i][0] # 3 + ] for i in range(self.NUM_DRONES)]) # Betaflight SITL motor mapping + + ret = np.array(np.sqrt(self.MAX_THRUST / 4 / self.KF * remapped_input)) + assert(ret.shape == (self.NUM_DRONES, 4)), "Error in preprocess action" + return ret + + ################################################################################ + + def _computeReward(self): + """Computes the current reward value(s). + + Unused as this subclass is not meant for reinforcement learning. + + Returns + ------- + int + Dummy value. + + """ + return -1 + + ################################################################################ + + def _computeTerminated(self): + """Computes the current terminated value(s). + + Unused as this subclass is not meant for reinforcement learning. + + Returns + ------- + bool + Dummy value. + + """ + return False + + ################################################################################ + + def _computeTruncated(self): + """Computes the current truncated value(s). + + Unused as this subclass is not meant for reinforcement learning. + + Returns + ------- + bool + Dummy value. + + """ + return False + + ################################################################################ + + def _computeInfo(self): + """Computes the current info dict(s). + + Unused as this subclass is not meant for reinforcement learning. + + Returns + ------- + dict[str, int] + Dummy value. + + """ + return {"answer": 42} #### Calculated by the Deep Thought supercomputer in 7.5M years diff --git a/gym_pybullet_drones/examples/beta2.py b/gym_pybullet_drones/examples/beta2.py new file mode 100644 index 000000000..07384ba02 --- /dev/null +++ b/gym_pybullet_drones/examples/beta2.py @@ -0,0 +1,176 @@ +"""Control + Betaflight. + +Setup +----- +Step 1: Clone betaflight: + $ git clone https://github.com/betaflight/betaflight + $ cd betaflight/ + +Step 2: Comment out line `delayMicroseconds_real(50); // max rate 20kHz` + (https://github.com/betaflight/betaflight/blob/master/src/main/main.c#L52) + from Betaflight's `SIMULATOR_BUILD` and compile: + $ cd betaflight/ + $ make arm_sdk_install + $ make TARGET=SITL + +Step 3: Install betaflight configurator (https://github.com/betaflight/betaflight-configurator/releases): + $ wget https://github.com/betaflight/betaflight-configurator/releases/download/10.9.0/betaflight-configurator_10.9.0_amd64.deb + $ sudo dpkg -i betaflight-configurator_10.9.0_amd64.deb + If needed, also run: + $ apt install libgconf-2-4 + $ apt --fix-broken install + +Step 4: Load the configuration file onto the target using the BF configurator: + First, start the SITL controller: + $ ./obj/main/betaflight_SITL.elf + Type address `tcp://localhost:5761` (top right) and click `Connect` + Find the `Presets` tab (on the left) -> `Load backup` -> select file `../assets/beta.txt` + Restart `betaflight_SITL.elf` + +Example +------- +In one terminal run the SITL Betaflight: + + $ cd betaflight/ + $ ./obj/main/betaflight_SITL.elf + +In a separate terminal, run: + + $ cd gym-pybullet-drones/gym_pybullet_drones/examples/ + $ python beta.py + +""" +import time +import argparse +import numpy as np +import csv + +from transforms3d.quaternions import rotate_vector, qconjugate, mat2quat, qmult +from transforms3d.utils import normalized_vector + +from gym_pybullet_drones.utils.enums import DroneModel, Physics +from gym_pybullet_drones.envs.BetaAviary import BetaAviary +from gym_pybullet_drones.control.CTBRControl import CTBRControl +from gym_pybullet_drones.utils.Logger import Logger +from gym_pybullet_drones.utils.utils import sync, str2bool + +DEFAULT_DRONES = DroneModel("racer") +DEFAULT_PHYSICS = Physics("pyb") +DEFAULT_GUI = True +DEFAULT_PLOT = True +DEFAULT_USER_DEBUG_GUI = False +DEFAULT_SIMULATION_FREQ_HZ = 500 +DEFAULT_CONTROL_FREQ_HZ = 500 +DEFAULT_DURATION_SEC = 20 +DEFAULT_OUTPUT_FOLDER = 'results' + +def run( + drone=DEFAULT_DRONES, + physics=DEFAULT_PHYSICS, + gui=DEFAULT_GUI, + plot=DEFAULT_PLOT, + user_debug_gui=DEFAULT_USER_DEBUG_GUI, + simulation_freq_hz=DEFAULT_SIMULATION_FREQ_HZ, + control_freq_hz=DEFAULT_CONTROL_FREQ_HZ, + duration_sec=DEFAULT_DURATION_SEC, + output_folder=DEFAULT_OUTPUT_FOLDER, + ): + #### Create the environment with or without video capture ## + env = BetaAviary(drone_model=drone, + num_drones=1, + initial_xyzs=np.array([[.0, .0, .1]]), + initial_rpys=np.array([[.0, .0, .0]]), + physics=physics, + pyb_freq=simulation_freq_hz, + ctrl_freq=control_freq_hz, + gui=gui, + user_debug_gui=user_debug_gui + ) + + ctrl = CTBRControl(drone_model=drone) + + #### 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=1, + output_folder=output_folder, + ) + + #### Run the simulation #################################### + with open("../assets/beta.csv", mode='r') as csv_file: + csv_reader = csv.DictReader(csv_file) + trajectory = iter([{ + "pos": np.array([ + float(row["p_x"]), + float(row["p_y"]), + float(row["p_z"]), + ]), + "vel": np.array([ + float(row["v_x"]), + float(row["v_y"]), + float(row["v_z"]), + ]), + } for row in csv_reader]) + action = np.zeros((1,4)) + ARM_TIME = 1. + TRAJ_TIME = 1.5 + START = time.time() + for i in range(0, int(duration_sec*env.CTRL_FREQ)): + t = i/env.CTRL_FREQ + #### Step the simulation ################################### + obs, reward, terminated, truncated, info = env.step(action, i) + + if t > env.TRAJ_TIME: + try: + target = next(trajectory) + action[0,:] = ctrl.computeControlFromState(control_timestep=env.CTRL_TIMESTEP, + state=obs[0], + target_pos=target["pos"], + target_vel=target["vel"] + ) + except: + break + + + #### Log the simulation #################################### + logger.log(drone=0, + timestamp=i/env.CTRL_FREQ, + state=obs[0] + ) + + #### Printout ############################################## + env.render() + + #### Sync the simulation ################################### + if gui: + pass + sync(i, START, env.CTRL_TIMESTEP) + + #### Close the environment ################################# + env.close() + + #### Save the simulation results ########################### + logger.save() + logger.save_as_csv("beta") # 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='Test flight script using SITL Betaflight') + parser.add_argument('--drone', default=DEFAULT_DRONES, type=DroneModel, help='Drone model (default: BETA)', metavar='', choices=DroneModel) + 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('--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('--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='') + ARGS = parser.parse_args() + + run(**vars(ARGS)) From 645896c9c810e5b5b6cecc97a5ba3d1b731e15a3 Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Sun, 8 Oct 2023 17:49:36 +0400 Subject: [PATCH 02/15] Use eeprom.bin instead of BF configurator --- README.md | 3 +- gym_pybullet_drones/assets/beta.txt | 82 ------------ gym_pybullet_drones/assets/eeprom.bin | Bin 0 -> 32768 bytes gym_pybullet_drones/examples/beta.py | 167 +++--------------------- gym_pybullet_drones/examples/beta2.py | 176 -------------------------- 5 files changed, 22 insertions(+), 406 deletions(-) delete mode 100644 gym_pybullet_drones/assets/beta.txt create mode 100644 gym_pybullet_drones/assets/eeprom.bin delete mode 100644 gym_pybullet_drones/examples/beta2.py diff --git a/README.md b/README.md index 9af67940a..305726787 100644 --- a/README.md +++ b/README.md @@ -40,13 +40,12 @@ python3 learn.py ### Betaflight SITL example (Ubuntu only) -First, check the steps in the docstrings of [`beta.py`](https://github.com/utiasDSL/gym-pybullet-drones/blob/main/gym_pybullet_drones/examples/beta.py), then, in one terminal, run the Betaflight SITL binary - ```sh git clone https://github.com/betaflight/betaflight cd betaflight/ make arm_sdk_install # if needed, `apt install curl`` make TARGET=SITL # comment out this line: https://github.com/betaflight/betaflight/blob/master/src/main/main.c#L52 +cp cp ~/gym-pybullet-drones/gym_pybullet_drones/assets/eeprom.bin ~/betaflight/ betaflight/obj/main/betaflight_SITL.elf ``` diff --git a/gym_pybullet_drones/assets/beta.txt b/gym_pybullet_drones/assets/beta.txt deleted file mode 100644 index bf44da53f..000000000 --- a/gym_pybullet_drones/assets/beta.txt +++ /dev/null @@ -1,82 +0,0 @@ -defaults nosave - - -# version -# Betaflight / SITL (SITL) 4.5.0 Jul 7 2023 / 10:53:28 (b54ae921f) MSP API: 1.46 - -# start the command batch -batch start - -# reset configuration to default settings -defaults nosave - -mcu_id 000000000000000100000002 -signature - -# feature -feature -GPS -feature -TELEMETRY - -# aux -aux 0 0 0 1300 1700 0 0 - -# master -set motor_pwm_protocol = PWM -set pid_process_denom = 16 - -profile 0 - -# profile 0 -set pidsum_limit = 1000 -set pidsum_limit_yaw = 1000 -set p_pitch = 70 -set i_pitch = 75 -set d_pitch = 43 -set f_pitch = 224 -set p_roll = 58 -set i_roll = 62 -set d_roll = 42 -set f_roll = 187 -set p_yaw = 58 -set i_yaw = 62 -set f_yaw = 187 -set d_min_roll = 42 -set d_min_pitch = 43 -set simplified_master_multiplier = 130 -set simplified_i_gain = 60 -set simplified_d_gain = 110 -set simplified_dmax_gain = 0 -set simplified_feedforward_gain = 120 -set simplified_pitch_d_gain = 90 -set simplified_pitch_pi_gain = 115 - -profile 1 - -profile 2 - -profile 3 - -# restore original profile selection -profile 0 - -rateprofile 0 - -# rateprofile 0 -set rates_type = BETAFLIGHT -set roll_rc_rate = 180 -set pitch_rc_rate = 180 -set yaw_rc_rate = 180 -set roll_srate = 0 -set pitch_srate = 0 -set yaw_srate = 0 - -rateprofile 1 - -rateprofile 2 - -rateprofile 3 - -# restore original rateprofile selection -rateprofile 0 - -# save configuration \ No newline at end of file diff --git a/gym_pybullet_drones/assets/eeprom.bin b/gym_pybullet_drones/assets/eeprom.bin new file mode 100644 index 0000000000000000000000000000000000000000..a3e16b7d7130f2dba63d6e3c2ce81f4010498789 GIT binary patch literal 32768 zcmeI!zfTik7zgn0bJw>A2eu7f8e2&v5U`YA<|ac3L7Oy@IuON$f$5!$VKEvO7#wtE zVqkD~(xsb;lLM3S4{%^?oEg*wpSx?}Km;Wk2fy#7*XR1)z3+3M_ulvV=ADnPg1A&M z11Aa3+!rya^=NBR}Q^}Oh~Ua#Yi);46k`!p1W zjyh!YTLMLlsAB2FHG`Zu!z|f->E<@IRK+@GmZ!0SDNSJgL~i@CB^S@FGJH;MHYs8) zb4!is#oQj^l0J^{18Fik@=1|Y?4)8h6=zbhNyXVz>~&&`dlQ1KL9B8m$db*8QgWY+5Fa|5gCgIuH)_j-MF!BaR!7?wB*3 ziT;4R@^YGZpl#}?jp$o(APS;U7IXj3@?8xb&9^(&dI#F*mVSES=zh(Y9LADM;;IJ5 zlY5SPyXdYTH={wd5o8YvQJ0?qmDe0zJB;EBO7YAjGIH~7P6F4|bVrTI% zWIzE5P=Epypa2CZKmiI+fC3bt00k&O0SZun0u-PC1t>rP3Q&Lo6rcbFC_n)UP=Epy zpa2CZKmiI+fC3bt00k&O0SZun0u-PC1t>rP3Q&Lo6rcbFC_n)UP=Epypa2CZKmiI+ zfC3bt00k&O0SZun0u-PC1t>rP3Q&Lo6rcbFC_n)UP=Epypa2CZKmiI+fC3bt00k&O M0SZun0)JQFJL>qrlK=n! literal 0 HcmV?d00001 diff --git a/gym_pybullet_drones/examples/beta.py b/gym_pybullet_drones/examples/beta.py index db96c991c..b092ad423 100644 --- a/gym_pybullet_drones/examples/beta.py +++ b/gym_pybullet_drones/examples/beta.py @@ -2,9 +2,10 @@ Setup ----- -Step 1: Clone betaflight: +Step 1: Clone and open betaflight's source: $ git clone https://github.com/betaflight/betaflight - $ cd betaflight/ + $ cd betaflight/ + $ code ./src/main/main.c Step 2: Comment out line `delayMicroseconds_real(50); // max rate 20kHz` (https://github.com/betaflight/betaflight/blob/master/src/main/main.c#L52) @@ -13,19 +14,9 @@ $ make arm_sdk_install $ make TARGET=SITL -Step 3: Install betaflight configurator (https://github.com/betaflight/betaflight-configurator/releases): - $ wget https://github.com/betaflight/betaflight-configurator/releases/download/10.9.0/betaflight-configurator_10.9.0_amd64.deb - $ sudo dpkg -i betaflight-configurator_10.9.0_amd64.deb - If needed, also run: - $ apt install libgconf-2-4 - $ apt --fix-broken install - -Step 4: Load the configuration file onto the target using the BF configurator: - First, start the SITL controller: - $ ./obj/main/betaflight_SITL.elf - Type address `tcp://localhost:5761` (top right) and click `Connect` - Find the `Presets` tab (on the left) -> `Load backup` -> select file `../assets/beta.txt` - Restart `betaflight_SITL.elf` +Step 3: Copy over the configured `eeprom.bin` file from folder + `gym-pybullet-drones/gym_pybullet_drones/assets/` to BF's main folder `betaflight/` + $ cp ~/gym-pybullet-drones/gym_pybullet_drones/assets/eeprom.bin ~/betaflight/ Example ------- @@ -40,25 +31,17 @@ $ python beta.py """ -import os import time import argparse -from datetime import datetime -import socket -import struct -import pdb -import math -import random import numpy as np -import pybullet as p -import matplotlib.pyplot as plt import csv from transforms3d.quaternions import rotate_vector, qconjugate, mat2quat, qmult from transforms3d.utils import normalized_vector from gym_pybullet_drones.utils.enums import DroneModel, Physics -from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary +from gym_pybullet_drones.envs.BetaAviary import BetaAviary +from gym_pybullet_drones.control.CTBRControl import CTBRControl from gym_pybullet_drones.utils.Logger import Logger from gym_pybullet_drones.utils.utils import sync, str2bool @@ -72,22 +55,6 @@ DEFAULT_DURATION_SEC = 20 DEFAULT_OUTPUT_FOLDER = 'results' -UDP_IP = "127.0.0.1" - -sock = socket.socket(socket.AF_INET, # Internet - socket.SOCK_DGRAM) # UDP - -sock_pwm = socket.socket(socket.AF_INET, # Internet - socket.SOCK_DGRAM) # UDP -sock_pwm.bind((UDP_IP, 9002)) -sock_pwm.settimeout(0.0) - -# sock_raw = socket.socket(socket.AF_INET, # Internet -# socket.SOCK_DGRAM) # UDP -# sock_raw.bind((UDP_IP, 9001)) -# sock_raw.settimeout(0.0) - - def run( drone=DEFAULT_DRONES, physics=DEFAULT_PHYSICS, @@ -100,7 +67,7 @@ def run( output_folder=DEFAULT_OUTPUT_FOLDER, ): #### Create the environment with or without video capture ## - env = CtrlAviary(drone_model=drone, + env = BetaAviary(drone_model=drone, num_drones=1, initial_xyzs=np.array([[.0, .0, .1]]), initial_rpys=np.array([[.0, .0, .0]]), @@ -111,6 +78,8 @@ def run( user_debug_gui=user_debug_gui ) + ctrl = CTBRControl(drone_model=drone) + #### Obtain the PyBullet Client ID from the environment #### PYB_CLIENT = env.getPyBulletClient() @@ -140,80 +109,21 @@ def run( TRAJ_TIME = 1.5 START = time.time() for i in range(0, int(duration_sec*env.CTRL_FREQ)): - - #### Step the simulation ################################### - obs, reward, terminated, truncated, info = env.step(action) - - #### State message to Betaflight ########################### - o = obs[0] # p, q, euler, v, w, rpm (all in world frame) - p = o[:3] - q = np.array([o[6], o[3], o[4], o[5]]) # w, x, y, z - v = o[10:13] - w = o[13:16] # world frame - w_body = rotate_vector(w, qconjugate(q)) # local frame t = i/env.CTRL_FREQ - fdm_packet = struct.pack( - '@dddddddddddddddddd', # t, w, a, q, v, p, pressure - t, # double timestamp in seconds - # minus signs due to ENU to NED conversion - w_body[0], -w_body[1], -w_body[2], # double imu_angular_velocity_rpy[3] - 0, 0, 0, # double imu_linear_acceleration_xyz[3] - 1., .0, .0, 0., # double imu_orientation_quat[4] w, x, y, z - 0, 0, 0, # double velocity_xyz[3] - 0, 0, 0, # double position_xyz[3] - 1.0 # double pressure; - ) - sock.sendto(fdm_packet, (UDP_IP, 9003)) - - #### RC defaults to Betaflight ############################# - thro = 1000 # Positive up - yaw = 1500 # Positive CCW - pitch = 1500 # Positive forward in x - roll = 1500 # Positive right/forward in y - if t > TRAJ_TIME: + #### Step the simulation ################################### + obs, reward, terminated, truncated, info = env.step(action, i) + + if t > env.TRAJ_TIME: try: target = next(trajectory) + action[0,:] = ctrl.computeControlFromState(control_timestep=env.CTRL_TIMESTEP, + state=obs[0], + target_pos=target["pos"], + target_vel=target["vel"] + ) except: break - - thro, roll, pitch, yaw = ctbr_controller( - target["pos"], - target["vel"], - # np.array([.0, .0, z]), - # np.array([.0, .0, vz]), - p, v, q - ) - thro, roll, pitch, yaw = ctbr2beta(thro, roll, pitch, yaw) - - #### RC message to Betaflight ############################## - aux1 = 1000 if t < ARM_TIME else 1500 # Arm - rc_packet = struct.pack( - '@dHHHHHHHHHHHHHHHH', - t, # datetime.now().timestamp(), # double timestamp; // in seconds - round(roll), round(pitch), round(thro), round(yaw), # roll, pitch, throttle, yaw - aux1, 1000, 1000, 1000, # aux 1, .. - 1000, 1000, 1000, 1000, - 1000, 1000, 1000, 1000 - ) - # print("rc", struct.unpack('@dHHHHHHHHHHHHHHHH', rc_packet)) - sock.sendto(rc_packet, (UDP_IP, 9004)) - - #### PWMs message from Betaflight ########################## - try: - data, addr = sock_pwm.recvfrom(16) # buffer size is 100 bytes (servo_packet size 16) - except socket.error as msg: - # print(msg) - pass - else: - # print("received message: ", data) - betaflight_pwms = np.array(struct.unpack('@ffff', data)) - remapped_input = np.array([ - betaflight_pwms[2], # 0 - betaflight_pwms[1], # 1 - betaflight_pwms[3], # 2 - betaflight_pwms[0] # 3 - ]) # Betaflight SITL motor mapping - action = np.array([np.sqrt(env.MAX_THRUST / 4 / env.KF * remapped_input)]) + #### Log the simulation #################################### logger.log(drone=0, @@ -240,41 +150,6 @@ def run( if plot: logger.plot() -def ctbr_controller(tar_pos, tar_vel, cur_pos, cur_vel, cur_att): - G = np.array([.0, .0, -9.8]) - K_P = np.array([3., 3., 8.]) - K_D = np.array([2.5, 2.5, 5.]) - K_RATES = np.array([5., 5., 1.]) - P = tar_pos - cur_pos - D = tar_vel - cur_vel - tar_acc = K_P * P + K_D * D - G - norm_thrust = np.dot(tar_acc, rotate_vector([.0, .0, 1.], cur_att)) - # Calculate target attitude - z_body = normalized_vector(tar_acc) - x_body = normalized_vector(np.cross(np.array([.0, 1., .0]), z_body)) - y_body = normalized_vector(np.cross(z_body, x_body)) - tar_att = mat2quat(np.vstack([x_body, y_body, z_body]).T) - # Calculate body rates - q_error = qmult(qconjugate(cur_att), tar_att) - body_rates = 2 * K_RATES * q_error[1:] - if q_error[0] < 0: - body_rates = -body_rates - return norm_thrust, *body_rates - -def ctbr2beta(thrust, roll, pitch, yaw): - MIN_CHANNEL = 1000 - MAX_CHANNEL = 2000 - MAX_RATE = 360 - MAX_THRUST = 40.9 - mid = (MAX_CHANNEL + MIN_CHANNEL) / 2 - d = (MAX_CHANNEL - MIN_CHANNEL) / 2 - thrust = thrust / MAX_THRUST * d * 2 + MIN_CHANNEL - rates = np.array([roll, pitch, -yaw]) - rates = rates / np.pi * 180 / MAX_RATE * d + mid - thrust = np.clip(thrust, MIN_CHANNEL, MAX_CHANNEL) - rates = np.clip(rates, MIN_CHANNEL, MAX_CHANNEL) - return thrust, *rates - if __name__ == "__main__": #### Define and parse (optional) arguments for the script ## parser = argparse.ArgumentParser(description='Test flight script using SITL Betaflight') diff --git a/gym_pybullet_drones/examples/beta2.py b/gym_pybullet_drones/examples/beta2.py deleted file mode 100644 index 07384ba02..000000000 --- a/gym_pybullet_drones/examples/beta2.py +++ /dev/null @@ -1,176 +0,0 @@ -"""Control + Betaflight. - -Setup ------ -Step 1: Clone betaflight: - $ git clone https://github.com/betaflight/betaflight - $ cd betaflight/ - -Step 2: Comment out line `delayMicroseconds_real(50); // max rate 20kHz` - (https://github.com/betaflight/betaflight/blob/master/src/main/main.c#L52) - from Betaflight's `SIMULATOR_BUILD` and compile: - $ cd betaflight/ - $ make arm_sdk_install - $ make TARGET=SITL - -Step 3: Install betaflight configurator (https://github.com/betaflight/betaflight-configurator/releases): - $ wget https://github.com/betaflight/betaflight-configurator/releases/download/10.9.0/betaflight-configurator_10.9.0_amd64.deb - $ sudo dpkg -i betaflight-configurator_10.9.0_amd64.deb - If needed, also run: - $ apt install libgconf-2-4 - $ apt --fix-broken install - -Step 4: Load the configuration file onto the target using the BF configurator: - First, start the SITL controller: - $ ./obj/main/betaflight_SITL.elf - Type address `tcp://localhost:5761` (top right) and click `Connect` - Find the `Presets` tab (on the left) -> `Load backup` -> select file `../assets/beta.txt` - Restart `betaflight_SITL.elf` - -Example -------- -In one terminal run the SITL Betaflight: - - $ cd betaflight/ - $ ./obj/main/betaflight_SITL.elf - -In a separate terminal, run: - - $ cd gym-pybullet-drones/gym_pybullet_drones/examples/ - $ python beta.py - -""" -import time -import argparse -import numpy as np -import csv - -from transforms3d.quaternions import rotate_vector, qconjugate, mat2quat, qmult -from transforms3d.utils import normalized_vector - -from gym_pybullet_drones.utils.enums import DroneModel, Physics -from gym_pybullet_drones.envs.BetaAviary import BetaAviary -from gym_pybullet_drones.control.CTBRControl import CTBRControl -from gym_pybullet_drones.utils.Logger import Logger -from gym_pybullet_drones.utils.utils import sync, str2bool - -DEFAULT_DRONES = DroneModel("racer") -DEFAULT_PHYSICS = Physics("pyb") -DEFAULT_GUI = True -DEFAULT_PLOT = True -DEFAULT_USER_DEBUG_GUI = False -DEFAULT_SIMULATION_FREQ_HZ = 500 -DEFAULT_CONTROL_FREQ_HZ = 500 -DEFAULT_DURATION_SEC = 20 -DEFAULT_OUTPUT_FOLDER = 'results' - -def run( - drone=DEFAULT_DRONES, - physics=DEFAULT_PHYSICS, - gui=DEFAULT_GUI, - plot=DEFAULT_PLOT, - user_debug_gui=DEFAULT_USER_DEBUG_GUI, - simulation_freq_hz=DEFAULT_SIMULATION_FREQ_HZ, - control_freq_hz=DEFAULT_CONTROL_FREQ_HZ, - duration_sec=DEFAULT_DURATION_SEC, - output_folder=DEFAULT_OUTPUT_FOLDER, - ): - #### Create the environment with or without video capture ## - env = BetaAviary(drone_model=drone, - num_drones=1, - initial_xyzs=np.array([[.0, .0, .1]]), - initial_rpys=np.array([[.0, .0, .0]]), - physics=physics, - pyb_freq=simulation_freq_hz, - ctrl_freq=control_freq_hz, - gui=gui, - user_debug_gui=user_debug_gui - ) - - ctrl = CTBRControl(drone_model=drone) - - #### 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=1, - output_folder=output_folder, - ) - - #### Run the simulation #################################### - with open("../assets/beta.csv", mode='r') as csv_file: - csv_reader = csv.DictReader(csv_file) - trajectory = iter([{ - "pos": np.array([ - float(row["p_x"]), - float(row["p_y"]), - float(row["p_z"]), - ]), - "vel": np.array([ - float(row["v_x"]), - float(row["v_y"]), - float(row["v_z"]), - ]), - } for row in csv_reader]) - action = np.zeros((1,4)) - ARM_TIME = 1. - TRAJ_TIME = 1.5 - START = time.time() - for i in range(0, int(duration_sec*env.CTRL_FREQ)): - t = i/env.CTRL_FREQ - #### Step the simulation ################################### - obs, reward, terminated, truncated, info = env.step(action, i) - - if t > env.TRAJ_TIME: - try: - target = next(trajectory) - action[0,:] = ctrl.computeControlFromState(control_timestep=env.CTRL_TIMESTEP, - state=obs[0], - target_pos=target["pos"], - target_vel=target["vel"] - ) - except: - break - - - #### Log the simulation #################################### - logger.log(drone=0, - timestamp=i/env.CTRL_FREQ, - state=obs[0] - ) - - #### Printout ############################################## - env.render() - - #### Sync the simulation ################################### - if gui: - pass - sync(i, START, env.CTRL_TIMESTEP) - - #### Close the environment ################################# - env.close() - - #### Save the simulation results ########################### - logger.save() - logger.save_as_csv("beta") # 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='Test flight script using SITL Betaflight') - parser.add_argument('--drone', default=DEFAULT_DRONES, type=DroneModel, help='Drone model (default: BETA)', metavar='', choices=DroneModel) - 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('--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('--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='') - ARGS = parser.parse_args() - - run(**vars(ARGS)) From e462b8045fce24ff13ec22945af230de750280dd Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Sun, 8 Oct 2023 20:52:33 +0400 Subject: [PATCH 03/15] Added variable number of drones in beta.py --- gym_pybullet_drones/examples/beta.py | 43 ++++++++++++++++------------ 1 file changed, 25 insertions(+), 18 deletions(-) diff --git a/gym_pybullet_drones/examples/beta.py b/gym_pybullet_drones/examples/beta.py index b092ad423..70513e8dc 100644 --- a/gym_pybullet_drones/examples/beta.py +++ b/gym_pybullet_drones/examples/beta.py @@ -54,6 +54,9 @@ DEFAULT_CONTROL_FREQ_HZ = 500 DEFAULT_DURATION_SEC = 20 DEFAULT_OUTPUT_FOLDER = 'results' +NUM_DRONES = 1 +INIT_XYZ = np.array([[.5*i, .5*i, .1] for i in range(NUM_DRONES)]) +INIT_RPY = np.array([[.0, .0, .0] for _ in range(NUM_DRONES)]) def run( drone=DEFAULT_DRONES, @@ -68,9 +71,9 @@ def run( ): #### Create the environment with or without video capture ## env = BetaAviary(drone_model=drone, - num_drones=1, - initial_xyzs=np.array([[.0, .0, .1]]), - initial_rpys=np.array([[.0, .0, .0]]), + num_drones=NUM_DRONES, + initial_xyzs=INIT_XYZ, + initial_rpys=INIT_RPY, physics=physics, pyb_freq=simulation_freq_hz, ctrl_freq=control_freq_hz, @@ -85,7 +88,7 @@ def run( #### Initialize the logger ################################# logger = Logger(logging_freq_hz=control_freq_hz, - num_drones=1, + num_drones=NUM_DRONES, output_folder=output_folder, ) @@ -104,7 +107,7 @@ def run( float(row["v_z"]), ]), } for row in csv_reader]) - action = np.zeros((1,4)) + action = np.zeros((NUM_DRONES,4)) ARM_TIME = 1. TRAJ_TIME = 1.5 START = time.time() @@ -114,22 +117,26 @@ def run( obs, reward, terminated, truncated, info = env.step(action, i) if t > env.TRAJ_TIME: - try: - target = next(trajectory) - action[0,:] = ctrl.computeControlFromState(control_timestep=env.CTRL_TIMESTEP, - state=obs[0], - target_pos=target["pos"], - target_vel=target["vel"] - ) - except: - break + for j in range(NUM_DRONES): + try: + target = next(trajectory) + print(target['pos']) + print(target['vel']) + action[j,:] = ctrl.computeControlFromState(control_timestep=env.CTRL_TIMESTEP, + state=obs[j], + target_pos=target["pos"]+[INIT_XYZ[j][0], INIT_XYZ[j][1], 0], + target_vel=target["vel"] + ) + except: + break #### Log the simulation #################################### - logger.log(drone=0, - timestamp=i/env.CTRL_FREQ, - state=obs[0] - ) + for j in range(NUM_DRONES): + logger.log(drone=j, + timestamp=i/env.CTRL_FREQ, + state=obs[j] + ) #### Printout ############################################## env.render() From 38c78c305f2ae900c96d1316e186162a4eb2f568 Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Tue, 10 Oct 2023 17:39:23 +0400 Subject: [PATCH 04/15] Removed typo, clarified README --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 66866767c..2c512fbc8 100644 --- a/README.md +++ b/README.md @@ -44,8 +44,8 @@ python3 learn.py git clone https://github.com/betaflight/betaflight cd betaflight/ make arm_sdk_install # if needed, `apt install curl`` -make TARGET=SITL # comment out this line: https://github.com/betaflight/betaflight/blob/master/src/main/main.c#L52 -cp cp ~/gym-pybullet-drones/gym_pybullet_drones/assets/eeprom.bin ~/betaflight/ +make TARGET=SITL # comment out line: https://github.com/betaflight/betaflight/blob/master/src/main/main.c#L52 +cp ~/gym-pybullet-drones/gym_pybullet_drones/assets/eeprom.bin ~/betaflight/ # assuming both gym-pybullet-drones/ and betaflight/ were cloned in ~/ betaflight/obj/main/betaflight_SITL.elf ``` From 645b7d57112110452e7e11dce2f1a88c5e537d7c Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Tue, 10 Oct 2023 18:55:56 +0400 Subject: [PATCH 05/15] Renaming last_action attribute and re-ordering betaaviary step method --- gym_pybullet_drones/envs/BetaAviary.py | 114 +++++++++++++------------ 1 file changed, 58 insertions(+), 56 deletions(-) diff --git a/gym_pybullet_drones/envs/BetaAviary.py b/gym_pybullet_drones/envs/BetaAviary.py index 39c449636..4a25bf506 100644 --- a/gym_pybullet_drones/envs/BetaAviary.py +++ b/gym_pybullet_drones/envs/BetaAviary.py @@ -88,68 +88,70 @@ def __init__(self, self.sock_pwm.bind((self.UDP_IP, 9002)) self.sock_pwm.settimeout(0.0) - self.last_action = np.zeros((self.NUM_DRONES, 4)) + self.beta_action = np.zeros((self.NUM_DRONES, 4)) ################################################################################ def step(self, action, i): - # Action is (thro, roll, pitch, yaw) + obs, reward, terminated, truncated, info = super().step(self.beta_action) + t = i/self.CTRL_FREQ - thro = 1000 # Positive up - yaw = 1500 # Positive CCW - pitch = 1500 # Positive forward in x - roll = 1500 # Positive right/forward in y - - if t > self.TRAJ_TIME: - thro, roll, pitch, yaw = self.ctbr2beta(*action[0]) - - #### RC message to Betaflight ############################## - aux1 = 1000 if t < self.ARM_TIME else 1500 # Arm - rc_packet = struct.pack( - '@dHHHHHHHHHHHHHHHH', - t, # datetime.now().timestamp(), # double timestamp; // in seconds - round(roll), round(pitch), round(thro), round(yaw), # roll, pitch, throttle, yaw - aux1, 1000, 1000, 1000, # aux 1, .. - 1000, 1000, 1000, 1000, - 1000, 1000, 1000, 1000 - ) - # print("rc", struct.unpack('@dHHHHHHHHHHHHHHHH', rc_packet)) - self.sock.sendto(rc_packet, (self.UDP_IP, 9004)) - - #### PWMs message from Betaflight ########################## - try: - data, addr = self.sock_pwm.recvfrom(16) # buffer size is 100 bytes (servo_packet size 16) - except socket.error as msg: - _action = self.last_action - pass - else: - # print("received message: ", data) - _action = np.array(struct.unpack('@ffff', data)).reshape((1,4)) - - obs, reward, terminated, truncated, info = super().step(_action) - self.last_action = _action - - #### State message to Betaflight ########################### - o = obs[0] # p, q, euler, v, w, rpm (all in world frame) - p = o[:3] - q = np.array([o[6], o[3], o[4], o[5]]) # w, x, y, z - v = o[10:13] - w = o[13:16] # world frame - w_body = rotate_vector(w, qconjugate(q)) # local frame - - fdm_packet = struct.pack( - '@dddddddddddddddddd', # t, w, a, q, v, p, pressure - t, # double timestamp in seconds - # minus signs due to ENU to NED conversion - w_body[0], -w_body[1], -w_body[2], # double imu_angular_velocity_rpy[3] - 0, 0, 0, # double imu_linear_acceleration_xyz[3] - 1., .0, .0, 0., # double imu_orientation_quat[4] w, x, y, z - 0, 0, 0, # double velocity_xyz[3] - 0, 0, 0, # double position_xyz[3] - 1.0 # double pressure; - ) - self.sock.sendto(fdm_packet, (self.UDP_IP, 9003)) + for j in range(self.NUM_DRONES): #TODO: add multi-drone support + + #### State message to Betaflight ########################### + o = obs[0] # p, q, euler, v, w, rpm (all in world frame) + p = o[:3] + q = np.array([o[6], o[3], o[4], o[5]]) # w, x, y, z + v = o[10:13] + w = o[13:16] # world frame + w_body = rotate_vector(w, qconjugate(q)) # local frame + + fdm_packet = struct.pack( + '@dddddddddddddddddd', # t, w, a, q, v, p, pressure + t, # double timestamp in seconds + # minus signs due to ENU to NED conversion + w_body[0], -w_body[1], -w_body[2], # double imu_angular_velocity_rpy[3] + 0, 0, 0, # double imu_linear_acceleration_xyz[3] + 1., .0, .0, 0., # double imu_orientation_quat[4] w, x, y, z + 0, 0, 0, # double velocity_xyz[3] + 0, 0, 0, # double position_xyz[3] + 1.0 # double pressure; + ) + self.sock.sendto(fdm_packet, (self.UDP_IP, 9003)) + + # Action is (thro, roll, pitch, yaw) + thro = 1000 # Positive up + yaw = 1500 # Positive CCW + pitch = 1500 # Positive forward in x + roll = 1500 # Positive right/forward in y + + if t > self.TRAJ_TIME: + thro, roll, pitch, yaw = self.ctbr2beta(*action[0]) + + #### RC message to Betaflight ############################## + aux1 = 1000 if t < self.ARM_TIME else 1500 # Arm + rc_packet = struct.pack( + '@dHHHHHHHHHHHHHHHH', + t, # datetime.now().timestamp(), # double timestamp; // in seconds + round(roll), round(pitch), round(thro), round(yaw), # roll, pitch, throttle, yaw + aux1, 1000, 1000, 1000, # aux 1, .. + 1000, 1000, 1000, 1000, + 1000, 1000, 1000, 1000 + ) + # print("rc", struct.unpack('@dHHHHHHHHHHHHHHHH', rc_packet)) + self.sock.sendto(rc_packet, (self.UDP_IP, 9004)) + + #### PWMs message from Betaflight ########################## + try: + data, addr = self.sock_pwm.recvfrom(16) # buffer size is 100 bytes (servo_packet size 16) + except socket.error as msg: + _action = self.beta_action + pass + else: + # print("received message: ", data) + _action = np.array(struct.unpack('@ffff', data)).reshape((1,4)) + self.beta_action = _action return obs, reward, terminated, truncated, info From 65fbd1c13e6247a310f1193fb0583b9fb2080cf1 Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Wed, 11 Oct 2023 13:08:22 +0400 Subject: [PATCH 06/15] Multidrone in BetaAviary using different UDP ports --- gym_pybullet_drones/envs/BetaAviary.py | 34 +++++++++++++++----------- gym_pybullet_drones/examples/beta.py | 4 +-- 2 files changed, 22 insertions(+), 16 deletions(-) diff --git a/gym_pybullet_drones/envs/BetaAviary.py b/gym_pybullet_drones/envs/BetaAviary.py index 4a25bf506..c5f36426f 100644 --- a/gym_pybullet_drones/envs/BetaAviary.py +++ b/gym_pybullet_drones/envs/BetaAviary.py @@ -8,6 +8,10 @@ from gym_pybullet_drones.envs.BaseAviary import BaseAviary from gym_pybullet_drones.utils.enums import DroneModel, Physics +BASE_PORT_PWM=9002 # In +BASE_PORT_STATE=9003 # Out +BASE_PORT_RC=9004 # Out + class BetaAviary(BaseAviary): """Multi-drone environment class for use of BetaFlight controller.""" @@ -80,13 +84,15 @@ def __init__(self, self.ARM_TIME = 1 self.TRAJ_TIME = 1.5 - self.sock = socket.socket(socket.AF_INET, # Internet - socket.SOCK_DGRAM) # UDP - - self.sock_pwm = socket.socket(socket.AF_INET, # Internet - socket.SOCK_DGRAM) # UDP - self.sock_pwm.bind((self.UDP_IP, 9002)) - self.sock_pwm.settimeout(0.0) + self.sock = [] + self.sock_pwm = [] + for i in range(self.NUM_DRONES): + self.sock.append(socket.socket(socket.AF_INET, # Internet + socket.SOCK_DGRAM)) # UDP + self.sock_pwm.append(socket.socket(socket.AF_INET, # Internet + socket.SOCK_DGRAM)) # UDP + self.sock_pwm[i].bind((self.UDP_IP, BASE_PORT_PWM + 10 * (i))) # drone0 will be on 9002, drone1 on 9012, etc. + self.sock_pwm[i].settimeout(0.0) self.beta_action = np.zeros((self.NUM_DRONES, 4)) @@ -100,7 +106,7 @@ def step(self, action, i): for j in range(self.NUM_DRONES): #TODO: add multi-drone support #### State message to Betaflight ########################### - o = obs[0] # p, q, euler, v, w, rpm (all in world frame) + o = obs[j,:] # p, q, euler, v, w, rpm (all in world frame) p = o[:3] q = np.array([o[6], o[3], o[4], o[5]]) # w, x, y, z v = o[10:13] @@ -118,7 +124,7 @@ def step(self, action, i): 0, 0, 0, # double position_xyz[3] 1.0 # double pressure; ) - self.sock.sendto(fdm_packet, (self.UDP_IP, 9003)) + self.sock[j].sendto(fdm_packet, (self.UDP_IP, BASE_PORT_STATE + 10 * (j))) # drone0 will be on 9003, drone1 on 9013, etc. # Action is (thro, roll, pitch, yaw) thro = 1000 # Positive up @@ -127,7 +133,7 @@ def step(self, action, i): roll = 1500 # Positive right/forward in y if t > self.TRAJ_TIME: - thro, roll, pitch, yaw = self.ctbr2beta(*action[0]) + thro, roll, pitch, yaw = self.ctbr2beta(*action[j,:]) #### RC message to Betaflight ############################## aux1 = 1000 if t < self.ARM_TIME else 1500 # Arm @@ -140,18 +146,18 @@ def step(self, action, i): 1000, 1000, 1000, 1000 ) # print("rc", struct.unpack('@dHHHHHHHHHHHHHHHH', rc_packet)) - self.sock.sendto(rc_packet, (self.UDP_IP, 9004)) + self.sock[j].sendto(rc_packet, (self.UDP_IP, BASE_PORT_RC + 10 * (j))) # drone0 will be on 9004, drone1 on 9014, etc. #### PWMs message from Betaflight ########################## try: - data, addr = self.sock_pwm.recvfrom(16) # buffer size is 100 bytes (servo_packet size 16) + data, addr = self.sock_pwm[j].recvfrom(16) # buffer size is 100 bytes (servo_packet size 16) except socket.error as msg: - _action = self.beta_action + _action = self.beta_action[j,:] pass else: # print("received message: ", data) _action = np.array(struct.unpack('@ffff', data)).reshape((1,4)) - self.beta_action = _action + self.beta_action[j,:] = _action return obs, reward, terminated, truncated, info diff --git a/gym_pybullet_drones/examples/beta.py b/gym_pybullet_drones/examples/beta.py index 70513e8dc..3e2f60f0f 100644 --- a/gym_pybullet_drones/examples/beta.py +++ b/gym_pybullet_drones/examples/beta.py @@ -54,8 +54,8 @@ DEFAULT_CONTROL_FREQ_HZ = 500 DEFAULT_DURATION_SEC = 20 DEFAULT_OUTPUT_FOLDER = 'results' -NUM_DRONES = 1 -INIT_XYZ = np.array([[.5*i, .5*i, .1] for i in range(NUM_DRONES)]) +NUM_DRONES = 2 +INIT_XYZ = np.array([[.3*i, .3*i, .1] for i in range(NUM_DRONES)]) INIT_RPY = np.array([[.0, .0, .0] for _ in range(NUM_DRONES)]) def run( From 0542623baa999692eca0d911bfbe19349aba08a2 Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Wed, 11 Oct 2023 15:06:06 +0400 Subject: [PATCH 07/15] Reinstated bf configurator file --- gym_pybullet_drones/assets/beta.txt | 82 ++++++++++++++++++++++++++++ gym_pybullet_drones/examples/beta.py | 2 +- 2 files changed, 83 insertions(+), 1 deletion(-) create mode 100644 gym_pybullet_drones/assets/beta.txt diff --git a/gym_pybullet_drones/assets/beta.txt b/gym_pybullet_drones/assets/beta.txt new file mode 100644 index 000000000..bf44da53f --- /dev/null +++ b/gym_pybullet_drones/assets/beta.txt @@ -0,0 +1,82 @@ +defaults nosave + + +# version +# Betaflight / SITL (SITL) 4.5.0 Jul 7 2023 / 10:53:28 (b54ae921f) MSP API: 1.46 + +# start the command batch +batch start + +# reset configuration to default settings +defaults nosave + +mcu_id 000000000000000100000002 +signature + +# feature +feature -GPS +feature -TELEMETRY + +# aux +aux 0 0 0 1300 1700 0 0 + +# master +set motor_pwm_protocol = PWM +set pid_process_denom = 16 + +profile 0 + +# profile 0 +set pidsum_limit = 1000 +set pidsum_limit_yaw = 1000 +set p_pitch = 70 +set i_pitch = 75 +set d_pitch = 43 +set f_pitch = 224 +set p_roll = 58 +set i_roll = 62 +set d_roll = 42 +set f_roll = 187 +set p_yaw = 58 +set i_yaw = 62 +set f_yaw = 187 +set d_min_roll = 42 +set d_min_pitch = 43 +set simplified_master_multiplier = 130 +set simplified_i_gain = 60 +set simplified_d_gain = 110 +set simplified_dmax_gain = 0 +set simplified_feedforward_gain = 120 +set simplified_pitch_d_gain = 90 +set simplified_pitch_pi_gain = 115 + +profile 1 + +profile 2 + +profile 3 + +# restore original profile selection +profile 0 + +rateprofile 0 + +# rateprofile 0 +set rates_type = BETAFLIGHT +set roll_rc_rate = 180 +set pitch_rc_rate = 180 +set yaw_rc_rate = 180 +set roll_srate = 0 +set pitch_srate = 0 +set yaw_srate = 0 + +rateprofile 1 + +rateprofile 2 + +rateprofile 3 + +# restore original rateprofile selection +rateprofile 0 + +# save configuration \ No newline at end of file diff --git a/gym_pybullet_drones/examples/beta.py b/gym_pybullet_drones/examples/beta.py index 3e2f60f0f..52afa7ab2 100644 --- a/gym_pybullet_drones/examples/beta.py +++ b/gym_pybullet_drones/examples/beta.py @@ -55,7 +55,7 @@ DEFAULT_DURATION_SEC = 20 DEFAULT_OUTPUT_FOLDER = 'results' NUM_DRONES = 2 -INIT_XYZ = np.array([[.3*i, .3*i, .1] for i in range(NUM_DRONES)]) +INIT_XYZ = np.array([[.3*i, .3*i, .1] for i in range(1,NUM_DRONES+1)]) INIT_RPY = np.array([[.0, .0, .0] for _ in range(NUM_DRONES)]) def run( From c7ee6b69306b9a88fb30561a2bd45bacf2bd75ce Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Wed, 11 Oct 2023 15:11:18 +0400 Subject: [PATCH 08/15] Added wip script to clone, compile, configure, run betaflight SITL --- gym_pybullet_drones/assets/clone_bfs.sh | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100755 gym_pybullet_drones/assets/clone_bfs.sh diff --git a/gym_pybullet_drones/assets/clone_bfs.sh b/gym_pybullet_drones/assets/clone_bfs.sh new file mode 100755 index 000000000..0f003ec2b --- /dev/null +++ b/gym_pybullet_drones/assets/clone_bfs.sh @@ -0,0 +1,6 @@ +#!/bin/bash + +# USE +# 1. .. + +echo 'TBD' \ No newline at end of file From 92a9c3c8d00edb24dbb22685bc36d80385d45e57 Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Wed, 11 Oct 2023 15:14:56 +0400 Subject: [PATCH 09/15] Added wip script to clone, compile, configure, run betaflight SITL --- gym_pybullet_drones/assets/clone_bfs.sh | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/gym_pybullet_drones/assets/clone_bfs.sh b/gym_pybullet_drones/assets/clone_bfs.sh index 0f003ec2b..a9473a4b1 100755 --- a/gym_pybullet_drones/assets/clone_bfs.sh +++ b/gym_pybullet_drones/assets/clone_bfs.sh @@ -3,4 +3,13 @@ # USE # 1. .. -echo 'TBD' \ No newline at end of file +echo 'TBD' + +git clone https://github.com/betaflight/betaflight +cd betaflight/ +# comment out line `delayMicroseconds_real(50); // max rate 20kHz` in ./src/main/main.c +# edit ports in ./src/main/target/SITL/sitl.c, ./src/main/drivers/serial_tcp.c +make arm_sdk_install +make TARGET=SITL +cp ~/gym-pybullet-drones/gym_pybullet_drones/assets/eeprom.bin ~/betaflight/ +./obj/main/betaflight_SITL.elf \ No newline at end of file From 66210aba071e37d98245f2007e47ed6a93a72431 Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Wed, 11 Oct 2023 16:28:18 +0400 Subject: [PATCH 10/15] Update clone_bfs.sh --- gym_pybullet_drones/assets/clone_bfs.sh | 59 ++++++++++++++++++++----- 1 file changed, 49 insertions(+), 10 deletions(-) diff --git a/gym_pybullet_drones/assets/clone_bfs.sh b/gym_pybullet_drones/assets/clone_bfs.sh index a9473a4b1..829b8346a 100755 --- a/gym_pybullet_drones/assets/clone_bfs.sh +++ b/gym_pybullet_drones/assets/clone_bfs.sh @@ -3,13 +3,52 @@ # USE # 1. .. -echo 'TBD' - -git clone https://github.com/betaflight/betaflight -cd betaflight/ -# comment out line `delayMicroseconds_real(50); // max rate 20kHz` in ./src/main/main.c -# edit ports in ./src/main/target/SITL/sitl.c, ./src/main/drivers/serial_tcp.c -make arm_sdk_install -make TARGET=SITL -cp ~/gym-pybullet-drones/gym_pybullet_drones/assets/eeprom.bin ~/betaflight/ -./obj/main/betaflight_SITL.elf \ No newline at end of file +# Check for the correct number of command-line arguments +if [ "$#" -ne 2 ]; then + echo "Usage: $0 " + exit 1 +fi + +# Extract command-line arguments +gpd_base_path="$1" +num_iterations="$2" + +# Create directory along gym-pybullet-donres +cd $gpd_base_path +mkdir betaflights/ +cd betaflights/ + +for ((i = 1; i <= num_iterations; i++)); do + + # Clone + git clone https://github.com/betaflight/betaflight "bf${i}" + cd "bf${i}/" + + # Edit + # comment out line `delayMicroseconds_real(50); // max rate 20kHz` in ./src/main/main.c + # edit ports in ./src/main/target/SITL/sitl.c, ./src/main/drivers/serial_tcp.c + + # Build + make arm_sdk_install + make TARGET=SITL + + # Copy configured memory + cp "${gpd_base_path}/gym_pybullet_drones/assets/eeprom.bin" . + +done + +# # Define the file you want to modify +# file="your_source_code.c" + +# # Define the base pattern to search for +# pattern1="#define PORT_PWM_RAW 9001 // Out" +# pattern2="#define PORT_PWM 9002 // Out" +# pattern3="#define PORT_STATE 9003 // In" +# pattern4="#define PORT_RC 9004 // In" + +# # Use a for loop to perform replacements +# for ((i = 1; i <= 10; i++)); do +# current_pattern="${base_pattern}${i}" +# replacement="new_pattern_${i}" +# sed -i "s/$current_pattern/$replacement/g" "$file" +# done From a8f3c2c1bf2ce810d4e867ac570196a3c4ba1dcc Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Wed, 11 Oct 2023 16:44:56 +0400 Subject: [PATCH 11/15] Updated automation script --- gym_pybullet_drones/assets/clone_bfs.sh | 47 ++++++++++++++----------- 1 file changed, 27 insertions(+), 20 deletions(-) diff --git a/gym_pybullet_drones/assets/clone_bfs.sh b/gym_pybullet_drones/assets/clone_bfs.sh index 829b8346a..7e14cc4a5 100755 --- a/gym_pybullet_drones/assets/clone_bfs.sh +++ b/gym_pybullet_drones/assets/clone_bfs.sh @@ -1,7 +1,6 @@ #!/bin/bash -# USE -# 1. .. +# Clone, edit, build, configure, multiple Betaflight SITL executables # Check for the correct number of command-line arguments if [ "$#" -ne 2 ]; then @@ -15,9 +14,19 @@ num_iterations="$2" # Create directory along gym-pybullet-donres cd $gpd_base_path +cd .. mkdir betaflights/ cd betaflights/ +pattern0="delayMicroseconds_real(50); // max rate 20kHz" +pattern1="#define PORT_PWM_RAW 9001 // Out" +pattern2="#define PORT_PWM 9002 // Out" +pattern3="#define PORT_STATE 9003 // In" +pattern4="#define PORT_RC 9004 // In" +pattern5="#define BASE_PORT 5760" + +replacement0="// delayMicroseconds_real(50); // max rate 20kHz" + for ((i = 1; i <= num_iterations; i++)); do # Clone @@ -25,8 +34,22 @@ for ((i = 1; i <= num_iterations; i++)); do cd "bf${i}/" # Edit - # comment out line `delayMicroseconds_real(50); // max rate 20kHz` in ./src/main/main.c - # edit ports in ./src/main/target/SITL/sitl.c, ./src/main/drivers/serial_tcp.c + sed -i "s/$pattern0/$replacement0/g" ./src/main/target/SITL/sitl.c + + replacement1="#define PORT_PWM_RAW 90${i}1 // Out" + sed -i "s/$pattern1/$replacement1/g" ./src/main/target/SITL/sitl.c + + replacement2="#define PORT_PWM 90${i}2 // Out" + sed -i "s/$pattern2/$replacement2/g" ./src/main/target/SITL/sitl.c + + replacement3="#define PORT_STATE 90${i}3 // In" + sed -i "s/$pattern3/$replacement3/g" ./src/main/target/SITL/sitl.c + + replacement4="#define PORT_PWPORT_RCM_RAW 90${i}4 // In" + sed -i "s/$pattern4/$replacement4/g" ./src/main/target/SITL/sitl.c + + replacement5="#define BASE_PORT 57${i}0" + sed -i "s/$pattern5/$replacement5/g" ./src/main/drivers/serial_tcp.c # Build make arm_sdk_install @@ -36,19 +59,3 @@ for ((i = 1; i <= num_iterations; i++)); do cp "${gpd_base_path}/gym_pybullet_drones/assets/eeprom.bin" . done - -# # Define the file you want to modify -# file="your_source_code.c" - -# # Define the base pattern to search for -# pattern1="#define PORT_PWM_RAW 9001 // Out" -# pattern2="#define PORT_PWM 9002 // Out" -# pattern3="#define PORT_STATE 9003 // In" -# pattern4="#define PORT_RC 9004 // In" - -# # Use a for loop to perform replacements -# for ((i = 1; i <= 10; i++)); do -# current_pattern="${base_pattern}${i}" -# replacement="new_pattern_${i}" -# sed -i "s/$current_pattern/$replacement/g" "$file" -# done From 0da365f9f9d760abea851e03023fa7d67a8e9667 Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Thu, 12 Oct 2023 21:10:07 +0400 Subject: [PATCH 12/15] Fixed betaflight sitl state port bug --- gym_pybullet_drones/assets/clone_bfs.sh | 4 ++++ gym_pybullet_drones/examples/beta.py | 20 ++++++++++++++++---- 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/gym_pybullet_drones/assets/clone_bfs.sh b/gym_pybullet_drones/assets/clone_bfs.sh index 7e14cc4a5..44e026ed5 100755 --- a/gym_pybullet_drones/assets/clone_bfs.sh +++ b/gym_pybullet_drones/assets/clone_bfs.sh @@ -24,8 +24,10 @@ pattern2="#define PORT_PWM 9002 // Out" pattern3="#define PORT_STATE 9003 // In" pattern4="#define PORT_RC 9004 // In" pattern5="#define BASE_PORT 5760" +pattern6="ret = udpInit(&stateLink, NULL, 9003, true);" replacement0="// delayMicroseconds_real(50); // max rate 20kHz" +replacement6="ret = udpInit(&stateLink, NULL, PORT_STATE, true);" for ((i = 1; i <= num_iterations; i++)); do @@ -51,6 +53,8 @@ for ((i = 1; i <= num_iterations; i++)); do replacement5="#define BASE_PORT 57${i}0" sed -i "s/$pattern5/$replacement5/g" ./src/main/drivers/serial_tcp.c + sed -i "s/$pattern6/$replacement6/g" ./src/main/target/SITL/sitl.c + # Build make arm_sdk_install make TARGET=SITL diff --git a/gym_pybullet_drones/examples/beta.py b/gym_pybullet_drones/examples/beta.py index 52afa7ab2..7500371e8 100644 --- a/gym_pybullet_drones/examples/beta.py +++ b/gym_pybullet_drones/examples/beta.py @@ -95,7 +95,7 @@ def run( #### Run the simulation #################################### with open("../assets/beta.csv", mode='r') as csv_file: csv_reader = csv.DictReader(csv_file) - trajectory = iter([{ + trajectory1 = iter([{ "pos": np.array([ float(row["p_x"]), float(row["p_y"]), @@ -107,6 +107,20 @@ def run( float(row["v_z"]), ]), } for row in csv_reader]) + with open("../assets/beta.csv", mode='r') as csv_file: + csv_reader = csv.DictReader(csv_file) + trajectory2 = iter(reversed([{ + "pos": np.array([ + float(row["p_x"]), + float(row["p_y"]), + float(row["p_z"]), + ]), + "vel": np.array([ + float(row["v_x"]), + float(row["v_y"]), + float(row["v_z"]), + ]), + } for row in csv_reader])) action = np.zeros((NUM_DRONES,4)) ARM_TIME = 1. TRAJ_TIME = 1.5 @@ -119,9 +133,7 @@ def run( if t > env.TRAJ_TIME: for j in range(NUM_DRONES): try: - target = next(trajectory) - print(target['pos']) - print(target['vel']) + target = next(trajectory1) if j%2 == 0 else next(trajectory2) action[j,:] = ctrl.computeControlFromState(control_timestep=env.CTRL_TIMESTEP, state=obs[j], target_pos=target["pos"]+[INIT_XYZ[j][0], INIT_XYZ[j][1], 0], From adaebb12b01625a9d6972ca25e7be34d1f5e1fd6 Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Thu, 12 Oct 2023 21:12:58 +0400 Subject: [PATCH 13/15] Removed unnecessary uart port change --- gym_pybullet_drones/assets/clone_bfs.sh | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gym_pybullet_drones/assets/clone_bfs.sh b/gym_pybullet_drones/assets/clone_bfs.sh index 44e026ed5..2e393e794 100755 --- a/gym_pybullet_drones/assets/clone_bfs.sh +++ b/gym_pybullet_drones/assets/clone_bfs.sh @@ -23,7 +23,7 @@ pattern1="#define PORT_PWM_RAW 9001 // Out" pattern2="#define PORT_PWM 9002 // Out" pattern3="#define PORT_STATE 9003 // In" pattern4="#define PORT_RC 9004 // In" -pattern5="#define BASE_PORT 5760" +# pattern5="#define BASE_PORT 5760" pattern6="ret = udpInit(&stateLink, NULL, 9003, true);" replacement0="// delayMicroseconds_real(50); // max rate 20kHz" @@ -50,8 +50,8 @@ for ((i = 1; i <= num_iterations; i++)); do replacement4="#define PORT_PWPORT_RCM_RAW 90${i}4 // In" sed -i "s/$pattern4/$replacement4/g" ./src/main/target/SITL/sitl.c - replacement5="#define BASE_PORT 57${i}0" - sed -i "s/$pattern5/$replacement5/g" ./src/main/drivers/serial_tcp.c + # replacement5="#define BASE_PORT 57${i}0" + # sed -i "s/$pattern5/$replacement5/g" ./src/main/drivers/serial_tcp.c sed -i "s/$pattern6/$replacement6/g" ./src/main/target/SITL/sitl.c From fc4a87efb7a6d8d5d4f45698e932e8745871202b Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Fri, 13 Oct 2023 12:24:18 +0400 Subject: [PATCH 14/15] Automated multi-bf SITL creation --- .gitignore | 3 + README.md | 2 +- gym_pybullet_drones/assets/clone_bfs.sh | 87 ++++++++++++++----------- gym_pybullet_drones/examples/beta.py | 46 ++++++------- 4 files changed, 73 insertions(+), 65 deletions(-) diff --git a/.gitignore b/.gitignore index d2316598d..98af37ec3 100644 --- a/.gitignore +++ b/.gitignore @@ -12,6 +12,9 @@ tmp/ # Learning results experiments/learning/results/save-* +# Betaflight +betaflight_sitl/ + # macOS users .DS_Store diff --git a/README.md b/README.md index 2c512fbc8..c3829e2a9 100644 --- a/README.md +++ b/README.md @@ -54,7 +54,7 @@ In another terminal, run the example ```sh conda activate drones cd gym_pybullet_drones/examples/ -python3 beta.py # also check the steps in the file's docstrings +python3 beta.py --num_drones 1 # also check the steps in the file's docstrings to use multiple drones ``` ## Troubleshooting diff --git a/gym_pybullet_drones/assets/clone_bfs.sh b/gym_pybullet_drones/assets/clone_bfs.sh index 2e393e794..454c80c5c 100755 --- a/gym_pybullet_drones/assets/clone_bfs.sh +++ b/gym_pybullet_drones/assets/clone_bfs.sh @@ -3,63 +3,72 @@ # Clone, edit, build, configure, multiple Betaflight SITL executables # Check for the correct number of command-line arguments -if [ "$#" -ne 2 ]; then - echo "Usage: $0 " +if [ "$#" -ne 1 ]; then + echo "Usage: $0 " exit 1 fi # Extract command-line arguments -gpd_base_path="$1" -num_iterations="$2" +desired_max_num_drones="$1" -# Create directory along gym-pybullet-donres -cd $gpd_base_path -cd .. -mkdir betaflights/ -cd betaflights/ +# Create gitignored directory in gym-pybullet-donres +SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd ) +cd $SCRIPT_DIR +cd ../../ +mkdir betaflight_sitl/ +cd betaflight_sitl/ -pattern0="delayMicroseconds_real(50); // max rate 20kHz" -pattern1="#define PORT_PWM_RAW 9001 // Out" -pattern2="#define PORT_PWM 9002 // Out" -pattern3="#define PORT_STATE 9003 // In" -pattern4="#define PORT_RC 9004 // In" -# pattern5="#define BASE_PORT 5760" -pattern6="ret = udpInit(&stateLink, NULL, 9003, true);" +# Step 1: Clone and open betaflight's source: +git clone https://github.com/betaflight/betaflight temp/ -replacement0="// delayMicroseconds_real(50); // max rate 20kHz" -replacement6="ret = udpInit(&stateLink, NULL, PORT_STATE, true);" -for ((i = 1; i <= num_iterations; i++)); do +# Step 2: Comment out line `delayMicroseconds_real(50); // max rate 20kHz` +# (https://github.com/betaflight/betaflight/blob/master/src/main/main.c#L52) +# from Betaflight's `SIMULATOR_BUILD` +cd temp/ +sed -i "s/delayMicroseconds_real(50);/\/\/delayMicroseconds_real(50);/g" ./src/main/main.c +sed -i "s/ret = udpInit(\&stateLink, NULL, 9003, true);/\/\/ret = udpInit(\&stateLink, NULL, PORT_STATE, true);/g" ./src/main/target/SITL/sitl.c +sed -i "s/printf(\"start UDP server.../\/\/printf(\"start UDP server.../g" ./src/main/target/SITL/sitl.c - # Clone - git clone https://github.com/betaflight/betaflight "bf${i}" - cd "bf${i}/" +# Prepare +make arm_sdk_install - # Edit - sed -i "s/$pattern0/$replacement0/g" ./src/main/target/SITL/sitl.c +cd .. - replacement1="#define PORT_PWM_RAW 90${i}1 // Out" - sed -i "s/$pattern1/$replacement1/g" ./src/main/target/SITL/sitl.c +pattern1="PORT_PWM_RAW 9001" +pattern2="PORT_PWM 9002" +pattern3="PORT_STATE 9003" +pattern4="PORT_RC 9004" +# pattern5="BASE_PORT 5760" - replacement2="#define PORT_PWM 90${i}2 // Out" - sed -i "s/$pattern2/$replacement2/g" ./src/main/target/SITL/sitl.c +for ((i = 0; i < desired_max_num_drones; i++)); do - replacement3="#define PORT_STATE 90${i}3 // In" - sed -i "s/$pattern3/$replacement3/g" ./src/main/target/SITL/sitl.c + # Copy + cp -r temp/ "bf${i}/" + cd "bf${i}/" - replacement4="#define PORT_PWPORT_RCM_RAW 90${i}4 // In" + # Step 3: Change the UDP ports used by each Betaflight SITL instancet + replacement1="PORT_PWM_RAW 90${i}1" + sed -i "s/$pattern1/$replacement1/g" ./src/main/target/SITL/sitl.c + replacement2="PORT_PWM 90${i}2" + sed -i "s/$pattern2/$replacement2/g" ./src/main/target/SITL/sitl.c + replacement3="PORT_STATE 90${i}3" + sed -i "s/$pattern3/$replacement3/g" ./src/main/target/SITL/sitl.c + replacement4="PORT_RC 90${i}4" sed -i "s/$pattern4/$replacement4/g" ./src/main/target/SITL/sitl.c - - # replacement5="#define BASE_PORT 57${i}0" + # replacement5="BASE_PORT 57${i}0" # sed -i "s/$pattern5/$replacement5/g" ./src/main/drivers/serial_tcp.c - sed -i "s/$pattern6/$replacement6/g" ./src/main/target/SITL/sitl.c - # Build - make arm_sdk_install - make TARGET=SITL + make TARGET=SITL + + cd .. + +done + +for ((i = 0; i < desired_max_num_drones; i++)); do - # Copy configured memory - cp "${gpd_base_path}/gym_pybullet_drones/assets/eeprom.bin" . + # Step 4: Copy over the configured `eeprom.bin` file from folder + cp "${SCRIPT_DIR}/eeprom.bin" "bf${i}/" done diff --git a/gym_pybullet_drones/examples/beta.py b/gym_pybullet_drones/examples/beta.py index 7500371e8..8d822262d 100644 --- a/gym_pybullet_drones/examples/beta.py +++ b/gym_pybullet_drones/examples/beta.py @@ -2,29 +2,23 @@ Setup ----- -Step 1: Clone and open betaflight's source: - $ git clone https://github.com/betaflight/betaflight - $ cd betaflight/ - $ code ./src/main/main.c - -Step 2: Comment out line `delayMicroseconds_real(50); // max rate 20kHz` - (https://github.com/betaflight/betaflight/blob/master/src/main/main.c#L52) - from Betaflight's `SIMULATOR_BUILD` and compile: - $ cd betaflight/ - $ make arm_sdk_install - $ make TARGET=SITL - -Step 3: Copy over the configured `eeprom.bin` file from folder - `gym-pybullet-drones/gym_pybullet_drones/assets/` to BF's main folder `betaflight/` - $ cp ~/gym-pybullet-drones/gym_pybullet_drones/assets/eeprom.bin ~/betaflight/ +Use script `gym_pybullet_drones/assets/clone_bfs.sh` to create +executables for as many drones as needed (e.g. 2): + $ ./gym_pybullet_drones/assets/clone_bfs.sh 2 Example ------- -In one terminal run the SITL Betaflight: +Run as many SITL Betaflight as drones in the simulation +in separate terminals (navigate the each `bf0`, `bf1`, etc. folder first): - $ cd betaflight/ + $ cd betaflights/bf0/ $ ./obj/main/betaflight_SITL.elf + $ cd betaflights/bf1/ + $ ./obj/main/betaflight_SITL.elf + + $ .. + In a separate terminal, run: $ cd gym-pybullet-drones/gym_pybullet_drones/examples/ @@ -54,12 +48,11 @@ DEFAULT_CONTROL_FREQ_HZ = 500 DEFAULT_DURATION_SEC = 20 DEFAULT_OUTPUT_FOLDER = 'results' -NUM_DRONES = 2 -INIT_XYZ = np.array([[.3*i, .3*i, .1] for i in range(1,NUM_DRONES+1)]) -INIT_RPY = np.array([[.0, .0, .0] for _ in range(NUM_DRONES)]) +DEFAULT_NUM_DRONES = 2 def run( drone=DEFAULT_DRONES, + num_drones=DEFAULT_NUM_DRONES, physics=DEFAULT_PHYSICS, gui=DEFAULT_GUI, plot=DEFAULT_PLOT, @@ -70,8 +63,10 @@ def run( output_folder=DEFAULT_OUTPUT_FOLDER, ): #### Create the environment with or without video capture ## + INIT_XYZ = np.array([[.3*i, .3*i, .1] for i in range(1,num_drones+1)]) + INIT_RPY = np.array([[.0, .0, .0] for _ in range(num_drones)]) env = BetaAviary(drone_model=drone, - num_drones=NUM_DRONES, + num_drones=num_drones, initial_xyzs=INIT_XYZ, initial_rpys=INIT_RPY, physics=physics, @@ -88,7 +83,7 @@ def run( #### Initialize the logger ################################# logger = Logger(logging_freq_hz=control_freq_hz, - num_drones=NUM_DRONES, + num_drones=num_drones, output_folder=output_folder, ) @@ -121,7 +116,7 @@ def run( float(row["v_z"]), ]), } for row in csv_reader])) - action = np.zeros((NUM_DRONES,4)) + action = np.zeros((num_drones,4)) ARM_TIME = 1. TRAJ_TIME = 1.5 START = time.time() @@ -131,7 +126,7 @@ def run( obs, reward, terminated, truncated, info = env.step(action, i) if t > env.TRAJ_TIME: - for j in range(NUM_DRONES): + for j in range(num_drones): try: target = next(trajectory1) if j%2 == 0 else next(trajectory2) action[j,:] = ctrl.computeControlFromState(control_timestep=env.CTRL_TIMESTEP, @@ -144,7 +139,7 @@ def run( #### Log the simulation #################################### - for j in range(NUM_DRONES): + for j in range(num_drones): logger.log(drone=j, timestamp=i/env.CTRL_FREQ, state=obs[j] @@ -173,6 +168,7 @@ def run( #### Define and parse (optional) arguments for the script ## parser = argparse.ArgumentParser(description='Test flight script using SITL Betaflight') parser.add_argument('--drone', default=DEFAULT_DRONES, type=DroneModel, help='Drone model (default: BETA)', 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('--plot', default=DEFAULT_PLOT, type=str2bool, help='Whether to plot the simulation results (default: True)', metavar='') From 2291bac0620d2a1932f471586c1e89ce89e4cbe0 Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Fri, 13 Oct 2023 13:20:50 +0400 Subject: [PATCH 15/15] Fully automated SITL betaflight --- gym_pybullet_drones/envs/BetaAviary.py | 10 ++++++++++ gym_pybullet_drones/examples/beta.py | 8 ++++---- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/gym_pybullet_drones/envs/BetaAviary.py b/gym_pybullet_drones/envs/BetaAviary.py index c5f36426f..6cf6caee6 100644 --- a/gym_pybullet_drones/envs/BetaAviary.py +++ b/gym_pybullet_drones/envs/BetaAviary.py @@ -2,6 +2,9 @@ from gymnasium import spaces import socket import struct +import os +import subprocess +import time from transforms3d.quaternions import rotate_vector, qconjugate @@ -79,6 +82,13 @@ def __init__(self, output_folder=output_folder ) + # Spawn SITL Betaflight instances (must have been created with assets/clone_bfs/sh first) + for i in range(num_drones): + FOLDER = os.path.dirname(os.path.abspath(__file__))+'/../../betaflight_sitl/bf'+str(i)+'/' + cmd = f"gnome-terminal -- bash -c 'cd {FOLDER} && ./obj/main/betaflight_SITL.elf; exec bash'" + subprocess.Popen(cmd, shell=True) + time.sleep(2) + # Initialize connection to BetaFlight Controller self.UDP_IP = udp_ip self.ARM_TIME = 1 diff --git a/gym_pybullet_drones/examples/beta.py b/gym_pybullet_drones/examples/beta.py index 8d822262d..f7da8dd5e 100644 --- a/gym_pybullet_drones/examples/beta.py +++ b/gym_pybullet_drones/examples/beta.py @@ -6,10 +6,10 @@ executables for as many drones as needed (e.g. 2): $ ./gym_pybullet_drones/assets/clone_bfs.sh 2 -Example +Note ------- -Run as many SITL Betaflight as drones in the simulation -in separate terminals (navigate the each `bf0`, `bf1`, etc. folder first): +This example will automatically start as many SITL Betaflight as drones +in the simulation in separate terminals: $ cd betaflights/bf0/ $ ./obj/main/betaflight_SITL.elf @@ -19,7 +19,7 @@ $ .. -In a separate terminal, run: +Run as: $ cd gym-pybullet-drones/gym_pybullet_drones/examples/ $ python beta.py