Add a controller for drones

src/env/gs_env/sim/controllers/drone_controller/README.md

@@ -0,0 +1,24 @@
+### Run demos
+- Make sure your PC has one GPU at least
+- Enter your conda env
+- Install dependencies by: `pip install -e`
+
+#### Use RC control FPV in Genesis
+- Flash HEX file in `./modified_BF_firmware/betaflight_4.4.0_STM32H743_forRC.hex` to your FCU (for STM32H743)
+- Use Type-c to power the FCU, and connect UART port (on FCU) and USB port (on PC) through USB2TTL module
+- Connect the FC and use mavlink to send FC_data from FCU to PC
+- Use `ls /dev/tty*` to check the port id and modified param `USB_path` in `./config/flight.yaml`
+- Do this since the default mavlink frequence for rc_channle is too low
+- Use RC to control the sim drone by:
+    ```
+    python src/env/gs_env/sim/controllers/drone_controller/eval/rc_FPV_eval.py
+    ```
+#### Position controller test
+- Try to get the target with no planning, thus **has poor performance**
+    ```
+    python src/env/gs_env/sim/controllers/drone_controller/eval/pos_ctrl_eval.py
+    ```
+
+### NOTE
+- Use `pip install pymavlink` to install pymavlink
+- Add `export SETUPTOOLS_USE_DISTUTILS=stdlib` into ~/.bashrc if distutils.core has been pointed to distutils but not setuptools/_distutils, or an assert will be triggered in some cases

src/env/gs_env/sim/controllers/drone_controller/config/pos_ctrl_eval/flight.yaml

@@ -0,0 +1,82 @@
+
+# =========== pid_param =============
+ang:
+  kp_r: 6500
+  ki_r: 0.005
+  kd_r: 0.0001
+  kf_r: 0.0
+
+  kp_p: 6500
+  ki_p: 0.005
+  kd_p: 0.0001
+  kf_p: 0.0
+
+  kp_y: 7000                                          
+  ki_y: 0.0
+  kd_y: 0.0
+  kf_y: 0.0
+
+rat:
+  kp_r: 6500
+  ki_r: 0.01
+  kd_r: 0.0
+  kf_r: 0.0
+
+  kp_p: 6500
+  ki_p: 0.01
+  kd_p: 0.0
+  kf_p: 0.0
+
+  kp_y: 7000.0
+  ki_y: 0.0
+  kd_y: 0.0
+  kf_y: 0.0
+
+pos:
+  kp_x: 0.5
+  ki_x: 0.001
+  kd_x: 0.005
+  kf_x: 0.0
+
+  kp_y: 0.5
+  ki_y: 0.001
+  kd_y: 0.005
+  kf_y: 0.0
+
+  kp_t: 1.7
+  ki_t: 0.01
+  kd_t: 0.01
+  kf_t: 0.0
+
+pid_exec_freq: 60
+
+# =========== drone_param =============
+# see in assets/drone_urdf/cf2x.urdf
+thrust_compensate: 0.0
+weight: 0.5   
+motor_inertia: 2.6e-07
+g: 9.81   
+kf: 3.16e-10
+base_rpm: 62293.9641914   # equal to (m*g/4*kf)^0.5
+inertia_matrix:       # no use
+  - [0.0014, 0.0,     0.0,     0.0]
+  - [0.0,    0.0014,  0.0,     0.0]
+  - [0.0,    0.0,     0.00217, 0.0]
+  - [0.0,    0.0,     0.0,     1.0]
+
+# =========== mavlink_param =============
+USB_path: "/dev/ttyUSB0"
+baudrate: 2000000
+
+ARM:       "ch5"
+ANGLE:     "ch6"
+OFFBOARD:  "ch8"
+YAW:       "ch3" 
+PITCH:     "ch2"
+ROLL:      "ch1"
+THRUTTLE:  "ch4"   
+
+# =========== odom_param =============
+gyro_update_freq: 8000
+att_update_freq: 100
+acc_update_freqL: 1000

src/env/gs_env/sim/controllers/drone_controller/config/pos_ctrl_eval/genesis_env.yaml

