Kondratenko drone control -- first iteration: Done

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

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))