diff --git a/README.md b/README.md index 69e81aa07..875cae53c 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,14 @@ +# Additions by Konstantin Kondratenko +### Implemented standard drone control methods. +1) through indications of the speed vector: movement along the axes OX, OY, OZ; formation of the direction of movement through the angle between the axes; linear combinations of vectors -- in velocity_commands.py file. +`Files-examples`: in the important_examples folder -- square_vel.py , square_withZ_vel.py, triple_offset.py +2) through setting the trajectory in the Cartesian coordinate system: [circle, cylinder, cone, ellipse, spiral] -- for one drone with CLI big_demo.py. Specifying position in spherical and cylindrical coordinate systems. Setting the position by incrementing values ​​along the axes relative to the previous step. +`Files-examples`: in the important_examples folder -- all files except [square_vel.py , square_withZ_vel.py, triple_offset.py] + +### Sequential union of trajectories +Combining trajectories by updated logic and signature. `Files-examples`: in the important_examples folder -- second_good_union.py, smart_good_union.py + + # gym-pybullet-drones [Simple](https://en.wikipedia.org/wiki/KISS_principle) OpenAI [Gym environment](https://gym.openai.com/envs/#classic_control) based on [PyBullet](https://github.com/bulletphysics/bullet3) for multi-agent reinforcement learning with quadrotors diff --git a/gym_pybullet_drones/important_examples/big_demo.py b/gym_pybullet_drones/important_examples/big_demo.py new file mode 100644 index 000000000..77bda3379 --- /dev/null +++ b/gym_pybullet_drones/important_examples/big_demo.py @@ -0,0 +1,252 @@ +import os +import time +import argparse +from datetime import datetime +import pdb +import math +import random +import numpy as np +import pybullet as p +import matplotlib.pyplot as plt + +from gym_pybullet_drones.utils.enums import DroneModel, Physics +from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary +from gym_pybullet_drones.envs.VisionAviary import VisionAviary +from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl +from gym_pybullet_drones.control.SimplePIDControl import SimplePIDControl +from gym_pybullet_drones.utils.Logger import Logger +from gym_pybullet_drones.utils.utils import sync, str2bool + +import gym_pybullet_drones.utils.position_commads as pc + +DEFAULT_GUI = True +DEFAULT_RECORD_VIDEO = False +DEFAULT_PLOT = True +DEFAULT_USER_DEBUG_GUI = False +DEFAULT_AGGREGATE = True +DEFAULT_OBSTACLES = True +DEFAULT_SIMULATION_FREQ_HZ = 240 +DEFAULT_CONTROL_FREQ_HZ = 240 +DEFAULT_DURATION_SEC = 4 +DEFAULT_OUTPUT_FOLDER = 'results' +DEFAULT_COLAB = False + +def run( + gui=DEFAULT_GUI, + record_video=DEFAULT_RECORD_VIDEO, + plot=DEFAULT_PLOT, + user_debug_gui=DEFAULT_USER_DEBUG_GUI, + aggregate=DEFAULT_AGGREGATE, + 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 ###################### + print("If You want cyrcle -- 1") + print("If You want cylindre -- 2") + print("If You want cone -- 3") + print("If You want spiral -- 4") + print("If You want ellipse -- 5") + print("If You want line -- 6") + command = int(input(":: ")) + if 5> command < 0: + print("Error of input value") + raise + PERIOD = 4 + NUM_WP = control_freq_hz*PERIOD + Radius = 0.3 + if command == 1: + print("If You want by centre -- 1") + print("If You want by init state -- 2") + sub_command = int(input(":: ")) + if sub_command == 1: + x = float(input("X : ")) + y = float(input("Y : ")) + z = float(input("Z : ")) + Radius = float(input("Radius : ")) + INIT_XYZ, TARGET_POS = pc.circle_frome_centre(Radius, NUM_WP, x, y, z) + if sub_command == 2: + x = float(input("X : ")) + y = float(input("Y : ")) + z = float(input("Z : ")) + Radius = float(input("Radius : ")) + INIT_XYZ, TARGET_POS = pc.circle(Radius, NUM_WP, x, y, z) + + if command == 2: + print("If You want by centre -- 1") + print("If You want by init state -- 2") + sub_command = int(input(":: ")) + if sub_command == 1: + x = float(input("X : ")) + y = float(input("Y : ")) + z = float(input("Z : ")) + Radius = float(input("Radius : ")) + height = float(input("height : ")) + INIT_XYZ, TARGET_POS = pc.cylinder_frome_centre(Radius, NUM_WP, height, x, y, z) + if sub_command == 2: + x = float(input("X : ")) + y = float(input("Y : ")) + z = float(input("Z : ")) + Radius = float(input("Radius : ")) + height = float(input("height : ")) + INIT_XYZ, TARGET_POS = pc.cylinder(Radius, NUM_WP, height, x, y, z) + + if command == 3: + print("If You want by centre -- 1") + print("If You want by init state -- 2") + sub_command = int(input(":: ")) + if sub_command == 1: + x = float(input("X : ")) + y = float(input("Y : ")) + z = float(input("Z : ")) + Radius = float(input("Radius : ")) + height = float(input("height : ")) + INIT_XYZ, TARGET_POS = INIT_XYZ, TARGET_POS = pc.cone_frome_centre(Radius, NUM_WP, height, x, y, z) + if sub_command == 2: + x = float(input("X : ")) + y = float(input("Y : ")) + z = float(input("Z : ")) + Radius = float(input("Radius : ")) + height = float(input("height : ")) + INIT_XYZ, TARGET_POS = pc.cone(Radius, NUM_WP, height, x, y, z) + + if command == 4: + print("If You want by centre -- 1") + print("If You want by init state -- 2") + sub_command = int(input(":: ")) + if sub_command == 1: + x = float(input("X : ")) + y = float(input("Y : ")) + z = float(input("Z : ")) + Radius = float(input("Radius : ")) + INIT_XYZ, TARGET_POS = pc.spiral_frome_centre(Radius, NUM_WP, x, y, z) + if sub_command == 2: + x = float(input("X : ")) + y = float(input("Y : ")) + z = float(input("Z : ")) + Radius = float(input("Radius : ")) + INIT_XYZ, TARGET_POS = pc.spiral(Radius, NUM_WP, x, y, z) + + if command == 5: + x = float(input("X : ")) + y = float(input("Y : ")) + z = float(input("Z : ")) + Radius_X = float(input("Radius_X : ")) + Radius_Y = float(input("Radius_Y : ")) + INIT_XYZ, TARGET_POS = pc.ellipse(Radius_X, Radius_Y, NUM_WP, x,y,z) + + if command == 6: + x0 = float(input("X0 : ")) + y0 = float(input("Y0 : ")) + z0 = float(input("Z0 : ")) + x1 = float(input("X1 : ")) + y1 = float(input("Y1 : ")) + z1 = float(input("Z1 : ")) + if z0 <= 0: + z0 = 0.1 + if z1 <= 0: + z1 = 0.1 + INIT_XYZ, TARGET_POS = pc.between_points(NUM_WP, x0, y0, z0, x1, y1, z1) + + wp_counter = 0 + AGGR_PHY_STEPS = int(simulation_freq_hz/control_freq_hz) if aggregate else 1 + + #### Create the environment ################################ + env = CtrlAviary(drone_model=DroneModel.CF2X, + num_drones=1, + initial_xyzs=INIT_XYZ, + physics=Physics.PYB_GND, + # physics=Physics.PYB, # For comparison + neighbourhood_radius=10, + freq=simulation_freq_hz, + aggregate_phy_steps=AGGR_PHY_STEPS, + 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=int(simulation_freq_hz/AGGR_PHY_STEPS), + num_drones=1, + output_folder=output_folder, + colab=colab + ) + + #### Initialize the controller ############################# + ctrl = DSLPIDControl(drone_model=DroneModel.CF2X) + + #### Run the simulation #################################### + CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ/control_freq_hz)) + action = {"0": np.array([0,0,0,0])} + START = time.time() + for i in range(0, int(duration_sec*env.SIM_FREQ), AGGR_PHY_STEPS): + + #### 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, done, info = env.step(action) + + #### Compute control at the desired frequency ############## + if i%CTRL_EVERY_N_STEPS == 0: + + #### Compute control for the current way point ############# + action["0"], _, _ = ctrl.computeControlFromState(control_timestep=CTRL_EVERY_N_STEPS*env.TIMESTEP, + state=obs["0"]["state"], + target_pos=TARGET_POS[wp_counter, :], + ) + + #### Go to the next way point and loop ##################### + wp_counter = wp_counter + 1 if wp_counter < (NUM_WP-1) else 0 + + #### Log the simulation #################################### + logger.log(drone=0, + timestamp=i/env.SIM_FREQ, + state= obs["0"]["state"], + control=np.hstack([TARGET_POS[wp_counter, :], np.zeros(9)]) + ) + + #### Printout ############################################## + if i%env.SIM_FREQ == 0: + env.render() + + #### Sync the simulation ################################### + if gui: + sync(i, START, env.TIMESTEP) + + #### Close the environment ################################# + env.close() + + #### Save the simulation results ########################### + logger.save() + logger.save_as_csv("gnd") # 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='Ground effect script using CtrlAviary and DSLPIDControl') + 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_VIDEO, 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('--aggregate', default=DEFAULT_AGGREGATE, type=str2bool, help='Whether to aggregate physics steps (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/important_examples/cylindrical_example.py b/gym_pybullet_drones/important_examples/cylindrical_example.py new file mode 100644 index 000000000..18470ca29 --- /dev/null +++ b/gym_pybullet_drones/important_examples/cylindrical_example.py @@ -0,0 +1,156 @@ + +import os +import time +import argparse +from datetime import datetime +import pdb +import math +import random +import numpy as np +import pybullet as p +import matplotlib.pyplot as plt + +from gym_pybullet_drones.utils.enums import DroneModel, Physics +from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary +from gym_pybullet_drones.envs.VisionAviary import VisionAviary +from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl +from gym_pybullet_drones.control.SimplePIDControl import SimplePIDControl +from gym_pybullet_drones.utils.Logger import Logger +from gym_pybullet_drones.utils.utils import sync, str2bool + +import gym_pybullet_drones.utils.position_commads as pc + +DEFAULT_GUI = True +DEFAULT_RECORD_VIDEO = False +DEFAULT_PLOT = True +DEFAULT_USER_DEBUG_GUI = False +DEFAULT_AGGREGATE = True +DEFAULT_OBSTACLES = True +DEFAULT_SIMULATION_FREQ_HZ = 240 +DEFAULT_CONTROL_FREQ_HZ = 240 +DEFAULT_DURATION_SEC = 4 +DEFAULT_OUTPUT_FOLDER = 'results' +DEFAULT_COLAB = False + +def run( + gui=DEFAULT_GUI, + record_video=DEFAULT_RECORD_VIDEO, + plot=DEFAULT_PLOT, + user_debug_gui=DEFAULT_USER_DEBUG_GUI, + aggregate=DEFAULT_AGGREGATE, + 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 ###################### + PERIOD = 4 + NUM_WP = control_freq_hz*PERIOD + Radius = 0.5 + init_const = 1 + INIT_XYZ = np.array([ 0, 0, init_const]).reshape(1,3) + TARGET_POS = np.zeros((NUM_WP,3)) + for i in range(NUM_WP): + t = np.radians((180*i)/NUM_WP)/3 # value in radians + TARGET_POS[i, :] = pc.cylindrical_to_cartesian(t,t*12, init_const + i/NUM_WP) + # print(f"{i} :: {TARGET_POS[i, :]} :: {t} ") + wp_counter = 0 + AGGR_PHY_STEPS = int(simulation_freq_hz/control_freq_hz) if aggregate else 1 + #### Create the environment ################################ + env = CtrlAviary(drone_model=DroneModel.CF2X, + num_drones=1, + initial_xyzs=INIT_XYZ, + physics=Physics.PYB_GND, + # physics=Physics.PYB, # For comparison + neighbourhood_radius=10, + freq=simulation_freq_hz, + aggregate_phy_steps=AGGR_PHY_STEPS, + 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=int(simulation_freq_hz/AGGR_PHY_STEPS), + num_drones=1, + output_folder=output_folder, + colab=colab + ) + + #### Initialize the controller ############################# + ctrl = DSLPIDControl(drone_model=DroneModel.CF2X) + + #### Run the simulation #################################### + CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ/control_freq_hz)) + action = {"0": np.array([0,0,0,0])} + START = time.time() + for i in range(0, int(duration_sec*env.SIM_FREQ)*3, AGGR_PHY_STEPS): + + #### 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, done, info = env.step(action) + + #### Compute control at the desired frequency ############## + if i%CTRL_EVERY_N_STEPS == 0: + + #### Compute control for the current way point ############# + action["0"], _, _ = ctrl.computeControlFromState(control_timestep=CTRL_EVERY_N_STEPS*env.TIMESTEP, + state=obs["0"]["state"], + target_pos=TARGET_POS[wp_counter, :], + ) + + #### Go to the next way point and loop ##################### + wp_counter = wp_counter + 1 if wp_counter < (NUM_WP-1) else 0 + + #### Log the simulation #################################### + logger.log(drone=0, + timestamp=i/env.SIM_FREQ, + state= obs["0"]["state"], + control=np.hstack([TARGET_POS[wp_counter, :], np.zeros(9)]) + ) + + #### Printout ############################################## + if i%env.SIM_FREQ == 0: + env.render() + + #### Sync the simulation ################################### + if gui: + sync(i, START, env.TIMESTEP) + + #### Close the environment ################################# + env.close() + + #### Save the simulation results ########################### + logger.save() + logger.save_as_csv("gnd") # 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='Ground effect script using CtrlAviary and DSLPIDControl') + 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_VIDEO, 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('--aggregate', default=DEFAULT_AGGREGATE, type=str2bool, help='Whether to aggregate physics steps (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/important_examples/relative_one_movement_example.py b/gym_pybullet_drones/important_examples/relative_one_movement_example.py new file mode 100644 index 000000000..6224b193e --- /dev/null +++ b/gym_pybullet_drones/important_examples/relative_one_movement_example.py @@ -0,0 +1,161 @@ + +import os +import time +import argparse +from datetime import datetime +import pdb +import math +import random +import numpy as np +import pybullet as p +import matplotlib.pyplot as plt + +from gym_pybullet_drones.utils.enums import DroneModel, Physics +from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary +from gym_pybullet_drones.envs.VisionAviary import VisionAviary +from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl +from gym_pybullet_drones.control.SimplePIDControl import SimplePIDControl +from gym_pybullet_drones.utils.Logger import Logger +from gym_pybullet_drones.utils.utils import sync, str2bool + +import gym_pybullet_drones.utils.position_commads as pc + +DEFAULT_GUI = True +DEFAULT_RECORD_VIDEO = False +DEFAULT_PLOT = True +DEFAULT_USER_DEBUG_GUI = False +DEFAULT_AGGREGATE = True +DEFAULT_OBSTACLES = True +DEFAULT_SIMULATION_FREQ_HZ = 240 +DEFAULT_CONTROL_FREQ_HZ = 240 +DEFAULT_DURATION_SEC = 4 +DEFAULT_OUTPUT_FOLDER = 'results' +DEFAULT_COLAB = False + +def run( + gui=DEFAULT_GUI, + record_video=DEFAULT_RECORD_VIDEO, + plot=DEFAULT_PLOT, + user_debug_gui=DEFAULT_USER_DEBUG_GUI, + aggregate=DEFAULT_AGGREGATE, + 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 ###################### + PERIOD = 4 + NUM_WP = control_freq_hz*PERIOD + Radius = 0.5 + init_const = 0.1 + INIT_XYZ = np.array([ init_const+ Radius*np.cos(0), init_const + np.sin(0), init_const]).reshape(1,3) + TARGET_POS = np.zeros((NUM_WP,3)) + for i in range(NUM_WP): + if i == 0: + TARGET_POS[i, :] = INIT_XYZ[:] + elif i < NUM_WP/3: + TARGET_POS[i, :] = pc.relative_movement(TARGET_POS[i-1, 0], TARGET_POS[i-1, 1],TARGET_POS[i-1, 2], -0.002, 0.003, 0.004) + elif i < 2*NUM_WP/3: + TARGET_POS[i, :] = pc.relative_movement(TARGET_POS[i-1, 0], TARGET_POS[i-1, 1],TARGET_POS[i-1, 2], -0.002, -0.003, -0.004) + else: + TARGET_POS[i, :] = pc.relative_movement(TARGET_POS[i-1, 0], TARGET_POS[i-1, 1],TARGET_POS[i-1, 2], 0.004, 0, 0) + wp_counter = 0 + AGGR_PHY_STEPS = int(simulation_freq_hz/control_freq_hz) if aggregate else 1 + #### Create the environment ################################ + env = CtrlAviary(drone_model=DroneModel.CF2X, + num_drones=1, + initial_xyzs=INIT_XYZ, + physics=Physics.PYB_GND, + # physics=Physics.PYB, # For comparison + neighbourhood_radius=10, + freq=simulation_freq_hz, + aggregate_phy_steps=AGGR_PHY_STEPS, + 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=int(simulation_freq_hz/AGGR_PHY_STEPS), + num_drones=1, + output_folder=output_folder, + colab=colab + ) + + #### Initialize the controller ############################# + ctrl = DSLPIDControl(drone_model=DroneModel.CF2X) + + #### Run the simulation #################################### + CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ/control_freq_hz)) + action = {"0": np.array([0,0,0,0])} + START = time.time() + for i in range(0, int(duration_sec*env.SIM_FREQ), AGGR_PHY_STEPS): + + #### 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, done, info = env.step(action) + + #### Compute control at the desired frequency ############## + if i%CTRL_EVERY_N_STEPS == 0: + + #### Compute control for the current way point ############# + action["0"], _, _ = ctrl.computeControlFromState(control_timestep=CTRL_EVERY_N_STEPS*env.TIMESTEP, + state=obs["0"]["state"], + target_pos=TARGET_POS[wp_counter, :], + ) + + #### Go to the next way point and loop ##################### + wp_counter = wp_counter + 1 if wp_counter < (NUM_WP-1) else 0 + + #### Log the simulation #################################### + logger.log(drone=0, + timestamp=i/env.SIM_FREQ, + state= obs["0"]["state"], + control=np.hstack([TARGET_POS[wp_counter, :], np.zeros(9)]) + ) + + #### Printout ############################################## + if i%env.SIM_FREQ == 0: + env.render() + + #### Sync the simulation ################################### + if gui: + sync(i, START, env.TIMESTEP) + + #### Close the environment ################################# + env.close() + + #### Save the simulation results ########################### + logger.save() + logger.save_as_csv("gnd") # 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='Ground effect script using CtrlAviary and DSLPIDControl') + 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_VIDEO, 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('--aggregate', default=DEFAULT_AGGREGATE, type=str2bool, help='Whether to aggregate physics steps (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/important_examples/second_good_union.py b/gym_pybullet_drones/important_examples/second_good_union.py new file mode 100644 index 000000000..914d2f022 --- /dev/null +++ b/gym_pybullet_drones/important_examples/second_good_union.py @@ -0,0 +1,174 @@ +import os +import time +import argparse +from datetime import datetime +import pdb +import math +import random +import numpy as np +import pybullet as p +import matplotlib.pyplot as plt + +from gym_pybullet_drones.utils.enums import DroneModel, Physics +from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary +from gym_pybullet_drones.envs.VisionAviary import VisionAviary +from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl +from gym_pybullet_drones.control.SimplePIDControl import SimplePIDControl +from gym_pybullet_drones.utils.Logger import Logger +from gym_pybullet_drones.utils.utils import sync, str2bool + +import gym_pybullet_drones.utils.position_commads as pc + +DEFAULT_GUI = True +DEFAULT_RECORD_VIDEO = False +DEFAULT_PLOT = True +DEFAULT_USER_DEBUG_GUI = False +DEFAULT_AGGREGATE = True +DEFAULT_OBSTACLES = True +# DEFAULT_SIMULATION_FREQ_HZ = 240 +# DEFAULT_CONTROL_FREQ_HZ = 240 +# DEFAULT_DURATION_SEC = 4 +DEFAULT_SIMULATION_FREQ_HZ = 480 +DEFAULT_CONTROL_FREQ_HZ = 480 +DEFAULT_DURATION_SEC = 12 +DEFAULT_OUTPUT_FOLDER = 'results' +DEFAULT_COLAB = False + +def run( + gui=DEFAULT_GUI, + record_video=DEFAULT_RECORD_VIDEO, + plot=DEFAULT_PLOT, + user_debug_gui=DEFAULT_USER_DEBUG_GUI, + aggregate=DEFAULT_AGGREGATE, + 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 ###################### + PERIOD = duration_sec + NUM_WP = control_freq_hz*PERIOD + TARGET_POS = np.zeros((NUM_WP,3)) + small_size = int(NUM_WP/4) + + x = float(input("X:: ")) + y = float(input("Y:: ")) + z = float(input("Z:: ")) + Radius = float(input("R:: ")) + h = float(input("H:: ")) + + INIT_XYZ, TARGET_POS_1 = pc.circle(Radius, small_size, x, y, z, 0.5) + INIT_XYZ_2, TARGET_POS_2 = pc.cone(Radius, small_size, h, TARGET_POS_1[small_size-1][0], TARGET_POS_1[small_size-1][1], TARGET_POS_1[small_size-1][2], 2) + INIT_XYZ_2, TARGET_POS_3 = pc.circle(-Radius, small_size,TARGET_POS_2[small_size-1][0], TARGET_POS_2[small_size-1][1], TARGET_POS_2[small_size-1][2], 0.5) + INIT_XYZ_2, TARGET_POS_4 = pc.cylinder(-Radius, small_size, h, TARGET_POS_3[small_size-1][0], TARGET_POS_3[small_size-1][1], TARGET_POS_3[small_size-1][2], 2) + for i in range(NUM_WP): + if -1 < i 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, done, info = env.step(action) + + #### Compute control at the desired frequency ############## + if i%CTRL_EVERY_N_STEPS == 0: + + #### Compute control for the current way point ############# + action["0"], _, _ = ctrl.computeControlFromState(control_timestep=CTRL_EVERY_N_STEPS*env.TIMESTEP, + state=obs["0"]["state"], + target_pos=TARGET_POS[wp_counter, :], + ) + + #### Go to the next way point and loop ##################### + wp_counter = wp_counter + 1 if wp_counter < (NUM_WP-1) else 0 + + #### Log the simulation #################################### + logger.log(drone=0, + timestamp=i/env.SIM_FREQ, + state= obs["0"]["state"], + control=np.hstack([TARGET_POS[wp_counter, :], np.zeros(9)]) + ) + + #### Printout ############################################## + if i%env.SIM_FREQ == 0: + env.render() + + #### Sync the simulation ################################### + if gui: + sync(i, START, env.TIMESTEP) + + #### Close the environment ################################# + env.close() + + #### Save the simulation results ########################### + logger.save() + logger.save_as_csv("gnd") # 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='Ground effect script using CtrlAviary and DSLPIDControl') + 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_VIDEO, 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('--aggregate', default=DEFAULT_AGGREGATE, type=str2bool, help='Whether to aggregate physics steps (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/important_examples/smart_good_union.py b/gym_pybullet_drones/important_examples/smart_good_union.py new file mode 100644 index 000000000..4acb30fa3 --- /dev/null +++ b/gym_pybullet_drones/important_examples/smart_good_union.py @@ -0,0 +1,164 @@ +import os +import time +import argparse +from datetime import datetime +import pdb +import math +import random +import numpy as np +import pybullet as p +import matplotlib.pyplot as plt + +from gym_pybullet_drones.utils.enums import DroneModel, Physics +from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary +from gym_pybullet_drones.envs.VisionAviary import VisionAviary +from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl +from gym_pybullet_drones.control.SimplePIDControl import SimplePIDControl +from gym_pybullet_drones.utils.Logger import Logger +from gym_pybullet_drones.utils.utils import sync, str2bool + +import gym_pybullet_drones.utils.position_commads as pc + +DEFAULT_GUI = True +DEFAULT_RECORD_VIDEO = False +DEFAULT_PLOT = True +DEFAULT_USER_DEBUG_GUI = False +DEFAULT_AGGREGATE = True +DEFAULT_OBSTACLES = True +DEFAULT_SIMULATION_FREQ_HZ = 240 +DEFAULT_CONTROL_FREQ_HZ = 240 +DEFAULT_DURATION_SEC = 4 +DEFAULT_OUTPUT_FOLDER = 'results' +DEFAULT_COLAB = False + +def run( + gui=DEFAULT_GUI, + record_video=DEFAULT_RECORD_VIDEO, + plot=DEFAULT_PLOT, + user_debug_gui=DEFAULT_USER_DEBUG_GUI, + aggregate=DEFAULT_AGGREGATE, + 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 ###################### + PERIOD = 4 + NUM_WP = control_freq_hz*PERIOD + Radius = 0.5 + height = 3 + TARGET_POS = np.zeros((NUM_WP,3)) + + small_size = int(NUM_WP/2) + TARGET_POS_1 = np.zeros((small_size,3)) + TARGET_POS_2 = np.zeros((small_size,3)) + + INIT_XYZ, TARGET_POS_1 = pc.circle(Radius, small_size, 0, 0, 0.3, segment=0.5) + INIT_XYZ_2, TARGET_POS_2 = pc.cylinder(Radius/2, small_size, height,TARGET_POS_1[small_size-1][0], TARGET_POS_1[small_size-1][1], TARGET_POS_1[small_size-1][2]) + + for i in range(int(small_size)): + TARGET_POS[i] = TARGET_POS_1 [i] + for i in range(int(small_size)): + TARGET_POS[i+int(small_size)] = TARGET_POS_2[i] + + wp_counter = 0 + AGGR_PHY_STEPS = int(simulation_freq_hz/control_freq_hz) if aggregate else 1 + + #### Create the environment ################################ + env = CtrlAviary(drone_model=DroneModel.CF2X, + num_drones=1, + initial_xyzs=INIT_XYZ, + physics=Physics.PYB_GND, + # physics=Physics.PYB, # For comparison + neighbourhood_radius=10, + freq=simulation_freq_hz, + aggregate_phy_steps=AGGR_PHY_STEPS, + 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=int(simulation_freq_hz/AGGR_PHY_STEPS), + num_drones=1, + output_folder=output_folder, + colab=colab + ) + + #### Initialize the controller ############################# + ctrl = DSLPIDControl(drone_model=DroneModel.CF2X) + + #### Run the simulation #################################### + CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ/control_freq_hz)) + action = {"0": np.array([0,0,0,0])} + START = time.time() + for i in range(0, int(duration_sec*env.SIM_FREQ), AGGR_PHY_STEPS): + + #### 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, done, info = env.step(action) + + #### Compute control at the desired frequency ############## + if i%CTRL_EVERY_N_STEPS == 0: + + #### Compute control for the current way point ############# + action["0"], _, _ = ctrl.computeControlFromState(control_timestep=CTRL_EVERY_N_STEPS*env.TIMESTEP, + state=obs["0"]["state"], + target_pos=TARGET_POS[wp_counter, :], + ) + + #### Go to the next way point and loop ##################### + wp_counter = wp_counter + 1 if wp_counter < (NUM_WP-1) else 0 + + #### Log the simulation #################################### + logger.log(drone=0, + timestamp=i/env.SIM_FREQ, + state= obs["0"]["state"], + control=np.hstack([TARGET_POS[wp_counter, :], np.zeros(9)]) + ) + + #### Printout ############################################## + if i%env.SIM_FREQ == 0: + env.render() + + #### Sync the simulation ################################### + if gui: + sync(i, START, env.TIMESTEP) + + #### Close the environment ################################# + env.close() + + #### Save the simulation results ########################### + logger.save() + logger.save_as_csv("gnd") # 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='Ground effect script using CtrlAviary and DSLPIDControl') + 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_VIDEO, 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('--aggregate', default=DEFAULT_AGGREGATE, type=str2bool, help='Whether to aggregate physics steps (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/important_examples/spherical_example.py b/gym_pybullet_drones/important_examples/spherical_example.py new file mode 100644 index 000000000..f31df1485 --- /dev/null +++ b/gym_pybullet_drones/important_examples/spherical_example.py @@ -0,0 +1,156 @@ + +import os +import time +import argparse +from datetime import datetime +import pdb +import math +import random +import numpy as np +import pybullet as p +import matplotlib.pyplot as plt + +from gym_pybullet_drones.utils.enums import DroneModel, Physics +from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary +from gym_pybullet_drones.envs.VisionAviary import VisionAviary +from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl +from gym_pybullet_drones.control.SimplePIDControl import SimplePIDControl +from gym_pybullet_drones.utils.Logger import Logger +from gym_pybullet_drones.utils.utils import sync, str2bool + +import gym_pybullet_drones.utils.position_commads as pc + +DEFAULT_GUI = True +DEFAULT_RECORD_VIDEO = False +DEFAULT_PLOT = True +DEFAULT_USER_DEBUG_GUI = False +DEFAULT_AGGREGATE = True +DEFAULT_OBSTACLES = True +DEFAULT_SIMULATION_FREQ_HZ = 240 +DEFAULT_CONTROL_FREQ_HZ = 240 +DEFAULT_DURATION_SEC = 4 +DEFAULT_OUTPUT_FOLDER = 'results' +DEFAULT_COLAB = False + +def run( + gui=DEFAULT_GUI, + record_video=DEFAULT_RECORD_VIDEO, + plot=DEFAULT_PLOT, + user_debug_gui=DEFAULT_USER_DEBUG_GUI, + aggregate=DEFAULT_AGGREGATE, + 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 ###################### + PERIOD = 4 + NUM_WP = control_freq_hz*PERIOD + Radius = 0.5 + init_const = 1 + INIT_XYZ = np.array([ 0, 0, init_const]).reshape(1,3) + TARGET_POS = np.zeros((NUM_WP,3)) + for i in range(NUM_WP): + t = np.radians((360*i)/NUM_WP) + TARGET_POS[i, :] = pc.spherical_to_cartesian(init_const + i/NUM_WP, t, 0+t/1000) + wp_counter = 0 + AGGR_PHY_STEPS = int(simulation_freq_hz/control_freq_hz) if aggregate else 1 + + #### Create the environment ################################ + env = CtrlAviary(drone_model=DroneModel.CF2X, + num_drones=1, + initial_xyzs=INIT_XYZ, + physics=Physics.PYB_GND, + # physics=Physics.PYB, # For comparison + neighbourhood_radius=10, + freq=simulation_freq_hz, + aggregate_phy_steps=AGGR_PHY_STEPS, + 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=int(simulation_freq_hz/AGGR_PHY_STEPS), + num_drones=1, + output_folder=output_folder, + colab=colab + ) + + #### Initialize the controller ############################# + ctrl = DSLPIDControl(drone_model=DroneModel.CF2X) + + #### Run the simulation #################################### + CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ/control_freq_hz)) + action = {"0": np.array([0,0,0,0])} + START = time.time() + for i in range(0, int(duration_sec*env.SIM_FREQ), AGGR_PHY_STEPS): + + #### 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, done, info = env.step(action) + + #### Compute control at the desired frequency ############## + if i%CTRL_EVERY_N_STEPS == 0: + + #### Compute control for the current way point ############# + action["0"], _, _ = ctrl.computeControlFromState(control_timestep=CTRL_EVERY_N_STEPS*env.TIMESTEP, + state=obs["0"]["state"], + target_pos=TARGET_POS[wp_counter, :], + ) + + #### Go to the next way point and loop ##################### + wp_counter = wp_counter + 1 if wp_counter < (NUM_WP-1) else 0 + + #### Log the simulation #################################### + logger.log(drone=0, + timestamp=i/env.SIM_FREQ, + state= obs["0"]["state"], + control=np.hstack([TARGET_POS[wp_counter, :], np.zeros(9)]) + ) + + #### Printout ############################################## + if i%env.SIM_FREQ == 0: + env.render() + + #### Sync the simulation ################################### + if gui: + sync(i, START, env.TIMESTEP) + + #### Close the environment ################################# + env.close() + + #### Save the simulation results ########################### + logger.save() + logger.save_as_csv("gnd") # 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='Ground effect script using CtrlAviary and DSLPIDControl') + 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_VIDEO, 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('--aggregate', default=DEFAULT_AGGREGATE, type=str2bool, help='Whether to aggregate physics steps (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/important_examples/square_vel.py b/gym_pybullet_drones/important_examples/square_vel.py new file mode 100644 index 000000000..f5041d070 --- /dev/null +++ b/gym_pybullet_drones/important_examples/square_vel.py @@ -0,0 +1,184 @@ + +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 gym_pybullet_drones.utils.velocity_commands as commands + +from gym_pybullet_drones.utils.enums import DroneModel, Physics +from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary +from gym_pybullet_drones.envs.VisionAviary import VisionAviary +from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl +from gym_pybullet_drones.control.SimplePIDControl import SimplePIDControl +# from gym_pybullet_drones.utils.Logger import Logger +from gym_pybullet_drones.utils.utils import sync, str2bool + +from gym_pybullet_drones.envs.VelocityAviary import VelocityAviary + + + + + +import time + +DEFAULT_DRONE = DroneModel("cf2x") +DEFAULT_GUI = True +DEFAULT_RECORD_VIDEO = False +DEFAULT_PLOT = True +DEFAULT_USER_DEBUG_GUI = False +DEFAULT_AGGREGATE = True +DEFAULT_OBSTACLES = False +DEFAULT_SIMULATION_FREQ_HZ = 240 +DEFAULT_CONTROL_FREQ_HZ = 48 +DEFAULT_DURATION_SEC = 5 +DEFAULT_OUTPUT_FOLDER = 'results' +DEFAULT_COLAB = False + +def run( + drone=DEFAULT_DRONE, + gui=DEFAULT_GUI, + record_video=DEFAULT_RECORD_VIDEO, + plot=DEFAULT_PLOT, + user_debug_gui=DEFAULT_USER_DEBUG_GUI, + aggregate=DEFAULT_AGGREGATE, + 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 ############################# + # INIT_XYZS = np.array([ + # [ 0, 0, .1], + # [.3, 0, .1], + # [.6, 0, .1], + # [0.9, 0, .1] + # ]) + # INIT_RPYS = np.array([ + # [0, 0, 0], + # [0, 0, np.pi/3], + # [0, 0, np.pi/4], + # [0, 0, np.pi/2] + # ]) + INIT_XYZS = np.array([ + [ 0, 0, 1.1], + [ 0, 0, 5.3], + [ 2, 0.3, 5.3], + [ 3, 0.4, 5.4] + ]) + INIT_RPYS = np.array([ + [0, 0, 0], + [3, 3, 3], + [0, 0, 0], + [3, 3, 3] + ]) + AGGR_PHY_STEPS = int(simulation_freq_hz/control_freq_hz) if aggregate else 1 + PHY = Physics.PYB + + #### Create the environment ################################ + env = VelocityAviary(drone_model=drone, + num_drones=4, + initial_xyzs=INIT_XYZS, + initial_rpys=INIT_RPYS, + physics=Physics.PYB, + neighbourhood_radius=10, + freq=simulation_freq_hz, + aggregate_phy_steps=AGGR_PHY_STEPS, + 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() + DRONE_IDS = env.getDroneIds() + + #### Compute number of control steps in the simlation ###### + PERIOD = duration_sec + NUM_WP = control_freq_hz*PERIOD + wp_counters = np.array([0 for i in range(4)]) + + #### Initialize the velocity target ######################## + TARGET_VEL = np.zeros((4,NUM_WP,4)) + for i in range(NUM_WP): + if i < NUM_WP/4: + TARGET_VEL[0, i, :] = commands.along_x_axis(1, True) + elif i < NUM_WP/2: + TARGET_VEL[0, i, :] = commands.along_y_axis(1, True) + elif i < 3*NUM_WP/4: + TARGET_VEL[0, i, :] = commands.along_x_axis(1, False) + else: + TARGET_VEL[0, i, :] = commands.along_y_axis(1, False) + TARGET_VEL[1, i, :] = commands.stand_still() + TARGET_VEL[2, i, :] = commands.stand_still() + TARGET_VEL[3, i, :] = commands.stand_still() + # ox - red; oy - green; oz - blue. + # Цифры: 1 - ox; 2 - oy; 3 - oz; 4 - скорость. -- задается первым тремя числами направление вектора, а 4 - его длина + + + + #### Run the simulation #################################### + CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ/control_freq_hz)) + action = {str(i): np.array([0,0,0,0]) for i in range(4)} + START = time.time() + i = 0 + while True: + i+=1 + ############################################################ + # for j in range(3): env._showDroneLocalAxes(j) + + #### Step the simulation ################################### + obs, reward, done, info = env.step(action) + + #### Compute control at the desired frequency ############## + if i%CTRL_EVERY_N_STEPS == 0: + + #### Compute control for the current way point ############# + for j in range(4): + action[str(j)] = TARGET_VEL[j, wp_counters[j], :] + + #### Go to the next way point and loop ##################### + for j in range(4): + wp_counters[j] = wp_counters[j] + 1 if wp_counters[j] < (NUM_WP-1) else 0 + + + #### Printout ############################################## + if i%env.SIM_FREQ == 0: + env.render() + + #### Sync the simulation ################################### + if gui: + sync(i, START, env.TIMESTEP) + + #### Close the environment ################################# + env.close() + + #### Plot the simulation results ########################### + +if __name__ == "__main__": + #### Define and parse (optional) arguments for the script ## + parser = argparse.ArgumentParser(description='Velocity control example using VelocityAviary') + parser.add_argument('--drone', default=DEFAULT_DRONE, type=DroneModel, help='Drone model (default: CF2X)', metavar='', choices=DroneModel) + 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_VIDEO, 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('--aggregate', default=DEFAULT_AGGREGATE, type=str2bool, help='Whether to aggregate physics steps (default: False)', metavar='') + parser.add_argument('--obstacles', default=DEFAULT_OBSTACLES, type=str2bool, help='Whether to add obstacles to the environment (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('--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/important_examples/square_withZ_vel.py b/gym_pybullet_drones/important_examples/square_withZ_vel.py new file mode 100644 index 000000000..0943313fa --- /dev/null +++ b/gym_pybullet_drones/important_examples/square_withZ_vel.py @@ -0,0 +1,185 @@ + +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 gym_pybullet_drones.utils.velocity_commands as commands + +from gym_pybullet_drones.utils.enums import DroneModel, Physics +from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary +from gym_pybullet_drones.envs.VisionAviary import VisionAviary +from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl +from gym_pybullet_drones.control.SimplePIDControl import SimplePIDControl +# from gym_pybullet_drones.utils.Logger import Logger +from gym_pybullet_drones.utils.utils import sync, str2bool + +from gym_pybullet_drones.envs.VelocityAviary import VelocityAviary + + + + + +import time + +DEFAULT_DRONE = DroneModel("cf2x") +DEFAULT_GUI = True +DEFAULT_RECORD_VIDEO = False +DEFAULT_PLOT = True +DEFAULT_USER_DEBUG_GUI = False +DEFAULT_AGGREGATE = True +DEFAULT_OBSTACLES = False +DEFAULT_SIMULATION_FREQ_HZ = 240 +DEFAULT_CONTROL_FREQ_HZ = 48 +DEFAULT_DURATION_SEC = 5 +DEFAULT_OUTPUT_FOLDER = 'results' +DEFAULT_COLAB = False + +def run( + drone=DEFAULT_DRONE, + gui=DEFAULT_GUI, + record_video=DEFAULT_RECORD_VIDEO, + plot=DEFAULT_PLOT, + user_debug_gui=DEFAULT_USER_DEBUG_GUI, + aggregate=DEFAULT_AGGREGATE, + 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 ############################# + # INIT_XYZS = np.array([ + # [ 0, 0, .1], + # [.3, 0, .1], + # [.6, 0, .1], + # [0.9, 0, .1] + # ]) + # INIT_RPYS = np.array([ + # [0, 0, 0], + # [0, 0, np.pi/3], + # [0, 0, np.pi/4], + # [0, 0, np.pi/2] + # ]) + INIT_XYZS = np.array([ + [ 0, 0, 1.1], + [ 0, 0, 5.3], + [ 2, 0.3, 5.3], + [ 3, 0.4, 5.4] + ]) + INIT_RPYS = np.array([ + [0, 0, 0], + [3, 3, 3], + [0, 0, 0], + [3, 3, 3] + ]) + AGGR_PHY_STEPS = int(simulation_freq_hz/control_freq_hz) if aggregate else 1 + PHY = Physics.PYB + + #### Create the environment ################################ + env = VelocityAviary(drone_model=drone, + num_drones=4, + initial_xyzs=INIT_XYZS, + initial_rpys=INIT_RPYS, + physics=Physics.PYB, + neighbourhood_radius=10, + freq=simulation_freq_hz, + aggregate_phy_steps=AGGR_PHY_STEPS, + 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() + DRONE_IDS = env.getDroneIds() + + #### Compute number of control steps in the simlation ###### + PERIOD = duration_sec + NUM_WP = control_freq_hz*PERIOD + wp_counters = np.array([0 for i in range(4)]) + + #### Initialize the velocity target ######################## + TARGET_VEL = np.zeros((4,NUM_WP,4)) + for i in range(NUM_WP): + if i < NUM_WP/4: + TARGET_VEL[0, i, :] = commands.compare_two(commands.along_x_axis(1, True), commands.along_z_axis(1, True)) + elif i < NUM_WP/2: + TARGET_VEL[0, i, :] = commands.compare_two(commands.along_y_axis(1, True), commands.along_z_axis(1, False)) + elif i < 3*NUM_WP/4: + TARGET_VEL[0, i, :] = commands.compare_two(commands.along_x_axis(1, False), commands.along_z_axis(1, True)) + else: + TARGET_VEL[0, i, :] = commands.compare_two(commands.along_y_axis(1, False), commands.along_z_axis(1, False)) + TARGET_VEL[1, i, :] = commands.stand_still() + TARGET_VEL[2, i, :] = commands.stand_still() + TARGET_VEL[3, i, :] = commands.stand_still() + # ox - red; oy - green; oz - blue. + # Цифры: 1 - ox; 2 - oy; 3 - oz; 4 - скорость. -- задается первым тремя числами направление вектора, а 4 - его длина + + + + #### Run the simulation #################################### + CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ/control_freq_hz)) + action = {str(i): np.array([0,0,0,0]) for i in range(4)} + START = time.time() + i = 0 + while True: + i += 1 + + ############################################################ + # for j in range(3): env._showDroneLocalAxes(j) + + #### Step the simulation ################################### + obs, reward, done, info = env.step(action) + + #### Compute control at the desired frequency ############## + if i%CTRL_EVERY_N_STEPS == 0: + + #### Compute control for the current way point ############# + for j in range(4): + action[str(j)] = TARGET_VEL[j, wp_counters[j], :] + + #### Go to the next way point and loop ##################### + for j in range(4): + wp_counters[j] = wp_counters[j] + 1 if wp_counters[j] < (NUM_WP-1) else 0 + + + #### Printout ############################################## + if i%env.SIM_FREQ == 0: + env.render() + + #### Sync the simulation ################################### + if gui: + sync(i, START, env.TIMESTEP) + + #### Close the environment ################################# + env.close() + + #### Plot the simulation results ########################### + +if __name__ == "__main__": + #### Define and parse (optional) arguments for the script ## + parser = argparse.ArgumentParser(description='Velocity control example using VelocityAviary') + parser.add_argument('--drone', default=DEFAULT_DRONE, type=DroneModel, help='Drone model (default: CF2X)', metavar='', choices=DroneModel) + 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_VIDEO, 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('--aggregate', default=DEFAULT_AGGREGATE, type=str2bool, help='Whether to aggregate physics steps (default: False)', metavar='') + parser.add_argument('--obstacles', default=DEFAULT_OBSTACLES, type=str2bool, help='Whether to add obstacles to the environment (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('--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/important_examples/triple_offset.py b/gym_pybullet_drones/important_examples/triple_offset.py new file mode 100644 index 000000000..a4a889bc4 --- /dev/null +++ b/gym_pybullet_drones/important_examples/triple_offset.py @@ -0,0 +1,169 @@ + +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 gym_pybullet_drones.utils.velocity_commands as commands + +from gym_pybullet_drones.utils.enums import DroneModel, Physics +from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary +from gym_pybullet_drones.envs.VisionAviary import VisionAviary +from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl +from gym_pybullet_drones.control.SimplePIDControl import SimplePIDControl +# from gym_pybullet_drones.utils.Logger import Logger +from gym_pybullet_drones.utils.utils import sync, str2bool + +from gym_pybullet_drones.envs.VelocityAviary import VelocityAviary + +import time + +DEFAULT_DRONE = DroneModel("cf2x") +DEFAULT_GUI = True +DEFAULT_RECORD_VIDEO = False +DEFAULT_PLOT = True +DEFAULT_USER_DEBUG_GUI = False +DEFAULT_AGGREGATE = True +DEFAULT_OBSTACLES = False +DEFAULT_SIMULATION_FREQ_HZ = 240 +DEFAULT_CONTROL_FREQ_HZ = 48 +DEFAULT_DURATION_SEC = 5 +DEFAULT_OUTPUT_FOLDER = 'results' +DEFAULT_COLAB = False + +def run( + drone=DEFAULT_DRONE, + gui=DEFAULT_GUI, + record_video=DEFAULT_RECORD_VIDEO, + plot=DEFAULT_PLOT, + user_debug_gui=DEFAULT_USER_DEBUG_GUI, + aggregate=DEFAULT_AGGREGATE, + 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 ############################# + INIT_XYZS = np.array([ + [ 0, 0, 1.1], + [ 0, 0, 5.3], + [ 2, 0.3, 5.3], + [ 3, 0.4, 5.4] + ]) + INIT_RPYS = np.array([ + [0, 0, 0], + [3, 3, 3], + [0, 0, 0], + [3, 3, 3] + ]) + AGGR_PHY_STEPS = int(simulation_freq_hz/control_freq_hz) if aggregate else 1 + PHY = Physics.PYB + + #### Create the environment ################################ + env = VelocityAviary(drone_model=drone, + num_drones=4, + initial_xyzs=INIT_XYZS, + initial_rpys=INIT_RPYS, + physics=Physics.PYB, + neighbourhood_radius=10, + freq=simulation_freq_hz, + aggregate_phy_steps=AGGR_PHY_STEPS, + 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() + DRONE_IDS = env.getDroneIds() + + #### Compute number of control steps in the simlation ###### + PERIOD = duration_sec + NUM_WP = control_freq_hz*PERIOD + wp_counters = np.array([0 for i in range(4)]) + + #### Initialize the velocity target ######################## + TARGET_VEL = np.zeros((4,NUM_WP,4)) + for i in range(NUM_WP): + if i < NUM_WP/4: + TARGET_VEL[0, i, :] = commands.compare_tree(commands.along_x_axis(1, True), commands.along_z_axis(1, True), commands.along_y_axis(i/NUM_WP, True)) + elif i < NUM_WP/2: + TARGET_VEL[0, i, :] = commands.compare_tree(commands.along_y_axis(1, True), commands.along_z_axis(1, False), commands.along_x_axis(i/NUM_WP, True)) + elif i < 3*NUM_WP/4: + TARGET_VEL[0, i, :] = commands.compare_tree(commands.along_x_axis(1, False), commands.along_z_axis(1, True), commands.along_y_axis(i/NUM_WP, False)) + else: + TARGET_VEL[0, i, :] = commands.compare_tree(commands.along_y_axis(1, False), commands.along_z_axis(1, False), commands.along_x_axis(i/NUM_WP, False)) + TARGET_VEL[1, i, :] = commands.stand_still() + TARGET_VEL[2, i, :] = commands.stand_still() + TARGET_VEL[3, i, :] = commands.stand_still() + + + + #### Run the simulation #################################### + CTRL_EVERY_N_STEPS = int(np.floor(env.SIM_FREQ/control_freq_hz)) + action = {str(i): np.array([0,0,0,0]) for i in range(4)} + START = time.time() + i = 0 + while True: + # time.sleep(0.05) + + i += 1 + + ############################################################ + # for j in range(3): env._showDroneLocalAxes(j) + + #### Step the simulation ################################### + obs, reward, done, info = env.step(action) + + #### Compute control at the desired frequency ############## + if i%CTRL_EVERY_N_STEPS == 0: + + #### Compute control for the current way point ############# + for j in range(4): + action[str(j)] = TARGET_VEL[j, wp_counters[j], :] + + #### Go to the next way point and loop ##################### + for j in range(4): + wp_counters[j] = wp_counters[j] + 1 if wp_counters[j] < (NUM_WP-1) else 0 + + + #### Printout ############################################## + if i%env.SIM_FREQ == 0: + env.render() + + #### Sync the simulation ################################### + if gui: + sync(i, START, env.TIMESTEP) + + #### Close the environment ################################# + env.close() + + #### Plot the simulation results ########################### + +if __name__ == "__main__": + #### Define and parse (optional) arguments for the script ## + parser = argparse.ArgumentParser(description='Velocity control example using VelocityAviary') + parser.add_argument('--drone', default=DEFAULT_DRONE, type=DroneModel, help='Drone model (default: CF2X)', metavar='', choices=DroneModel) + 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_VIDEO, 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('--aggregate', default=DEFAULT_AGGREGATE, type=str2bool, help='Whether to aggregate physics steps (default: False)', metavar='') + parser.add_argument('--obstacles', default=DEFAULT_OBSTACLES, type=str2bool, help='Whether to add obstacles to the environment (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('--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/utils/position_commads.py b/gym_pybullet_drones/utils/position_commads.py index 874ac45b3..f10b07546 100644 --- a/gym_pybullet_drones/utils/position_commads.py +++ b/gym_pybullet_drones/utils/position_commads.py @@ -1,156 +1,118 @@ import numpy as np +def between_points(NUM_WP: int, x0: float, y0: float, z0: float, x1: float, y1: float, z1: float): + INIT_XYZ = np.array([x0, y0, z0]).reshape(1,3) + TARGET_POS = np.zeros((NUM_WP,3)) + for i in range(NUM_WP): + TARGET_POS[i, :] = INIT_XYZ[0, 0] + (x1-x0)*i/NUM_WP, INIT_XYZ[0, 1] + (y1-y0)*i/NUM_WP, INIT_XYZ[0, 2] + (z1-z0)*i/NUM_WP + return INIT_XYZ, TARGET_POS + def relative_movement(x0: float, y0: float, z0: float, dx: float, dy: float, dz: float): - """ - Use: TARGET_POS[i, :] = relative_movement(TARGET_POS[i-1, 0], TARGET_POS[i-1, 1],TARGET_POS[i-1, 2], -0.002, 0.003, 0.004) - """ return [x0+dx, y0+dy, z0+dz] -def circle(Radius: float, NUM_WP: int, init_const:float): - """ - Radius -- 0.5 of (The Width of a Circle); look at :: https://youtu.be/pnRNAIQAc50 ; - init_const -- const value > 0 - Use: INIT_XYZ, TARGET_POS = circle(Radius, NUM_WP, init_const) - """ - INIT_XYZ = np.array([ init_const+ Radius*np.cos(0), init_const + np.sin(0), init_const]).reshape(1,3) +def circle(Radius: float, NUM_WP: int, init_X: float,init_Y: float, init_Z: float, segment = 1): + if init_Z <= 0: + init_Z = 0.1 + INIT_XYZ = np.array([init_X, init_Y, init_Z]).reshape(1,3) TARGET_POS = np.zeros((NUM_WP,3)) for i in range(NUM_WP): - t = (2*np.pi*i)/NUM_WP - TARGET_POS[i, :] = INIT_XYZ[0, 0] + Radius*np.cos(t), INIT_XYZ[0, 1] + Radius*np.sin(t), INIT_XYZ[0, 2] #+ i/NUM_WP + t = (segment*2*np.pi*i)/NUM_WP + np.pi/2 + TARGET_POS[i, :] = INIT_XYZ[0, 0] + Radius*np.cos(t) , INIT_XYZ[0, 1] + Radius*np.sin(t) - Radius, INIT_XYZ[0, 2] #+ i/NUM_WP return INIT_XYZ, TARGET_POS -def circle_many_drones(num_drones: int, Radius: float, NUM_WP: int, init_const:float): - """ - Radius -- 0.5 of (The Width of a Circle); look at :: https://youtu.be/pnRNAIQAc50 ; - init_const -- const value > 0 - num_drones > 1 - Use: INIT_XYZS, TARGET_POS = circle_many_drones(num_drones, R, NUM_WP, 0.1) - """ +def circle_frome_centre(Radius: float, NUM_WP: int, init_X: float,init_Y: float, init_Z: float,segment = 1): + if init_Z <= 0: + init_Z = 0.1 + INIT_XYZ = np.array([init_X, init_Y + Radius, init_Z]).reshape(1,3) + TARGET_POS = np.zeros((NUM_WP,3)) + for i in range(NUM_WP): + t = (segment*2*np.pi*i)/NUM_WP + np.pi/2 + TARGET_POS[i, :] = INIT_XYZ[0, 0] + Radius*np.cos(t) , INIT_XYZ[0, 1] + Radius*np.sin(t) - Radius, INIT_XYZ[0, 2] #+ i/NUM_WP + return INIT_XYZ, TARGET_POS + +def circle_many_drones(num_drones: int, Radius: float, NUM_WP: int, init_X: float,init_Y: float, init_Z: float): + if init_Z <= 0: + init_Z = 0.1 init_step = 0.1 - INIT_XYZ = np.array([[init_const+ Radius*np.cos(0), init_const + np.sin(0), init_const+i*init_step] for i in range(num_drones)]) + INIT_XYZ = np.array([[init_X, init_Y, init_Z+i*init_step] for i in range(num_drones)]) TARGET_POS = np.zeros((NUM_WP,3)) for i in range(NUM_WP): - t = (2*np.pi*i)/NUM_WP - TARGET_POS[i, :] = INIT_XYZ[0, 0] + Radius*np.cos(t), INIT_XYZ[0, 1] + Radius*np.sin(t), INIT_XYZ[0, 2] #+ i/NUM_WP + t = (2*np.pi*i)/NUM_WP + np.pi/2 + TARGET_POS[i, :] = INIT_XYZ[0, 0] + Radius*np.cos(t) , INIT_XYZ[0, 1] + Radius*np.sin(t) - Radius, INIT_XYZ[0, 2] #+ i/NUM_WP return INIT_XYZ, TARGET_POS -def cylinder(Radius: float, NUM_WP: int, init_const:float, height: float): - """ - Radius -- 0.5 of (The Width of a Circle); look at :: https://youtu.be/pnRNAIQAc50 ; - init_const -- const value > 0 - height - height of cylinder - Use: INIT_XYZ, TARGET_POS = cylinder(Radius, NUM_WP, init_const, height) - """ - INIT_XYZ = np.array([ init_const+ Radius*np.cos(0), init_const + np.sin(0), init_const]).reshape(1,3) +def cylinder(Radius: float, NUM_WP: int, height: float, init_X: float,init_Y: float, init_Z: float, segment = 1): + if init_Z <= 0: + init_Z = 0.1 + INIT_XYZ = np.array([init_X, init_Y, init_Z]).reshape(1,3) TARGET_POS = np.zeros((NUM_WP,3)) for i in range(NUM_WP): - t = (2*np.pi*i)/NUM_WP - TARGET_POS[i, :] = INIT_XYZ[0, 0] + Radius*np.cos(t), INIT_XYZ[0, 1] + Radius*np.sin(t), INIT_XYZ[0, 2] + height*i/NUM_WP + t = (segment*2*np.pi*i)/NUM_WP + np.pi/2 + TARGET_POS[i, :] = INIT_XYZ[0, 0] + Radius*np.cos(t) , INIT_XYZ[0, 1] + Radius*np.sin(t) - Radius, INIT_XYZ[0, 2] + height*i/NUM_WP + # print(f"{i} :: { INIT_XYZ[0, 0] + Radius*np.cos(t) , INIT_XYZ[0, 1] + Radius*np.sin(t) - Radius, INIT_XYZ[0, 2]}") return INIT_XYZ, TARGET_POS - -def cylinder_many_drones(num_drones: int, Radius: float, NUM_WP: int, init_const:float, height: float): - """ - Radius -- 0.5 of (The Width of a Circle); look at :: https://youtu.be/pnRNAIQAc50 ; - init_const -- const value > 0 - height - height of cylinder - Use: INIT_XYZS, TARGET_POS = cylinder_many_drones(num_drones, R, NUM_WP, init_const, height) - """ - init_step = 0.1 - INIT_XYZ = np.array([[init_const+ Radius*np.cos(0), init_const + np.sin(0), init_const+i*init_step] for i in range(num_drones)]) - TARGET_POS = np.zeros((NUM_WP,3)) +def cylinder_frome_centre(Radius: float, NUM_WP: int, height: float, init_X: float,init_Y: float, init_Z: float, segment = 1): + if init_Z <= 0: + init_Z = 0.1 + INIT_XYZ = np.array([init_X, init_Y + Radius, init_Z]).reshape(1,3) + TARGET_POS = np.zeros((NUM_WP,3)) for i in range(NUM_WP): - t = (2*np.pi*i)/NUM_WP - TARGET_POS[i, :] = INIT_XYZ[0, 0] + Radius*np.cos(t), INIT_XYZ[0, 1] + Radius*np.sin(t), INIT_XYZ[0, 2] + height*i/NUM_WP + t = (segment*2*np.pi*i)/NUM_WP + np.pi/2 + TARGET_POS[i, :] = INIT_XYZ[0, 0] + Radius*np.cos(t) , INIT_XYZ[0, 1] + Radius*np.sin(t) - Radius, INIT_XYZ[0, 2] + height*i/NUM_WP return INIT_XYZ, TARGET_POS -def ellipse(Radius_x: float, Radius_y: float, NUM_WP: int, init_const:float): - """ - Radius_x -- "A" value in the analytically the equation of a standard ellipse ; look at :: https://en.wikipedia.org/wiki/Ellipse - Radius_y -- "B" value in the analytically the equation of a standard ellipse ; look at :: https://en.wikipedia.org/wiki/Ellipse - init_const -- const value > 0 - Use: INIT_XYZ, TARGET_POS = ellipse(Radius_x, Radius_y, NUM_WP, init_const) - """ - INIT_XYZ = np.array([ Radius_x*np.cos(0) - Radius_x, np.sin(0), init_const]).reshape(1,3) +def ellipse(Radius_x: float, Radius_y: float, NUM_WP: int, init_X: float,init_Y: float, init_Z: float, segment = 1): + if init_Z == 0: + init_Z = 0.1 + INIT_XYZ = np.array([init_X, init_Y, init_Z]).reshape(1,3) TARGET_POS = np.zeros((NUM_WP,3)) for i in range(NUM_WP): - t = (2*np.pi*i)/NUM_WP - if i == 0: - TARGET_POS[i :] = INIT_XYZ[0, 0], INIT_XYZ[0, 1], INIT_XYZ[0, 2] - continue - TARGET_POS[i, :] = INIT_XYZ[0, 0] + Radius_x*np.cos(t), INIT_XYZ[0, 1] + Radius_y*np.sin(t), init_const #+ i/NUM_WP + t = (segment*2*np.pi*i)/NUM_WP + np.pi/2 + TARGET_POS[i, :] = INIT_XYZ[0, 0] + Radius_x*np.cos(t), INIT_XYZ[0, 1] + Radius_y*np.sin(t) - Radius_y, INIT_XYZ[0, 2] #+ i/NUM_WP return INIT_XYZ, TARGET_POS -def ellipse_many_drones(num_drones: int, Radius_x: float, Radius_y: float, NUM_WP: int, init_const:float): - """ - Radius_x -- "A" value in the analytically the equation of a standard ellipse ; look at :: https://en.wikipedia.org/wiki/Ellipse - Radius_y -- "B" value in the analytically the equation of a standard ellipse ; look at :: https://en.wikipedia.org/wiki/Ellipse - init_const -- const value > 0 - Use: INIT_XYZS, TARGET_POS = ps.spiral_many_drones(num_drones, R, NUM_WP, init_const) - """ - init_step = 0.1 - INIT_XYZ = np.array([[init_const+ Radius_x*np.cos(0), init_const + np.sin(0), init_const+i*init_step] for i in range(num_drones)]) +def cone(Radius: float, NUM_WP: int, height: float, init_X: float,init_Y: float, init_Z: float, segment = 1): + if init_Z == 0: + init_Z = 0.1 + INIT_XYZ = np.array([init_X, init_Y, init_Z]).reshape(1,3) TARGET_POS = np.zeros((NUM_WP,3)) for i in range(NUM_WP): - t = (2*np.pi*i)/NUM_WP - TARGET_POS[i, :] = INIT_XYZ[0, 0] + Radius_x*np.cos(t), init_const + Radius_y*np.sin(t), init_const #+ i/NUM_WP + t = (segment*2*np.pi*i)/NUM_WP + np.pi/2 + TARGET_POS[i, :] = INIT_XYZ[0, 0] + (1-i/NUM_WP)*Radius*np.cos(t) , INIT_XYZ[0, 1] + (1-i/NUM_WP)*(Radius*np.sin(t)) - Radius, INIT_XYZ[0, 2] + height*i/NUM_WP return INIT_XYZ, TARGET_POS -def spiral(Radius: float, NUM_WP: int, init_const:float): - """ - Radius -- 0.5 of (The Width of a Circle); look at :: https://youtu.be/pnRNAIQAc50 ; - init_const -- const value > 0 - Use: INIT_XYZ, TARGET_POS = spiral(Radius, NUM_WP, init_const) - """ - INIT_XYZ = np.array([ init_const+ Radius*np.cos(0), init_const + np.sin(0), init_const]).reshape(1,3) +def cone_frome_centre(Radius: float, NUM_WP: int, height: float, init_X: float,init_Y: float, init_Z: float, segment = 1): + if init_Z == 0: + init_Z = 0.1 + INIT_XYZ = np.array([init_X, init_Y + Radius, init_Z]).reshape(1,3) TARGET_POS = np.zeros((NUM_WP,3)) for i in range(NUM_WP): - t = (2*np.pi*i)/NUM_WP - TARGET_POS[i, :] = INIT_XYZ[0, 0] + (1-i/NUM_WP) * Radius*np.cos(t), INIT_XYZ[0, 1] + (1-i/NUM_WP) * Radius*np.sin(t), INIT_XYZ[0, 2] + t = (segment*2*np.pi*i)/NUM_WP + np.pi/2 + TARGET_POS[i, :] = INIT_XYZ[0, 0] + (1-i/NUM_WP)*Radius*np.cos(t) , INIT_XYZ[0, 1] + (1-i/NUM_WP)*(Radius*np.sin(t)) - Radius, INIT_XYZ[0, 2] + height*i/NUM_WP return INIT_XYZ, TARGET_POS - -def spiral_many_drones(num_drones: int, Radius: float, NUM_WP: int, init_const:float): - """ - Radius -- 0.5 of (The Width of a Circle); look at :: https://youtu.be/pnRNAIQAc50 ; - init_const -- const value > 0 - Use: INIT_XYZ, TARGET_POS = spiral_many_drones(num_drones,Radius, NUM_WP, init_const) - """ - init_step = 0.1 - INIT_XYZ = np.array([[init_const+ Radius*np.cos(0), init_const + np.sin(0), init_const+i*init_step] for i in range(num_drones)]) - TARGET_POS = np.zeros((NUM_WP,3)) - for i in range(NUM_WP): - t = (2*np.pi*i)/NUM_WP - TARGET_POS[i, :] = INIT_XYZ[0, 0] + (-i/NUM_WP) * Radius*np.cos(t), INIT_XYZ[0, 1] + (-i/NUM_WP) * Radius*np.sin(t), INIT_XYZ[0, 2] #+ i/NUM_WP - return INIT_XYZ, TARGET_POS -def cone(Radius: float, NUM_WP: int, init_const:float, height: float): - """ - Radius -- 0.5 of (The Width of a Circle); look at :: https://youtu.be/pnRNAIQAc50 ; - init_const -- const value > 0 - height - height of cone - Use: INIT_XYZ, TARGET_POS = cone(Radius, NUM_WP, init_const, height) - """ - INIT_XYZ = np.array([ init_const+ Radius*np.cos(0), init_const + np.sin(0), init_const]).reshape(1,3) +def spiral(Radius: float, NUM_WP: int, init_X: float,init_Y: float, init_Z: float, segment = 1): + if init_Z == 0: + init_Z = 0.1 + INIT_XYZ = np.array([init_X, init_Y, init_Z]).reshape(1,3) TARGET_POS = np.zeros((NUM_WP,3)) for i in range(NUM_WP): - t = (2*np.pi*i)/NUM_WP - TARGET_POS[i, :] = INIT_XYZ[0, 0] + (1-i/NUM_WP) * Radius*np.cos(t), INIT_XYZ[0, 1] + (1-i/NUM_WP) * Radius*np.sin(t), INIT_XYZ[0, 2] + height*i/NUM_WP + t = (segment*2*np.pi*i)/NUM_WP + np.pi/2 + TARGET_POS[i, :] = INIT_XYZ[0, 0] + (1-i/NUM_WP)*Radius*np.cos(t) , INIT_XYZ[0, 1] + (1-i/NUM_WP)*(Radius*np.sin(t)) - Radius, INIT_XYZ[0, 2] return INIT_XYZ, TARGET_POS -def cone_many_drones(num_drones: int, Radius: float, NUM_WP: int, init_const:float, height: float): - """ - Radius -- 0.5 of (The Width of a Circle); look at :: https://youtu.be/pnRNAIQAc50 ; - init_const -- const value > 0 - Use: INIT_XYZS, TARGET_POS = ps.cone_many_drones(num_drones, R, NUM_WP, init_const, height) - """ - init_step = 0.1 - INIT_XYZ = np.array([[init_const+ Radius*np.cos(0), init_const + np.sin(0), init_const+i*init_step] for i in range(num_drones)]) +def spiral_frome_centre(Radius: float, NUM_WP: int, init_X: float,init_Y: float, init_Z: float, segment = 1): + if init_Z == 0: + init_Z = 0.1 + INIT_XYZ = np.array([init_X, init_Y + Radius, init_Z]).reshape(1,3) TARGET_POS = np.zeros((NUM_WP,3)) for i in range(NUM_WP): - t = (2*np.pi*i)/NUM_WP - TARGET_POS[i, :] = INIT_XYZ[0, 0] + (-i/NUM_WP) * Radius*np.cos(t), INIT_XYZ[0, 1] + (-i/NUM_WP) * Radius*np.sin(t), INIT_XYZ[0, 2] + height*i/NUM_WP + t = (segment*2*np.pi*i)/NUM_WP + np.pi/2 + TARGET_POS[i, :] = INIT_XYZ[0, 0] + (1-i/NUM_WP)*Radius*np.cos(t) , INIT_XYZ[0, 1] + (1-i/NUM_WP)*(Radius*np.sin(t)) - Radius, INIT_XYZ[0, 2] return INIT_XYZ, TARGET_POS + def spherical_to_cartesian(R:float, phi:float, teta:float): # angles in radians """ conversion from Cartesian to spherical coordinate system diff --git a/gym_pybullet_drones/utils/velocity_commands.py b/gym_pybullet_drones/utils/velocity_commands.py index f758d9ea3..6e3b2ac1a 100644 --- a/gym_pybullet_drones/utils/velocity_commands.py +++ b/gym_pybullet_drones/utils/velocity_commands.py @@ -30,7 +30,6 @@ def compare_tree(first_list: list, second_list: list, third_list:list)->list: out[3] = out[3]/3 # здесь можно отказаться от этой строки и вернуть сразу list(map(sum, zip(first_list, second_list, third_list))) return out -# Плохие функции углов -- их можно улучшить, добавив обертку приведения углов и уменьшив их количетсво вдвое (если получится))) def angle_between_ox_oy(angle: float, intensity: float, ascending: bool)->list: # positive; 0 < angle < 90 in degrees if angle <= 0 or angle >= 90: raise(f"Angle gotta be 0 < angle < 90; now angle == {angle}") @@ -59,5 +58,4 @@ def angle_between_oy_oz(angle: float, intensity: float, ascending: bool)->list: def angle_between_oz_oy(angle: float, intensity: float, ascending: bool)->list: # positive; 0 < angle < 90 in degrees if angle <= 0 or angle >= 90: raise(f"Angle gotta be 0 < angle < 90; now angle == {angle}") - return [0, math.tan(math.radians(angle)), 1, 1] if ascending else [0, -math.tan(math.radians(angle)), -1, 1] - \ No newline at end of file + return [0, math.tan(math.radians(angle)), 1, 1] if ascending else [0, -math.tan(math.radians(angle)), -1, 1] \ No newline at end of file