@@ -0,0 +1,29 @@
+
+# ========= genesis =========
+num_envs: 1     
+drone_num: 1
+dt: 0.01
+max_vis_FPS: 30 
+max_dis_num: 5
+cam_quat: [-0.455, -0.542, 0.542, 0.455]  # FPV camera, 70.0 degree, show in https://quaternions.online/
+cam_pos: [0.02, 0.0, 0.04]
+cam_res: [16, 12]                         # width, height
+drone_init_pos: [0.0, 0.0, 0.01]           # Initial position of the drone
+use_FPV_camera: True
+viewer_follow_drone: True
+load_map: False
+use_rc: False
+show_viewer: True
+render_cam: False
+controller: "position"
+vis_waypoints: True
+map: "gates"
+min_dis: 0.8
+map_width: 3
+map_length: 3
+
+
+
+
+
+

src/env/gs_env/sim/controllers/drone_controller/config/rc_FPV_eval/flight.yaml

@@ -0,0 +1,82 @@
+
+# =========== pid_param =============
+ang:
+  kp_r: 6500
+  ki_r: 0.005
+  kd_r: 0.0001
+  kf_r: 0.0
+
+  kp_p: 6500
+  ki_p: 0.005
+  kd_p: 0.0001
+  kf_p: 0.0
+
+  kp_y: 4000                                          
+  ki_y: 0.0
+  kd_y: 0.0
+  kf_y: 0.0
+
+rat:
+  kp_r: 6500
+  ki_r: 0.01
+  kd_r: 0.0
+  kf_r: 0.0
+
+  kp_p: 6500
+  ki_p: 0.01
+  kd_p: 0.0
+  kf_p: 0.0
+
+  kp_y: 5000.0
+  ki_y: 0.0
+  kd_y: 0.0
+  kf_y: 0.0
+
+pos:
+  kp_x: 1.0
+  ki_x: 0.001
+  kd_x: 0.0
+  kf_x: 0.0
+
+  kp_y: 1.0
+  ki_y: 0.001
+  kd_y: 0.0
+  kf_y: 0.0
+
+  kp_t: 1.0
+  ki_t: 0.001
+  kd_t: 0.0
+  kf_t: 0.0
+
+pid_exec_freq: 60
+
+# =========== drone_param =============
+# see in assets/drone_urdf/cf2x.urdf
+thrust_compensate: 0.7
+weight: 0.5   
+motor_inertia: 2.6e-07
+g: 9.81   
+kf: 3.16e-10
+base_rpm: 62293.9641914   # equal to (m*g/4*kf)^0.5
+inertia_matrix:       # no use
+  - [0.0014, 0.0,     0.0,     0.0]
+  - [0.0,    0.0014,  0.0,     0.0]
+  - [0.0,    0.0,     0.00217, 0.0]
+  - [0.0,    0.0,     0.0,     1.0]
+
+# =========== mavlink_param =============
+USB_path: "/dev/ttyUSB0"
+baudrate: 2000000
+
+ARM:       "ch5"
+ANGLE:     "ch6"
+OFFBOARD:  "ch8"
+YAW:       "ch3" 
+PITCH:     "ch2"
+ROLL:      "ch1"
+THRUTTLE:  "ch4"   
+
+# =========== odom_param =============
+gyro_update_freq: 8000
+att_update_freq: 100
+acc_update_freqL: 1000

src/env/gs_env/sim/controllers/drone_controller/config/rc_FPV_eval/genesis_env.yaml

@@ -0,0 +1,29 @@
+
+# ========= genesis =========
+num_envs: 1     
+drone_num: 1
+dt: 0.01
+max_vis_FPS: 30 
+max_dis_num: 5
+cam_quat: [-0.455, -0.542, 0.542, 0.455]  # FPV camera, 70.0 degree, show in https://quaternions.online/
+cam_pos: [0.02, 0.0, 0.04]
+cam_res: [16, 12]                         # width, height
+drone_init_pos: [0.0, 0.0, 0.01]           # Initial position of the drone
+use_FPV_camera: True
+viewer_follow_drone: False
+load_map: False
+use_rc: False
+show_viewer: True
+render_cam: False
+controller: "angle"
+vis_waypoints: True
+map: "gates"
+min_dis: 0.8
+map_width: 3
+map_length: 3
+
+
+
+
+
+

src/env/gs_env/sim/controllers/drone_controller/drone_urdf/body.mtl

@@ -0,0 +1,6 @@
+newmtl material_0
+Ka 1 0 0
+Kd 0.6 0.6 0.6
+Ks 0.8 0.8 0.8
+Ns 250
+d 1