├── .gitignore ├── .ipynb_checkpoints └── testImg-checkpoint.ipynb ├── .vscode └── settings.json ├── CamDown.py ├── CommonFunctions.py ├── Doc ├── Images │ ├── QuadRotorPlus.png │ ├── QuadRotorX.png │ ├── dir_spyder.png │ ├── dir_spyderSelect.png │ ├── dir_spyder_set.png │ ├── equ_gps.gif │ ├── equ_imu.gif │ ├── sensors_3d.png │ ├── sensors_att.png │ ├── sensors_pos.png │ ├── spyder_effect.png │ ├── spyder_qt5set1.png │ ├── spyder_qt5set2.png │ ├── test_2d.png │ └── test_3d.png └── SensorSystem.md ├── LICENSE ├── MemoryStore.py ├── ModuleTest ├── dynamic_actuator.m ├── dynamic_basic.m ├── rk4.m └── testResult.m ├── Paper ├── Modeling and Analysis of Temperature Effect on MEMS Gyroscope.pdf └── NEO-M8-FW3_DataSheet_(UBX-15031086).pdf ├── QuadrotorFlyGui.py ├── QuadrotorFlyModel.py ├── QuadrotorFlyTest.py ├── README.md ├── SensorBase.py ├── SensorCompass.py ├── SensorGps.py ├── SensorImu.py ├── StateEstimator.py ├── Test ├── Test_full.py ├── Test_fullWithCamdown.py ├── Test_fullWithSensor.py └── Test_simple.py ├── UuvModel.py └── testImg.ipynb /.gitignore: -------------------------------------------------------------------------------- 1 | *.asv 2 | /__pycache__/ 3 | /.idea/ 4 | /weight/ 5 | /logs/ 6 | /__pycache__/ 7 | /Data/ -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.pythonPath": "d:\\Anaconda3\\envs\\cpu2\\python.exe" 3 | } -------------------------------------------------------------------------------- /CamDown.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """'used for manage and generate cam data with set ground-image. 4 | it is a virtual camera 5 | 6 | By xiaobo 7 | Contact linxiaobo110@gmail.com 8 | Created on 十一月 21 20:29 2019 9 | """ 10 | 11 | # Copyright (C) 12 | # 13 | # This file is part of quadrotorfly 14 | # 15 | # GWpy is free software: you can redistribute it and/or modify 16 | # it under the terms of the GNU General Public License as published by 17 | # the Free Software Foundation, either version 3 of the License, or 18 | # (at your option) any later version. 19 | # 20 | # GWpy is distributed in the hope that it will be useful, 21 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 22 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 23 | # GNU General Public License for more details. 24 | # 25 | # You should have received a copy of the GNU General Public License 26 | # along with GWpy. If not, see . 27 | 28 | 29 | import numpy as np 30 | import CommonFunctions as Cf 31 | import cv2 32 | from enum import Enum 33 | import enum 34 | from numba import jit 35 | 36 | """ 37 | ******************************************************************************************************** 38 | **------------------------------------------------------------------------------------------------------- 39 | ** Compiler : python 3.6 40 | ** Module Name: CamDown 41 | ** Module Date: 2019/11/21 42 | ** Module Auth: xiaobo 43 | ** Version : V0.1 44 | ** Description: a virtual camera looking down 45 | **------------------------------------------------------------------------------------------------------- 46 | ** Reversion : 47 | ** Modified By: 48 | ** Date : 49 | ** Content : 50 | ** Notes : 51 | ********************************************************************************************************/ 52 | """ 53 | 54 | 55 | class CamDownPara(Enum): 56 | Render_Mode_Mem = enum.auto() 57 | Render_Mode_Cpu = enum.auto() 58 | Render_Mode_Gpu = enum.auto() 59 | 60 | 61 | class CamDown(object): 62 | def __init__(self, img_horizon=400, img_vertical=400, img_depth=3, 63 | sensor_horizon=4., sensor_vertical=4., cam_focal=2.36, 64 | ground_img_path='./Data/groundImgWood.jpg', 65 | small_ground_img_path='./Data/groundImgSmall.jpg', 66 | small_land_img_path='./Data/landingMark.jpg', 67 | render_mode=CamDownPara.Render_Mode_Mem): 68 | """ 69 | ****************** horizon **************** 70 | * 71 | v 72 | e 73 | r 74 | t 75 | i 76 | c 77 | a 78 | l 79 | ******************************************** 80 | :param img_horizon: the num of vertical pixes of the image which is generated by sensor 81 | :param img_vertical: the num of horizon pixes of the image which is generated by sensor 82 | :param img_depth: the num of chanel the image, i.e. rgb is 3 83 | :param sensor_horizon: mm, the height of the active area of the sensor 84 | :param sensor_vertical: mm, the width of the active area of the sensor 85 | :param cam_focal: mm, the focal of the lens of the camera 86 | """ 87 | self.imgHorizon = img_horizon 88 | self.imgVertical = img_vertical 89 | self.imgDepth = img_depth 90 | self.skx = sensor_horizon * 1. / img_horizon # skx is sensor_k_x 91 | self.sky = sensor_vertical * 1. / img_vertical # skx is sensor_k_y 92 | self.sx0 = sensor_horizon * 0.5 93 | self.sy0 = sensor_vertical * 0.5 94 | self.camFocal = cam_focal 95 | self.axCamImgArr = np.zeros([self.imgVertical * self.imgHorizon, 3]) 96 | self.pixCamImg = np.zeros([self.imgVertical, self.imgHorizon, self.imgDepth], dtype=np.uint8) 97 | self.groundImgPath = ground_img_path 98 | self.groundImg = None 99 | 100 | # small image 101 | self.smallGroundImgPath = small_ground_img_path 102 | self.smallLandingImgPath = small_land_img_path 103 | self.smallGroundImg = None 104 | self.smallLandingImg = None 105 | self.smallImgHorizon = 7854 106 | self.smallImgVertical = 3490 107 | self.smallLandingImgHos = 315 108 | self.smallLandingImgVet = 315 109 | self.renderMode = render_mode 110 | 111 | # init the array 112 | for ii in range(self.imgVertical): 113 | for j in range(self.imgHorizon): 114 | self.axCamImgArr[ii * self.imgHorizon + j, :] = [ii, j, 1] 115 | 116 | def load_ground_img(self): 117 | if self.renderMode == CamDownPara.Render_Mode_Mem: 118 | if self.groundImg is not None: 119 | del self.groundImg 120 | else: 121 | self.groundImg = cv2.imread(self.groundImgPath) 122 | elif self.renderMode == CamDownPara.Render_Mode_Cpu: 123 | if self.smallGroundImg is not None: 124 | del self.smallGroundImg 125 | else: 126 | self.smallGroundImg = cv2.imread(self.smallGroundImgPath) 127 | # if read success, get size of image 128 | if self.smallGroundImg is not None: 129 | self.smallImgVertical, self.smallImgHorizon = self.smallGroundImg.shape 130 | elif self.renderMode == CamDownPara.Render_Mode_Gpu: 131 | if self.smallGroundImg is not None: 132 | del self.smallGroundImg 133 | else: 134 | self.smallGroundImg = cv2.imread(self.smallGroundImgPath) 135 | self.smallLandingImg = cv2.imread(self.smallLandingImgPath) 136 | # if read success, get size of image 137 | if self.smallGroundImg is not None: 138 | self.smallImgVertical, self.smallImgHorizon, _ = self.smallGroundImg.shape 139 | if self.smallLandingImg is not None: 140 | self.smallLandingImgVet, self.smallLandingImgHos, _ = self.smallLandingImg.shape 141 | 142 | def get_img_by_state(self, pos, att): 143 | m_img2sensor = np.array([[self.skx, 0, self.sx0], 144 | [0, -self.sky, -self.sy0], 145 | [0, 0, 1]]) 146 | m_sensor2cam = np.array([[pos[2] / self.camFocal, 0, self.imgVertical / 2], 147 | [0, pos[2] / self.camFocal, -self.imgHorizon / 2], 148 | [0, 0, 1]]) 149 | m_cam2world = Cf.get_rotation_matrix(att) 150 | m_cam2world[0:2, 2] = pos[0:2] 151 | m_cam2world[2, :] = np.array([0, 0, 1]) 152 | m_trans = m_cam2world.dot(m_sensor2cam.dot(m_img2sensor)) 153 | if self.renderMode == CamDownPara.Render_Mode_Mem: 154 | ax_real = m_trans.dot(self.axCamImgArr.transpose()).transpose() 155 | for ii in range(self.imgVertical): 156 | for j in range(self.imgHorizon): 157 | self.pixCamImg[ii, j, :] = self.groundImg[int(ax_real[ii * self.imgHorizon + j, 0]), 158 | int(ax_real[ii * self.imgHorizon + j, 1] + 10000), :] 159 | elif self.renderMode == CamDownPara.Render_Mode_Cpu: 160 | ax_real = m_trans.dot(self.axCamImgArr.transpose()).transpose() 161 | for ii in range(self.imgVertical): 162 | for j in range(self.imgHorizon): 163 | ax_vertical = int(ax_real[ii * self.imgHorizon + j, 0]) % self.smallImgVertical 164 | ax_horizon = int(ax_real[ii * self.imgHorizon + j, 1] + 10000) % self.smallImgHorizon 165 | self.pixCamImg[ii, j, :] = self.smallGroundImg[ax_vertical, ax_horizon, :] 166 | 167 | elif self.renderMode == CamDownPara.Render_Mode_Gpu: 168 | ax_real = m_trans.dot(self.axCamImgArr.transpose()).transpose() 169 | accelerate_img_mapping_gpu(ax_real, self.smallGroundImg, self.smallLandingImg, self.pixCamImg, 170 | self.imgVertical, self.imgHorizon, self.smallImgHorizon, self.smallImgVertical, 171 | self.smallLandingImgHos, self.smallLandingImgVet) 172 | return self.pixCamImg 173 | 174 | 175 | @jit(nopython=True) 176 | def accelerate_img_mapping_gpu(m_ax_real, m_img_small, m_img_landing, m_img_result, 177 | img_vertical, img_horizon, small_horizon, small_vertical, land_horizon, land_vertical): 178 | """ 179 | # 用来加速映射计算的函数 180 | :param m_ax_real: 映射后的座标 181 | :param m_img_small: 贴图，小图片 182 | :param m_img_result: 目标图像 183 | :param m_img_landing 184 | :param img_vertical: 目标图像高度 185 | :param img_horizon: 目标图像宽度 186 | :param small_horizon: 贴图宽度 187 | :param small_vertical: 贴图高度 188 | :param land_horizon: 189 | :param land_vertical 190 | :return: 191 | """ 192 | land_hos_max = 5000 + land_horizon 193 | land_vet_min = 5000 - land_vertical 194 | for ii in range(img_vertical): 195 | for j in range(img_horizon): 196 | ax_vertical = int(m_ax_real[ii * img_horizon + j, 0]) 197 | ax_horizon = int(m_ax_real[ii * img_horizon + j, 1] + 10000) 198 | if (ax_horizon > 5000) and (ax_horizon < land_hos_max) and (ax_vertical > land_vet_min) \ 199 | and (ax_vertical < 5000): 200 | ax_horizon = ax_horizon - 5000 201 | ax_vertical = ax_vertical - land_vet_min 202 | m_img_result[ii, j, :] = m_img_landing[ax_vertical, ax_horizon, :] 203 | else: 204 | ax_vertical = ax_vertical % small_vertical 205 | ax_horizon = ax_horizon % small_horizon 206 | m_img_result[ii, j, :] = m_img_small[ax_vertical, ax_horizon, :] 207 | return m_img_result 208 | 209 | 210 | if __name__ == '__main__': 211 | " used for testing this module" 212 | testFlag = 3 213 | 214 | if testFlag == 1: 215 | cam1 = CamDown() 216 | print('init completed!') 217 | cam1.load_ground_img() 218 | print('Load img completed!') 219 | pos_0 = np.array([4251.97508977, -5843.00458236, 227.35483937]) 220 | att_0 = np.array([-0.13647458, -0.1990263, 0.27836947]) 221 | img1 = cam1.get_img_by_state(pos_0, att_0) 222 | import matplotlib.pyplot as plt 223 | plt.imshow(cam1.pixCamImg / 255) 224 | cv2.imwrite('Data/test.jpg', img1) 225 | 226 | elif testFlag == 2: 227 | import matplotlib.pyplot as plt 228 | # import matplotlib as mpl 229 | from QuadrotorFlyModel import QuadModel, QuadSimOpt, QuadParas, StructureType, SimInitType 230 | import MemoryStore 231 | D2R = np.pi / 180 232 | 233 | print("PID controller test: ") 234 | uavPara = QuadParas(structure_type=StructureType.quad_x) 235 | simPara = QuadSimOpt(init_mode=SimInitType.fixed, enable_sensor_sys=False, 236 | init_att=np.array([10., -10., 30]), init_pos=np.array([5, -5, 0])) 237 | quad1 = QuadModel(uavPara, simPara) 238 | record = MemoryStore.DataRecord() 239 | record.clear() 240 | step_cnt = 0 241 | 242 | # init the camera 243 | cam1 = CamDown() 244 | cam1.load_ground_img() 245 | print('Load img completed!') 246 | fourcc = cv2.VideoWriter_fourcc(*'MJPG') 247 | out1 = cv2.VideoWriter('Data/img/test.avi', fourcc, 1 / quad1.uavPara.ts, (cam1.imgVertical, cam1.imgHorizon)) 248 | for i in range(1000): 249 | ref = np.array([0., 0., 1., 0.]) 250 | stateTemp = quad1.observe() 251 | # get image 252 | pos_0 = quad1.position * 1000 253 | att_0 = quad1.attitude 254 | img1 = cam1.get_img_by_state(pos_0, att_0) 255 | # file_name = 'Data/img/test_' + str(i) + '.jpg' 256 | # cv2.imwrite(file_name, img1) 257 | out1.write(img1) 258 | 259 | action2, oil = quad1.get_controller_pid(stateTemp, ref) 260 | print('action: ', action2) 261 | action2 = np.clip(action2, 0.1, 0.9) 262 | quad1.step(action2) 263 | record.buffer_append((stateTemp, action2)) 264 | step_cnt = step_cnt + 1 265 | record.episode_append() 266 | out1.release() 267 | 268 | print('Quadrotor structure type', quad1.uavPara.structureType) 269 | # quad1.reset_states() 270 | print('Quadrotor get reward:', quad1.get_reward()) 271 | data = record.get_episode_buffer() 272 | bs = data[0] 273 | ba = data[1] 274 | t = range(0, record.count) 275 | # mpl.style.use('seaborn') 276 | fig1 = plt.figure(1) 277 | plt.clf() 278 | plt.subplot(3, 1, 1) 279 | plt.plot(t, bs[t, 6] / D2R, label='roll') 280 | plt.plot(t, bs[t, 7] / D2R, label='pitch') 281 | plt.plot(t, bs[t, 8] / D2R, label='yaw') 282 | plt.ylabel('Attitude $(\circ)$', fontsize=15) 283 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 284 | plt.subplot(3, 1, 2) 285 | plt.plot(t, bs[t, 0], label='x') 286 | plt.plot(t, bs[t, 1], label='y') 287 | plt.ylabel('Position (m)', fontsize=15) 288 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 289 | plt.subplot(3, 1, 3) 290 | plt.plot(t, bs[t, 2], label='z') 291 | plt.ylabel('Altitude (m)', fontsize=15) 292 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 293 | plt.show() 294 | # performance test 295 | elif testFlag == 3: 296 | import matplotlib.pyplot as plt 297 | # import matplotlib as mpl 298 | from QuadrotorFlyModel import QuadModel, QuadSimOpt, QuadParas, StructureType, SimInitType 299 | import MemoryStore 300 | import time 301 | D2R = np.pi / 180 302 | video_write_flag = True 303 | 304 | print("PID controller test: ") 305 | uavPara = QuadParas(structure_type=StructureType.quad_x) 306 | simPara = QuadSimOpt(init_mode=SimInitType.fixed, enable_sensor_sys=False, 307 | init_att=np.array([0., 0., 0]), init_pos=np.array([0, -0, 0])) 308 | quad1 = QuadModel(uavPara, simPara) 309 | record = MemoryStore.DataRecord() 310 | record.clear() 311 | step_cnt = 0 312 | 313 | # init the camera 314 | cam1 = CamDown(render_mode=CamDownPara.Render_Mode_Gpu) 315 | cam1.load_ground_img() 316 | print('Load img completed!') 317 | if video_write_flag: 318 | v_format = cv2.VideoWriter_fourcc(*'MJPG') 319 | out1 = cv2.VideoWriter('Data/img/test.avi', v_format, 1 / quad1.uavPara.ts, (cam1.imgVertical, cam1.imgHorizon)) 320 | for i in range(1000): 321 | if i == 0: 322 | time_start = time.time() 323 | ref = np.array([0., 0., 3., 0.]) 324 | stateTemp = quad1.observe() 325 | # get image 326 | pos_0 = quad1.position * 1000 327 | att_0 = quad1.attitude 328 | img1 = cam1.get_img_by_state(pos_0, att_0) 329 | # file_name = 'Data/img/test_' + str(i) + '.jpg' 330 | # cv2.imwrite(file_name, img1) 331 | if video_write_flag: 332 | out1.write(img1) 333 | 334 | action2, oil = quad1.get_controller_pid(stateTemp, ref) 335 | print('action: ', action2) 336 | action2 = np.clip(action2, 0.1, 0.9) 337 | quad1.step(action2) 338 | record.buffer_append((stateTemp, action2)) 339 | step_cnt = step_cnt + 1 340 | time_end = time.time() 341 | print('time cost:', str(time_end - time_start)) 342 | record.episode_append() 343 | if video_write_flag: 344 | out1.release() 345 | 346 | print('Quadrotor structure type', quad1.uavPara.structureType) 347 | # quad1.reset_states() 348 | print('Quadrotor get reward:', quad1.get_reward()) 349 | data = record.get_episode_buffer() 350 | bs = data[0] 351 | ba = data[1] 352 | t = range(0, record.count) 353 | # mpl.style.use('seaborn') 354 | fig1 = plt.figure(1) 355 | plt.clf() 356 | plt.subplot(3, 1, 1) 357 | plt.plot(t, bs[t, 6] / D2R, label='roll') 358 | plt.plot(t, bs[t, 7] / D2R, label='pitch') 359 | plt.plot(t, bs[t, 8] / D2R, label='yaw') 360 | plt.ylabel('Attitude $(\circ)$', fontsize=15) 361 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 362 | plt.subplot(3, 1, 2) 363 | plt.plot(t, bs[t, 0], label='x') 364 | plt.plot(t, bs[t, 1], label='y') 365 | plt.ylabel('Position (m)', fontsize=15) 366 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 367 | plt.subplot(3, 1, 3) 368 | plt.plot(t, bs[t, 2], label='z') 369 | plt.ylabel('Altitude (m)', fontsize=15) 370 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 371 | plt.show() 372 | -------------------------------------------------------------------------------- /CommonFunctions.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """This file implement many common methods and constant 4 | 5 | By xiaobo 6 | Contact linxiaobo110@gmail.com 7 | Created on 五月 05 11:03 2019 8 | """ 9 | 10 | # Copyright (C) 11 | # 12 | # This file is part of QuadrotorFly 13 | # 14 | # GWpy is free software: you can redistribute it and/or modify 15 | # it under the terms of the GNU General Public License as published by 16 | # the Free Software Foundation, either version 3 of the License, or 17 | # (at your option) any later version. 18 | # 19 | # GWpy is distributed in the hope that it will be useful, 20 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 21 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 22 | # GNU General Public License for more details. 23 | # 24 | # You should have received a copy of the GNU General Public License 25 | # along with GWpy. If not, see . 26 | 27 | 28 | import numpy as np 29 | import warnings 30 | 31 | """ 32 | ******************************************************************************************************** 33 | **------------------------------------------------------------------------------------------------------- 34 | ** Compiler : python 3.6 35 | ** Module Name: CommonFunctions 36 | ** Module Date: 2019/5/5 37 | ** Module Auth: xiaobo 38 | ** Version : V0.1 39 | ** Description: create the file 40 | **------------------------------------------------------------------------------------------------------- 41 | ** Reversion : 42 | ** Modified By: 43 | ** Date : 44 | ** Content : 45 | ** Notes : 46 | ********************************************************************************************************/ 47 | """ 48 | 49 | 50 | D2R = np.pi / 180 51 | 52 | 53 | class QuadrotorFlyError(Exception): 54 | """General exception of QuadrotorFly""" 55 | def __init__(self, error_info): 56 | super().__init__(self) 57 | self.errorInfo = error_info 58 | warnings.warn("QuadrotorFly Error:" + self.errorInfo, DeprecationWarning) 59 | 60 | def __str__(self): 61 | return "QuadrotorFly Error:" + self.errorInfo 62 | 63 | 64 | def get_rotation_matrix(att): 65 | cos_att = np.cos(att) 66 | sin_att = np.sin(att) 67 | 68 | rotation_x = np.array([[1, 0, 0], [0, cos_att[0], -sin_att[0]], [0, sin_att[0], cos_att[0]]]) 69 | rotation_y = np.array([[cos_att[1], 0, sin_att[1]], [0, 1, 0], [-sin_att[1], 0, cos_att[1]]]) 70 | rotation_z = np.array([[cos_att[2], -sin_att[2], 0], [sin_att[2], cos_att[2], 0], [0, 0, 1]]) 71 | rotation_matrix = np.dot(rotation_z, np.dot(rotation_y, rotation_x)) 72 | 73 | return rotation_matrix 74 | 75 | 76 | def get_rotation_inv_matrix(att): 77 | att = -att 78 | cos_att = np.cos(att) 79 | sin_att = np.sin(att) 80 | 81 | rotation_x = np.array([[1, 0, 0], [0, cos_att[0], -sin_att[0]], [0, sin_att[0], cos_att[0]]]) 82 | rotation_y = np.array([[cos_att[1], 0, sin_att[1]], [0, 1, 0], [-sin_att[1], 0, cos_att[1]]]) 83 | rotation_z = np.array([[cos_att[2], -sin_att[2], 0], [sin_att[2], cos_att[2], 0], [0, 0, 1]]) 84 | rotation_matrix = np.dot(rotation_x, np.dot(rotation_y, rotation_z)) 85 | 86 | return rotation_matrix 87 | 88 | 89 | if __name__ == '__main__': 90 | try: 91 | raise QuadrotorFlyError('Quadrotor Exception Test') 92 | except QuadrotorFlyError as e: 93 | print(e) 94 | -------------------------------------------------------------------------------- /Doc/Images/QuadRotorPlus.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linxiaobo110/QuadrotorFly/147c14aa1c166a5bc509a0459ad18b6e3a354cf5/Doc/Images/QuadRotorPlus.png -------------------------------------------------------------------------------- /Doc/Images/QuadRotorX.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linxiaobo110/QuadrotorFly/147c14aa1c166a5bc509a0459ad18b6e3a354cf5/Doc/Images/QuadRotorX.png -------------------------------------------------------------------------------- /Doc/Images/dir_spyder.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linxiaobo110/QuadrotorFly/147c14aa1c166a5bc509a0459ad18b6e3a354cf5/Doc/Images/dir_spyder.png -------------------------------------------------------------------------------- /Doc/Images/dir_spyderSelect.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linxiaobo110/QuadrotorFly/147c14aa1c166a5bc509a0459ad18b6e3a354cf5/Doc/Images/dir_spyderSelect.png -------------------------------------------------------------------------------- /Doc/Images/dir_spyder_set.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linxiaobo110/QuadrotorFly/147c14aa1c166a5bc509a0459ad18b6e3a354cf5/Doc/Images/dir_spyder_set.png -------------------------------------------------------------------------------- /Doc/Images/equ_gps.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linxiaobo110/QuadrotorFly/147c14aa1c166a5bc509a0459ad18b6e3a354cf5/Doc/Images/equ_gps.gif -------------------------------------------------------------------------------- /Doc/Images/equ_imu.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linxiaobo110/QuadrotorFly/147c14aa1c166a5bc509a0459ad18b6e3a354cf5/Doc/Images/equ_imu.gif -------------------------------------------------------------------------------- /Doc/Images/sensors_3d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linxiaobo110/QuadrotorFly/147c14aa1c166a5bc509a0459ad18b6e3a354cf5/Doc/Images/sensors_3d.png -------------------------------------------------------------------------------- /Doc/Images/sensors_att.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linxiaobo110/QuadrotorFly/147c14aa1c166a5bc509a0459ad18b6e3a354cf5/Doc/Images/sensors_att.png -------------------------------------------------------------------------------- /Doc/Images/sensors_pos.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linxiaobo110/QuadrotorFly/147c14aa1c166a5bc509a0459ad18b6e3a354cf5/Doc/Images/sensors_pos.png -------------------------------------------------------------------------------- /Doc/Images/spyder_effect.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linxiaobo110/QuadrotorFly/147c14aa1c166a5bc509a0459ad18b6e3a354cf5/Doc/Images/spyder_effect.png -------------------------------------------------------------------------------- /Doc/Images/spyder_qt5set1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linxiaobo110/QuadrotorFly/147c14aa1c166a5bc509a0459ad18b6e3a354cf5/Doc/Images/spyder_qt5set1.png -------------------------------------------------------------------------------- /Doc/Images/spyder_qt5set2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linxiaobo110/QuadrotorFly/147c14aa1c166a5bc509a0459ad18b6e3a354cf5/Doc/Images/spyder_qt5set2.png -------------------------------------------------------------------------------- /Doc/Images/test_2d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linxiaobo110/QuadrotorFly/147c14aa1c166a5bc509a0459ad18b6e3a354cf5/Doc/Images/test_2d.png -------------------------------------------------------------------------------- /Doc/Images/test_3d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linxiaobo110/QuadrotorFly/147c14aa1c166a5bc509a0459ad18b6e3a354cf5/Doc/Images/test_3d.png -------------------------------------------------------------------------------- /Doc/SensorSystem.md: -------------------------------------------------------------------------------- 1 | # v0.2 更新的内容 2 | 1. 加入了传感器子系统（默认不使能），包含IMU（陀螺仪+加速度计），GPS，compass（磁力计）， 3 | 1. 加入了一个简单的卡尔曼滤波估计器。 4 | 1. 加入了时间系统，即无人机有一个自己的内部时间，每次迭代会累计采样周期，reset回导致时间归零。 5 | 2. 参考例子在StateEstimator.py和Test_fullWithSensor.py 6 | 3. 可以通过继承SensorBase来完成自定义传感器, 通过继承StateEstimatorBase来实现自定义的位姿估计器 7 | 8 | 每个传感器考虑不同的特性，主要内容如下： 9 | * IMU考虑噪声和零飘，100Hz的更新频率（与系统默认相同） 10 | * GPS考虑低采样率（10Hz）、噪声、启动延时（1s）和时延（100ms，即latency） 11 | * 磁力计考虑噪声和偏置，坐标系假设为东北天 12 | 13 | # 使能传感器系统后的注意事项 14 | 下面假设quad1是QuadrotorFlyModel类的一个实例，并且这个类在实例化时使能了传感器系统，即 15 | ```python 16 | q1 = Qfm.QuadModel(Qfm.QuadParas(), Qfm.QuadSimOpt(enable_sensor_sys=True)) 17 | ``` 18 | 19 | ## 及时获取时间戳 20 | 不同的传感器采样时刻和启动的时长是不一样的，因此需要启动时要注意延时一定的时间。 21 | 使用quad1.ts来获取当前时间戳 22 | 23 | ## 模型调用接口的返回值发生变化 24 | 使能传感器系统后，quad1.step函数和observe函数返回的是传感器信息，传感器信息采用字典类型（dict）储存，格式如下： 25 | 26 | ```python 27 | print(quad1.observe()) 28 | {'imu': (True, array([ 5.78443156e-02, -3.81653176e-01, -9.50556019e+00, 1.75481697e-03, 29 | -2.82903321e-02, 6.36374403e-02])), 30 | 'gps': (False, array([ 1.00240709, -0.24003651, 0.64546363])), 31 | 'compass': (True, array([ 7.89924613, 32.83536768, -93.90494583]))} 32 | ``` 33 | 34 | 其中，字典的键名是传感器的名称（imu, gps, compass)，值的第一个参数是传感器这一刻是否更新（True代表更新，False反之），第二个值是传感器数据，定义分别如下： 35 | 36 | | 名称|imu|gps|compass| 37 | | --------- | -------- | -----: | --: | 38 | |参数1-3|加速度(m/(s^2))|位置(m)|磁场强度(T) | 39 | |参数4-6| 角速度（rad/s)|x|x| 40 | 41 | 注意，这些传感器的值是带有噪声和零飘的，并且零飘是传感器初始化的时候随机的， 可以通过quad1.imu0.gyroBias和quad1.imu0.accBias获取imu的零飘。假设磁力计的北边和正北大概差16度。 42 | 43 | ## 真实状态和估计状态存在差异 44 | 控制的目的是稳定系统的真实状态，但是只能观测到传感器返回的信息，通过估计器（观测器、滤波器之类的名称）解算得到系统状态与真实的状态会有一定的差异（受估计器性能影响）。但是可以通过quad1.state（这是一个属性化的函数）获得无人机的真实状态，保存数据的时候注意两组数据都需要保存，方便对比。 45 | 46 | # 带传感器的完整实验 47 | ```python 48 | # 弧度到角度的转换参数 49 | D2R = Qfm.D2R 50 | # 仿真参数设置 51 | simPara = Qfm.QuadSimOpt( 52 | # 初值重置方式（随机或者固定）；姿态初值参数（随机就是上限，固定就是设定值）；位置初值参数（同上） 53 | init_mode=Qfm.SimInitType.rand, init_att=np.array([5., 5., 5.]), init_pos=np.array([1., 1., 1.]), 54 | # 仿真运行的最大位置，最大速度，最大姿态角（角度，不是弧度注意），最大角速度（角度每秒） 55 | max_position=8, max_velocity=8, max_attitude=180, max_angular=200, 56 | # 系统噪声，分别在位置环和速度环 57 | sysnoise_bound_pos=0, sysnoise_bound_att=0, 58 | #执行器模式，简单模式没有电机动力学， 59 | actuator_mode=Qfm.ActuatorMode.dynamic 60 | ) 61 | # 无人机参数设置，可以为不同的无人机设置不同的参数 62 | uavPara = Qfm.QuadParas( 63 | # 重力加速度；采样时间；机型plus或者x 64 | g=9.8, tim_sample=0.01, structure_type=Qfm.StructureType.quad_plus, 65 | # 无人机臂长（米）；质量（千克）；飞机绕三个轴的转动惯量（千克·平方米） 66 | uav_l=0.45, uav_m=1.5, uav_ixx=1.75e-2, uav_iyy=1.75e-2, uav_izz=3.18e-2, 67 | # 螺旋桨推力系数（牛每平方（弧度每秒）），螺旋桨扭力系数（牛·米每平方（弧度每秒）），旋翼转动惯量， 68 | rotor_ct=1.11e-5, rotor_cm=1.49e-7, rotor_i=9.9e-5, 69 | # 电机转速比例参数（度每秒），电机转速偏置参数（度每秒），电机相应时间（秒） 70 | rotor_cr=646, rotor_wb=166, rotor_t=1.36e-2 71 | ) 72 | 73 | # 使用参数新建无人机 74 | quad1 = Qfm.QuadModel(uavPara, simPara) 75 | # 新建卡尔曼滤波器（已实现的，作为参考） 76 | filter1 = StateEstimator.KalmanFilterSimple() 77 | # 新建GUI 78 | gui = Qfg.QuadrotorFlyGui([quad1]) 79 | # 新建存储对象，用于储存过程数据 80 | record = MemoryStore.DataRecord() 81 | 82 | # 复位 83 | quad1.reset_states() 84 | record.clear() 85 | filter1.reset(quad1.state) 86 | 87 | # 设置imu零飘，这个实际是拿不到的，这里简化了 88 | filter1.gyroBias = quad1.imu0.gyroBias 89 | filter1.accBias = quad1.imu0.accBias 90 | print(filter1.gyroBias, filter1.accBias) 91 | 92 | # 设置仿真时间 93 | t = np.arange(0, 30, 0.01) 94 | ii_len = len(t) 95 | 96 | # 仿真过程 97 | for ii in range(ii_len): 98 | # 等待传感器启动 99 | if ii < 100: 100 | # 获取传感器数据 101 | sensor_data1 = quad1.observe() 102 | # 在传感器启动前使用系统状态直接获取控制器值 103 | _, oil = quad1.get_controller_pid(quad1.state) 104 | # 其实只需要油门维持稳定 105 | action = np.ones(4) * oil 106 | # 执行一步 107 | quad1.step(action) 108 | # 把获取到的传感器数据给估计器 109 | state_est = filter1.update(sensor_data1, quad1.ts) 110 | # 储存数据 111 | record.buffer_append((quad1.state, state_est)) 112 | else: 113 | # 获取传感器数据 114 | sensor_data1 = quad1.observe() 115 | # 使用估计器估计的状态来计算控制量而非真实的状态 116 | action, oil = quad1.get_controller_pid(filter1.state, np.array([0, 0, 2, 0])) 117 | # 执行一步 118 | quad1.step(action) 119 | # 把获取到的传感器数据给估计器 120 | state_est = filter1.update(sensor_data1, quad1.ts) 121 | # 储存数据 122 | record.buffer_append((quad1.state, state_est)) 123 | # 这个不能省去 124 | record.episode_append() 125 | 126 | # 输出结果 127 | # 1. 获取最新的数据 128 | data = record.get_episode_buffer() 129 | # 1.1 真实的状态 130 | bs_r = data[0] 131 | # 1.2 估计的状态 132 | bs_e = data[1] 133 | # 2. 产生时间序列 134 | t = range(0, record.count) 135 | # 3. 画图 136 | # 3.1 画姿态和角速度 137 | fig1 = plt.figure(2) 138 | yLabelList = ['roll', 'pitch', 'yaw', 'rate_roll', 'rate_pit', 'rate_yaw'] 139 | for ii in range(6): 140 | plt.subplot(6, 1, ii + 1) 141 | plt.plot(t, bs_r[:, 6 + ii] / D2R, '-b', label='real') 142 | plt.plot(t, bs_e[:, 6 + ii] / D2R, '-g', label='est') 143 | plt.legend() 144 | plt.ylabel(yLabelList[ii]) 145 | 146 | # 3.2 画位置和速度 147 | yLabelList = ['p_x', 'p_y', 'p_z', 'vel_x', 'vel_y', 'vel_z'] 148 | plt.figure(3) 149 | for ii in range(6): 150 | plt.subplot(6, 1, ii + 1) 151 | plt.plot(t, bs_r[:, ii], '-b', label='real') 152 | plt.plot(t, bs_e[:, ii], '-g', label='est') 153 | plt.legend() 154 | plt.ylabel(yLabelList[ii]) 155 | plt.show() 156 | print("Simulation finish!") 157 | ``` 158 | 159 | 结果 160 | 1. 从3d的效果图可以看出有一点抖动 161 | ![sensors_3d](Images/sensors_3d.png) 162 | 163 | 2. 位置的真实和估计 164 | ![sensors_pos](Images/sensors_pos.png) 165 | 166 | 3. 姿态的真实和估计 167 | ![sensors_att](Images/sensors_att.png) 168 | 169 | # 基本原理 170 | ## 传感器仿真的原理 171 | ### IMU考虑噪声和零飘，100Hz的更新频率（与系统默认相同） 172 | 173 | 陀螺仪参考的参数来自传感器MPU6500 174 | 175 | ![equ_imu](Images/equ_imu.gif) 176 | 177 | 178 | 其中下标带o的是传感器的输出，$\omega$是真实的角速度，$a_F$是真实的合外力造成的加速度，g是重力加速度，$R_{i2b}$是惯性系到机体系的转换矩阵， $b_g$和$b_a$分别是陀螺仪和加速度计的零飘，$w_g$和$w_a$分别是陀螺仪和加速度计的噪声，这里假设成白噪声。 179 | 180 | ### GPS考虑低采样率、噪声、启动延时和时延 181 | GPS默认的参数来自M8N，采样率为10H、精度为1米、启动延时为1s，时延（即latency)是100ms。时延和启动延时是不一样的，时延指由于采样过程或者传输过程导致采集的数据表征的是之前时刻的状态，启动延时就是启动的前1秒内没有响应，之后就有响应。 182 | 183 | ![equ_gps](Images/equ_gps.gif) 184 | 185 | 其中t代表连续的时间，$t_k$是采样的序列，$T_s$和$T_l$分别代表启动延时和时延，$w_p$是传感器噪声。 186 | 187 | ### 磁力计主要考虑低采样率 188 | 与imu类似，采样率只有imu的1/10，假设的惯性坐标系为东北天。 189 | 190 | 191 | 192 | ## 最简卡尔曼滤波器的原理 193 | 懒，下次再说吧，详细见代码。简单的说就是首先判断各个传感器是否更新，更新的话就运行相应的更新程序。在姿态估计上，陀螺仪当作过程更新，加速度计（就是测重力方向）和磁力计当作测量更新；在位置估计上，加速度计做过程更新，gps做测量更新。 -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /MemoryStore.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """The file used to implement the data store and replay 4 | 5 | By xiaobo 6 | Contact linxiaobo110@gmail.com 7 | Created on Wed Jan 17 10:40:44 2018 8 | """ 9 | 10 | # Copyright (C) 11 | # 12 | # This file is part of QuadrotorFly 13 | # 14 | # GWpy is free software: you can redistribute it and/or modify 15 | # it under the terms of the GNU General Public License as published by 16 | # the Free Software Foundation, either version 3 of the License, or 17 | # (at your option) any later version. 18 | # 19 | # GWpy is distributed in the hope that it will be useful, 20 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 21 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 22 | # GNU General Public License for more details. 23 | # 24 | # You should have received a copy of the GNU General Public License 25 | # along with GWpy. If not, see . 26 | 27 | 28 | from collections import deque 29 | import random 30 | import numpy as np 31 | 32 | """ 33 | ******************************************************************************************************** 34 | **------------------------------------------------------------------------------------------------------- 35 | ** Compiler : python 3.6 36 | ** Module Name: MemoryStore 37 | ** Module Date: 2018-04-17 38 | ** Module Auth: xiaobo 39 | ** Version : V0.1 40 | ** Description: create the module 41 | **------------------------------------------------------------------------------------------------------- 42 | ** Reversion : V0.2 43 | ** Modified By: xiaobo 44 | ** Date : 2019-4-25 45 | ** Content : rewrite the module, add note 46 | ** Notes : 47 | ********************************************************************************************************/ 48 | **------------------------------------------------------------------------------------------------------- 49 | ** Reversion : V0.3 50 | ** Modified By: xiaobo 51 | ** Date : 2019-5-20 52 | ** Content : modify the data record, compatible wit v0.2, store data each episode independently 53 | ** Notes : 54 | ********************************************************************************************************/ 55 | """ 56 | 57 | 58 | class ReplayBuffer(object): 59 | """ storing data in order replaying for train algorithm""" 60 | 61 | def __init__(self, buffer_size, random_seed=123): 62 | # size of minimize buffer is able to train 63 | self.buffer_size = buffer_size 64 | # counter for replay buffer 65 | self.count = 0 66 | # buffer, contain all data together 67 | self.buffer = deque() 68 | # used for random sampling 69 | random.seed(random_seed) 70 | # when count rise over the buffer_size, the train can begin 71 | self.isBufferFull = False 72 | # counter for episode 73 | self.episodeNum = 0 74 | # record the start position of each episode in buffer 75 | self.episodePos = deque() 76 | # record the sum rewards of steps for each episode 77 | self.episodeRewards = deque() 78 | 79 | def buffer_append(self, experience): 80 | """append data to buffer, should run each step after system update""" 81 | if self.count < self.buffer_size: 82 | self.buffer.append(experience) 83 | self.count += 1 84 | self.isBufferFull = False 85 | else: 86 | self.buffer.popleft() 87 | self.buffer.append(experience) 88 | self.isBufferFull = True 89 | 90 | def episode_append(self, rewards): 91 | self.episodeNum += 1 92 | self.episodePos.append(self.count) 93 | self.episodeRewards.append(rewards) 94 | 95 | def size(self): 96 | return self.count 97 | 98 | def buffer_sample_batch(self, batch_size): 99 | """sample a batch of data with size of batch_size""" 100 | if self.count < batch_size: 101 | batch = random.sample(self.buffer, self.count) 102 | else: 103 | batch = random.sample(self.buffer, batch_size) 104 | 105 | return batch 106 | 107 | def clear(self): 108 | self.buffer.clear() 109 | self.count = 0 110 | self.episodeNum = 0 111 | self.episodePos.clear() 112 | self.episodeRewards.clear() 113 | 114 | 115 | class DataRecord(object): 116 | """data record for show result""" 117 | def __init__(self, compatibility_mode=False): 118 | # new buffer, store data sepeartely with different episode, new in 0.3 119 | self.episodeList = list() 120 | self.bufferTemp = deque() 121 | self.compatibilityMode = compatibility_mode 122 | 123 | # counter for replay buffer 124 | self.count = 0 125 | 126 | # counter for episode 127 | self.episodeNum = 0 128 | 129 | # record the sum rewards of steps for each episode 130 | self.episodeRewards = deque() 131 | # record the average td error of steps for each episode 132 | self.episodeTdErr = deque() 133 | # record some sample of weights, once after episode 134 | self.episodeWeights = deque() 135 | # record some sample of weights, once each step, for observing the vary of weights 136 | self.weights = deque() 137 | 138 | if self.compatibilityMode: 139 | # buffer, contain all data together, discarded in 0.3 140 | self.buffer = deque() 141 | # record the start position of each episode in buffer, discarded in 0.3 142 | self.episodePos = deque() 143 | 144 | def buffer_append(self, experience, weights=0): 145 | """append data to buffer, should run each step after system update""" 146 | self.bufferTemp.append(experience) 147 | self.count += 1 148 | self.weights.append(weights) 149 | 150 | if self.compatibilityMode: 151 | self.buffer.append(experience) 152 | 153 | # if self.count < self.buffer_size: 154 | # self.buffer.append(experience) 155 | # self.count += 1 156 | # self.isBufferFull = False 157 | # else: 158 | # self.buffer.popleft() 159 | # self.buffer.append(experience) 160 | 161 | def episode_append(self, rewards=0, td_err=0, weights=0): 162 | """append data to episode buffer, should run each episode after episode finish""" 163 | self.episodeNum += 1 164 | self.episodeRewards.append(rewards) 165 | self.episodeTdErr.append(td_err) 166 | self.episodeWeights.append(weights) 167 | 168 | self.episodeList.append(self.bufferTemp) 169 | self.bufferTemp = deque() 170 | if self.compatibilityMode: 171 | self.episodePos.append(self.count) 172 | 173 | def get_episode_buffer(self, index=-1): 174 | if index == -1: 175 | index = self.episodeNum - 1 176 | elif index > (self.episodeNum - 1): 177 | self.print_mess("Does not exist this episode!") 178 | else: 179 | index = index 180 | return 181 | 182 | buffer_temp = self.episodeList[index] 183 | data = list() 184 | item_len = len(buffer_temp[0]) 185 | for ii in range(item_len): 186 | x = np.array([_[ii] for _ in buffer_temp]) 187 | data.append(x) 188 | return data 189 | 190 | def size(self): 191 | return self.count 192 | 193 | def clear(self): 194 | self.count = 0 195 | self.episodeNum = 0 196 | self.episodeRewards.clear() 197 | self.bufferTemp.clear() 198 | self.episodeList.clear() 199 | if self.compatibilityMode: 200 | self.buffer.clear() 201 | self.episodePos.clear() 202 | 203 | @classmethod 204 | def print_mess(cls, mes=""): 205 | # implement with print or warning if the project exist 206 | print(mes) 207 | -------------------------------------------------------------------------------- /ModuleTest/dynamic_actuator.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linxiaobo110/QuadrotorFly/147c14aa1c166a5bc509a0459ad18b6e3a354cf5/ModuleTest/dynamic_actuator.m -------------------------------------------------------------------------------- /ModuleTest/dynamic_basic.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linxiaobo110/QuadrotorFly/147c14aa1c166a5bc509a0459ad18b6e3a354cf5/ModuleTest/dynamic_basic.m -------------------------------------------------------------------------------- /ModuleTest/rk4.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linxiaobo110/QuadrotorFly/147c14aa1c166a5bc509a0459ad18b6e3a354cf5/ModuleTest/rk4.m -------------------------------------------------------------------------------- /ModuleTest/testResult.m: -------------------------------------------------------------------------------- 1 | tim_sample = 0.01; 2 | rotor_ct = 1.11e-5; 3 | rotor_cm = 1.49e-7; 4 | rotor_cr=646; 5 | rotor_wb=166; 6 | rotor_i=9.90e-5; 7 | rotor_t=1.36e-2; 8 | 9 | testFlag = 2; 10 | 11 | % acutator 12 | if testFlag == 1 13 | rotorRate = 0; 14 | disp('resutl 1'); 15 | for u = 0.2:0.2:0.8 16 | rateDot = 1/rotor_t * (rotor_cr * u + rotor_wb - rotorRate) 17 | end 18 | disp('resutl 2'); 19 | for u = 0.2:0.2:0.8 20 | rotorRate = 2000 * u;%400,800.... 21 | rateDot = 1/rotor_t * (rotor_cr * u + rotor_wb - rotorRate) 22 | end 23 | disp('resutl 3'); 24 | rotorRate = 0; 25 | f = @dynamic_actuator; 26 | for u = 0.2:0.2:0.8 27 | rotorRate = 0; 28 | rotorRate = rk4(f, rotorRate, u, tim_sample) 29 | end 30 | end 31 | 32 | if testFlag == 2 33 | stateTemp = [1., 2., 3., 0.2, 0.3, 0.4, 0.3, 0.4, 0.5, 0.4, 0.5, 0.6]; 34 | u = [100, 20, 20, 20]; 35 | disp('resutl1'); 36 | result1 = dynamic_basic(stateTemp, u) 37 | disp('result2'); 38 | f2 = @dynamic_basic; 39 | result2 = rk4(f2, stateTemp, u, tim_sample) 40 | end 41 | -------------------------------------------------------------------------------- /Paper/Modeling and Analysis of Temperature Effect on MEMS Gyroscope.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linxiaobo110/QuadrotorFly/147c14aa1c166a5bc509a0459ad18b6e3a354cf5/Paper/Modeling and Analysis of Temperature Effect on MEMS Gyroscope.pdf -------------------------------------------------------------------------------- /Paper/NEO-M8-FW3_DataSheet_(UBX-15031086).pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linxiaobo110/QuadrotorFly/147c14aa1c166a5bc509a0459ad18b6e3a354cf5/Paper/NEO-M8-FW3_DataSheet_(UBX-15031086).pdf -------------------------------------------------------------------------------- /QuadrotorFlyGui.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """This file implement the GUI for QuadrotorFly 4 | This module refer the 'quadcopter simulator' by abhijitmajumdar 5 | 6 | By xiaobo 7 | Contact linxiaobo110@gmail.com 8 | Created on Apr 29 20:53 2019 9 | """ 10 | 11 | # Copyright (C) 12 | # 13 | # This file is part of QuadrotorFly 14 | # 15 | # GWpy is free software: you can redistribute it and/or modify 16 | # it under the terms of the GNU General Public License as published by 17 | # the Free Software Foundation, either version 3 of the License, or 18 | # (at your option) any later version. 19 | # 20 | # GWpy is distributed in the hope that it will be useful, 21 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 22 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 23 | # GNU General Public License for more details. 24 | # 25 | # You should have received a copy of the GNU General Public License 26 | # along with GWpy. If not, see . 27 | 28 | 29 | import numpy as np 30 | import QuadrotorFlyModel as Qfm 31 | import matplotlib.pyplot as plt 32 | import mpl_toolkits.mplot3d.axes3d as axes3d 33 | import CommonFunctions as Cf 34 | 35 | """ 36 | ******************************************************************************************************** 37 | **------------------------------------------------------------------------------------------------------- 38 | ** Compiler : python 3.6 39 | ** Module Name: QuadrotorFlyGui 40 | ** Module Date: 2019/4/29 41 | ** Module Auth: xiaobo 42 | ** Version : V0.1 43 | ** Description: 44 | **------------------------------------------------------------------------------------------------------- 45 | ** Reversion : 46 | ** Modified By: 47 | ** Date : 48 | ** Content : 49 | ** Notes : 50 | ********************************************************************************************************/ 51 | """ 52 | 53 | 54 | class QuadrotorFlyGuiEnv(object): 55 | def __init__(self, bound_x=3., bound_y=3., bound_z=5.): 56 | """Define the environment of quadrotor simulation 57 | :param bound_x: 58 | :param bound_y: 59 | :param bound_z: 60 | """ 61 | self.fig = plt.figure() 62 | self.boundX = bound_x * 1. 63 | self.boundY = bound_y * 1. 64 | self.boundZ = bound_z * 1. 65 | self.ax = axes3d.Axes3D(self.fig) 66 | self.ax.set_xlim3d([-self.boundX, self.boundX]) 67 | self.ax.set_ylim3d([-self.boundY, self.boundY]) 68 | self.ax.set_zlim3d([0, self.boundZ]) 69 | self.ax.set_xlabel('X') 70 | self.ax.set_ylabel('Y') 71 | self.ax.set_zlabel('Z') 72 | self.ax.set_title('QuadrotorFly Simulation') 73 | 74 | 75 | # def get_rotation_matrix(att): 76 | # cos_att = np.cos(att) 77 | # sin_att = np.sin(att) 78 | # 79 | # rotation_x = np.array([[1, 0, 0], [0, cos_att[0], -sin_att[0]], [0, sin_att[0], cos_att[0]]]) 80 | # rotation_y = np.array([[cos_att[1], 0, sin_att[1]], [0, 1, 0], [-sin_att[1], 0, cos_att[1]]]) 81 | # rotation_z = np.array([[cos_att[2], -sin_att[2], 0], [sin_att[2], cos_att[2], 0], [0, 0, 1]]) 82 | # rotation_matrix = np.dot(rotation_z, np.dot(rotation_y, rotation_x)) 83 | # 84 | # return rotation_matrix 85 | 86 | 87 | class QuadrotorFlyGuiUav(object): 88 | """Draw quadrotor class""" 89 | def __init__(self, quads: list, ax: axes3d.Axes3D): 90 | self.quads = list() 91 | self.quadGui = list() 92 | self.ax = ax 93 | 94 | # type checking 95 | for quad_temp in quads: 96 | if isinstance(quad_temp, Qfm.QuadModel): 97 | self.quads.append(quad_temp) 98 | else: 99 | raise Cf.QuadrotorFlyError("Not a QuadrotorModel type") 100 | 101 | index = 1 102 | for quad_temp in self.quads: 103 | label = ax.text([], [], [], str(index), fontsize='large') 104 | index += 1 105 | if quad_temp.uavPara.structureType == Qfm.StructureType.quad_plus: 106 | hub, = ax.plot([], [], [], marker='o', color='green', markersize=6, antialiased=False) 107 | bar_x, = ax.plot([], [], [], color='red', linewidth=3, antialiased=False) 108 | bar_y, = ax.plot([], [], [], color='black', linewidth=3, antialiased=False) 109 | self.quadGui.append({'hub': hub, 'barX': bar_x, 'barY': bar_y, 'label': label}) 110 | elif quad_temp.uavPara.structureType == Qfm.StructureType.quad_x: 111 | hub, = self.ax.plot([], [], [], marker='o', color='green', markersize=6, antialiased=False) 112 | front_bar1, = self.ax.plot([], [], [], color='red', linewidth=3, antialiased=False) 113 | front_bar2, = self.ax.plot([], [], [], color='red', linewidth=3, antialiased=False) 114 | back_bar1, = self.ax.plot([], [], [], color='black', linewidth=3, antialiased=False) 115 | back_bar2, = self.ax.plot([], [], [], color='black', linewidth=3, antialiased=False) 116 | self.quadGui.append({'hub': hub, 'bar_frontLeft': front_bar1, 'bar_frontRight': front_bar2, 117 | 'bar_rearLeft': back_bar1, 'bar_rearRight': back_bar2, 'label': label}) 118 | 119 | def render(self): 120 | counts = len(self.quads) 121 | for ii in range(counts): 122 | quad = self.quads[ii] 123 | quad_gui = self.quadGui[ii] 124 | uav_l = quad.uavPara.uavL 125 | position = quad.position 126 | # move label 127 | quad_gui['label'].set_position((position[0] + uav_l, position[1])) 128 | quad_gui['label'].set_3d_properties(position[2] + uav_l, zdir='x') 129 | # move uav 130 | if quad.uavPara.structureType == Qfm.StructureType.quad_plus: 131 | attitude = quad.attitude 132 | rot_matrix = Cf.get_rotation_matrix(attitude) 133 | points = np.array([[-uav_l, 0, 0], [uav_l, 0, 0], [0, -uav_l, 0], [0, uav_l, 0], [0, 0, 0]]).T 134 | points_rotation = np.dot(rot_matrix, points) 135 | points_rotation[0, :] += position[0] 136 | points_rotation[1, :] += position[1] 137 | points_rotation[2, :] += position[2] 138 | quad_gui['barX'].set_data(points_rotation[0, 0:2], points_rotation[1, 0:2]) 139 | quad_gui['barX'].set_3d_properties(points_rotation[2, 0:2]) 140 | quad_gui['barY'].set_data(points_rotation[0, 2:4], points_rotation[1, 2:4]) 141 | quad_gui['barY'].set_3d_properties(points_rotation[2, 2:4]) 142 | quad_gui['hub'].set_data(points_rotation[0, 4], points_rotation[1, 4]) 143 | quad_gui['hub'].set_3d_properties(points_rotation[2, 4]) 144 | elif quad.uavPara.structureType == Qfm.StructureType.quad_x: 145 | attitude = quad.attitude 146 | rot_matrix = Cf.get_rotation_matrix(attitude) 147 | pos_rotor = uav_l * np.sqrt(0.5) 148 | # this points is the position of rotor in the body frame; the [0, 0, 0] is the center of UAV; 149 | # and the sequence is front_left, front_right, back_left, back_right. 150 | points = np.array([[pos_rotor, pos_rotor, 0], [0, 0, 0], [pos_rotor, -pos_rotor, 0], [0, 0, 0], 151 | [-pos_rotor, pos_rotor, 0], [0, 0, 0], [-pos_rotor, -pos_rotor, 0], [0, 0, 0]]).T 152 | # trans axi from body-frame to world-frame 153 | points_rotation = np.dot(rot_matrix, points) 154 | points_rotation[0, :] += position[0] 155 | points_rotation[1, :] += position[1] 156 | points_rotation[2, :] += position[2] 157 | quad_gui['bar_frontLeft'].set_data(points_rotation[0, 0:2], points_rotation[1, 0:2]) 158 | quad_gui['bar_frontLeft'].set_3d_properties(points_rotation[2, 0:2]) 159 | quad_gui['bar_frontRight'].set_data(points_rotation[0, 2:4], points_rotation[1, 2:4]) 160 | quad_gui['bar_frontRight'].set_3d_properties(points_rotation[2, 2:4]) 161 | quad_gui['bar_rearLeft'].set_data(points_rotation[0, 4:6], points_rotation[1, 4:6]) 162 | quad_gui['bar_rearLeft'].set_3d_properties(points_rotation[2, 4:6]) 163 | quad_gui['bar_rearRight'].set_data(points_rotation[0, 6:8], points_rotation[1, 6:8]) 164 | quad_gui['bar_rearRight'].set_3d_properties(points_rotation[2, 6:8]) 165 | quad_gui['hub'].set_data(position[0], position[1]) 166 | quad_gui['hub'].set_3d_properties(position[2]) 167 | 168 | 169 | class QuadrotorFlyGui(object): 170 | """ Gui manage class""" 171 | def __init__(self, quads: list): 172 | self.quads = quads 173 | self.env = QuadrotorFlyGuiEnv() 174 | self.ax = self.env.ax 175 | self.quadGui = QuadrotorFlyGuiUav(self.quads, self.ax) 176 | 177 | def render(self): 178 | self.quadGui.render() 179 | plt.pause(0.000000000000001) 180 | 181 | 182 | if __name__ == '__main__': 183 | import MemoryStore 184 | " used for testing this module" 185 | D2R = Qfm.D2R 186 | testFlag = 1 187 | if testFlag == 1: 188 | # import matplotlib as mpl 189 | print("PID controller test: ") 190 | uavPara = Qfm.QuadParas(structure_type=Qfm.StructureType.quad_plus) 191 | simPara = Qfm.QuadSimOpt(init_mode=Qfm.SimInitType.rand, 192 | init_att=np.array([10., 10., 0]), init_pos=np.array([0, 3, 0])) 193 | quad1 = Qfm.QuadModel(uavPara, simPara) 194 | record = MemoryStore.DataRecord() 195 | record.clear() 196 | # multi uav test 197 | quad2 = Qfm.QuadModel(uavPara, simPara) 198 | 199 | # gui init 200 | gui = QuadrotorFlyGui([quad1, quad2]) 201 | 202 | # simulation begin 203 | step_cnt = 0 204 | for i in range(1000): 205 | ref = np.array([0., 0., 1., 0.]) 206 | stateTemp = quad1.observe() 207 | action2, oil = quad1.get_controller_pid(stateTemp, ref) 208 | print('action: ', action2) 209 | action2 = np.clip(action2, 0.1, 0.9) 210 | quad1.step(action2) 211 | 212 | # multi uav test 213 | action2, oil2 = quad2.get_controller_pid(quad2.observe(), ref) 214 | quad2.step(action2) 215 | 216 | gui.render() 217 | record.buffer_append((stateTemp, action2)) 218 | step_cnt = stateTemp + 1 219 | 220 | record.episode_append() 221 | 222 | print('Quadrotor structure type', quad1.uavPara.structureType) 223 | # quad1.reset_states() 224 | print('Quadrotor get reward:', quad1.get_reward()) 225 | data = record.get_episode_buffer() 226 | bs = data[0] 227 | ba = data[1] 228 | t = range(0, record.count) 229 | # mpl.style.use('seaborn') 230 | fig1 = plt.figure(2) 231 | plt.clf() 232 | plt.subplot(3, 1, 1) 233 | plt.plot(t, bs[t, 6] / D2R, label='roll') 234 | plt.plot(t, bs[t, 7] / D2R, label='pitch') 235 | plt.plot(t, bs[t, 8] / D2R, label='yaw') 236 | plt.ylabel('Attitude $(\circ)$', fontsize=15) 237 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 238 | plt.subplot(3, 1, 2) 239 | plt.plot(t, bs[t, 0], label='x') 240 | plt.plot(t, bs[t, 1], label='y') 241 | plt.ylabel('Position (m)', fontsize=15) 242 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 243 | plt.subplot(3, 1, 3) 244 | plt.plot(t, bs[t, 2], label='z') 245 | plt.ylabel('Altitude (m)', fontsize=15) 246 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 247 | plt.show() 248 | -------------------------------------------------------------------------------- /QuadrotorFlyModel.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """The file used to describe the dynamic of quadrotor UAV 4 | 5 | By xiaobo 6 | Contact linxiaobo110@gmail.com 7 | Created on Fri Apr 19 10:40:44 2019 8 | """ 9 | 10 | # Copyright (C) 11 | # 12 | # This file is part of QuadrotorFly 13 | # 14 | # GWpy is free software: you can redistribute it and/or modify 15 | # it under the terms of the GNU General Public License as published by 16 | # the Free Software Foundation, either version 3 of the License, or 17 | # (at your option) any later version. 18 | # 19 | # GWpy is distributed in the hope that it will be useful, 20 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 21 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 22 | # GNU General Public License for more details. 23 | # 24 | # You should have received a copy of the GNU General Public License 25 | # along with GWpy. If not, see . 26 | 27 | 28 | import numpy as np 29 | import enum 30 | from enum import Enum 31 | import MemoryStore 32 | import SensorImu 33 | import SensorBase 34 | import SensorGps 35 | import SensorCompass 36 | 37 | """ 38 | ******************************************************************************************************** 39 | **------------------------------------------------------------------------------------------------------- 40 | ** Compiler : python 3.6 41 | ** Module Name: QuadrotorFlyModel 42 | ** Module Date: 2019-04-19 43 | ** Module Auth: xiaobo 44 | ** Version : V0.1 45 | ** Description: create the module 46 | **------------------------------------------------------------------------------------------------------- 47 | ** Reversion : 48 | ** Modified By: 49 | ** Date : 50 | ** Content : 51 | ** Notes : 52 | ********************************************************************************************************/ 53 | """ 54 | 55 | # definition of key constant 56 | D2R = np.pi / 180 57 | state_dim = 12 58 | action_dim = 4 59 | state_bound = np.array([10, 10, 10, 5, 5, 5, 80 * D2R, 80 * D2R, 180 * D2R, 100 * D2R, 100 * D2R, 100 * D2R]) 60 | action_bound = np.array([1, 1, 1, 1]) 61 | 62 | 63 | def rk4(func, x0, action, h): 64 | """Runge Kutta 4 order update function 65 | :param func: system dynamic 66 | :param x0: system state 67 | :param action: control input 68 | :param h: time of sample 69 | :return: state of next time 70 | """ 71 | k1 = func(x0, action) 72 | k2 = func(x0 + h * k1 / 2, action) 73 | k3 = func(x0 + h * k2 / 2, action) 74 | k4 = func(x0 + h * k3, action) 75 | # print('rk4 debug: ', k1, k2, k3, k4) 76 | x1 = x0 + h * (k1 + 2 * k2 + 2 * k3 + k4) / 6 77 | return x1 78 | 79 | 80 | class StructureType(Enum): 81 | quad_x = enum.auto() 82 | quad_plus = enum.auto() 83 | 84 | 85 | class QuadParas(object): 86 | """Define the parameters of quadrotor model 87 | 88 | """ 89 | 90 | def __init__(self, g=9.81, rotor_num=4, tim_sample=0.01, structure_type=StructureType.quad_plus, 91 | uav_l=0.450, uav_m=1.50, uav_ixx=1.75e-2, uav_iyy=1.75e-2, uav_izz=3.18e-2, 92 | rotor_ct=1.11e-5, rotor_cm=1.49e-7, rotor_cr=646, rotor_wb=166, rotor_i=9.90e-5, rotor_t=1.36e-2): 93 | """init the quadrotor parameters 94 | These parameters are able to be estimation in web(https://flyeval.com/) if you do not have a real UAV. 95 | common parameters: 96 | -g : N/kg, acceleration gravity 97 | -rotor-num : int, number of rotors, e.g. 4, 6, 8 98 | -tim_sample : s, sample time of system 99 | -structure_type: quad_x, quad_plus 100 | uav: 101 | -uav_l : m, distance from center of mass to center of rotor 102 | -uav_m : kg, the mass of quadrotor 103 | -uav_ixx : kg.m^2 central principal moments of inertia of UAV in x 104 | -uav_iyy : kg.m^2 central principal moments of inertia of UAV in y 105 | -uav_izz : kg.m^2 central principal moments of inertia of UAV in z 106 | rotor (assume that four rotors are the same): 107 | -rotor_ct : N/(rad/s)^2, lump parameter thrust coefficient, which translate rate of rotor to thrust 108 | -rotor_cm : N.m/(rad/s)^2, lump parameter torque coefficient, like ct, usd in yaw 109 | -rotor_cr : rad/s, scale para which translate oil to rate of motor 110 | -rotor_wb : rad/s, bias para which translate oil to rate of motor 111 | -rotor_i : kg.m^2, inertia of moment of rotor(including motor and propeller) 112 | -rotor_t : s, time para of dynamic response of motor 113 | """ 114 | self.g = g 115 | self.numOfRotors = rotor_num 116 | self.ts = tim_sample 117 | self.structureType = structure_type 118 | self.uavL = uav_l 119 | self.uavM = uav_m 120 | self.uavInertia = np.array([uav_ixx, uav_iyy, uav_izz]) 121 | self.rotorCt = rotor_ct 122 | self.rotorCm = rotor_cm 123 | self.rotorCr = rotor_cr 124 | self.rotorWb = rotor_wb 125 | self.rotorInertia = rotor_i 126 | self.rotorTimScale = 1 / rotor_t 127 | 128 | 129 | class SimInitType(Enum): 130 | rand = enum.auto() 131 | fixed = enum.auto() 132 | 133 | 134 | class ActuatorMode(Enum): 135 | simple = enum.auto() 136 | dynamic = enum.auto() 137 | disturbance = enum.auto() 138 | dynamic_voltage = enum.auto() 139 | disturbance_voltage = enum.auto() 140 | 141 | 142 | class QuadSimOpt(object): 143 | """contain the parameters for guiding the simulation process 144 | """ 145 | 146 | def __init__(self, init_mode=SimInitType.rand, init_att=np.array([5, 5, 5]), init_pos=np.array([1, 1, 1]), 147 | max_position=10, max_velocity=10, max_attitude=180, max_angular=200, 148 | sysnoise_bound_pos=0, sysnoise_bound_att=0, 149 | actuator_mode=ActuatorMode.simple, enable_sensor_sys=False): 150 | """ init the parameters for simulation process, focus on conditions during an episode 151 | :param init_mode: 152 | :param init_att: 153 | :param init_pos: 154 | :param sysnoise_bound_pos: 155 | :param sysnoise_bound_att: 156 | :param actuator_mode: 157 | :param enable_sensor_sys: whether the sensor system is enable, including noise and bias of sensor 158 | """ 159 | self.initMode = init_mode 160 | self.initAtt = init_att 161 | self.initPos = init_pos 162 | self.actuatorMode = actuator_mode 163 | self.sysNoisePos = sysnoise_bound_pos 164 | self.sysNoiseAtt = sysnoise_bound_att 165 | self.maxPosition = max_position 166 | self.maxVelocity = max_velocity 167 | self.maxAttitude = max_attitude * D2R 168 | self.maxAngular = max_angular * D2R 169 | self.enableSensorSys = enable_sensor_sys 170 | 171 | 172 | class QuadActuator(object): 173 | """Dynamic of actuator including motor and propeller 174 | """ 175 | 176 | def __init__(self, quad_para: QuadParas, mode: ActuatorMode): 177 | """Parameters is maintain together 178 | :param quad_para: parameters of quadrotor,maintain together 179 | :param mode: 'simple', without dynamic of motor; 'dynamic' with dynamic; 180 | """ 181 | self.para = quad_para 182 | self.motorPara_scale = self.para.rotorTimScale * self.para.rotorCr 183 | self.motorPara_bias = self.para.rotorTimScale * self.para.rotorWb 184 | self.mode = mode 185 | 186 | # states of actuator 187 | self.outThrust = np.zeros([self.para.numOfRotors]) 188 | self.outTorque = np.zeros([self.para.numOfRotors]) 189 | # rate of rotor 190 | self.rotorRate = np.zeros([self.para.numOfRotors]) 191 | 192 | def dynamic_actuator(self, rotor_rate, action): 193 | """dynamic of motor and propeller 194 | input: rotorRate, u 195 | output: rotorRateDot, 196 | """ 197 | 198 | rate_dot = self.motorPara_scale * action + self.motorPara_bias - self.para.rotorTimScale * rotor_rate 199 | return rate_dot 200 | 201 | def reset(self): 202 | """reset all state""" 203 | 204 | self.outThrust = np.zeros([self.para.numOfRotors]) 205 | self.outTorque = np.zeros([self.para.numOfRotors]) 206 | # rate of rotor 207 | self.rotorRate = np.zeros([self.para.numOfRotors]) 208 | 209 | def step(self, action: 'int > 0'): 210 | """calculate the next state based on current state and u 211 | :param action: 212 | :return: 213 | """ 214 | action = np.clip(action, 0, 1) 215 | # if u > 1: 216 | # u = 1 217 | 218 | if self.mode == ActuatorMode.simple: 219 | # without dynamic of motor 220 | self.rotorRate = self.para.rotorCr * action + self.para.rotorWb 221 | elif self.mode == ActuatorMode.dynamic: 222 | # with dynamic of motor 223 | self.rotorRate = rk4(self.dynamic_actuator, self.rotorRate, action, self.para.ts) 224 | else: 225 | self.rotorRate = 0 226 | 227 | self.outThrust = self.para.rotorCt * np.square(self.rotorRate) 228 | self.outTorque = self.para.rotorCm * np.square(self.rotorRate) 229 | return self.outThrust, self.outTorque 230 | 231 | 232 | class QuadModel(object): 233 | """module interface, main class including basic dynamic of quad 234 | """ 235 | 236 | def __init__(self, uav_para: QuadParas, sim_para: QuadSimOpt): 237 | """init a quadrotor 238 | :param uav_para: parameters of quadrotor,maintain together 239 | :param sim_para: 'simple', without dynamic of motor; 'dynamic' with dynamic; 240 | """ 241 | self.uavPara = uav_para 242 | self.simPara = sim_para 243 | self.actuator = QuadActuator(self.uavPara, sim_para.actuatorMode) 244 | 245 | # states of quadrotor 246 | # -position, m 247 | self.position = np.array([0, 0, 0]) 248 | # -velocity, m/s 249 | self.velocity = np.array([0, 0, 0]) 250 | # -attitude, rad 251 | self.attitude = np.array([0, 0, 0]) 252 | # -angular, rad/s 253 | self.angular = np.array([0, 0, 0]) 254 | # accelerate, m/(s^2) 255 | self.acc = np.zeros(3) 256 | 257 | # time control, s 258 | self.__ts = 0 259 | 260 | # initial the sensors 261 | if self.simPara.enableSensorSys: 262 | self.sensorList = list() 263 | self.imu0 = SensorImu.SensorImu() 264 | self.gps0 = SensorGps.SensorGps() 265 | self.mag0 = SensorCompass.SensorCompass() 266 | self.sensorList.append(self.imu0) 267 | self.sensorList.append(self.gps0) 268 | self.sensorList.append(self.mag0) 269 | 270 | # initial the states 271 | self.reset_states() 272 | 273 | @property 274 | def ts(self): 275 | """return the tick of system""" 276 | return self.__ts 277 | 278 | def generate_init_att(self): 279 | """used to generate a init attitude according to simPara""" 280 | angle = self.simPara.initAtt * D2R 281 | if self.simPara.initMode == SimInitType.rand: 282 | phi = (1 * np.random.random() - 0.5) * angle[0] 283 | theta = (1 * np.random.random() - 0.5) * angle[1] 284 | psi = (1 * np.random.random() - 0.5) * angle[2] 285 | else: 286 | phi = angle[0] 287 | theta = angle[1] 288 | psi = angle[2] 289 | return np.array([phi, theta, psi]) 290 | 291 | def generate_init_pos(self): 292 | """used to generate a init position according to simPara""" 293 | pos = self.simPara.initPos 294 | if self.simPara.initMode == SimInitType.rand: 295 | x = (1 * np.random.random() - 0.5) * pos[0] 296 | y = (1 * np.random.random() - 0.5) * pos[1] 297 | z = (1 * np.random.random() - 0.5) * pos[2] 298 | else: 299 | x = pos[0] 300 | y = pos[1] 301 | z = pos[2] 302 | return np.array([x, y, z]) 303 | 304 | def reset_states(self, att='none', pos='none'): 305 | self.__ts = 0 306 | self.actuator.reset() 307 | if isinstance(att, str): 308 | self.attitude = self.generate_init_att() 309 | else: 310 | self.attitude = att 311 | 312 | if isinstance(pos, str): 313 | self.position = self.generate_init_pos() 314 | else: 315 | self.position = pos 316 | 317 | self.velocity = np.array([0, 0, 0]) 318 | self.angular = np.array([0, 0, 0]) 319 | 320 | # sensor system reset 321 | if self.simPara.enableSensorSys: 322 | for sensor in self.sensorList: 323 | sensor.reset(self.state) 324 | 325 | def dynamic_basic(self, state, action): 326 | """ calculate /dot(state) = f(state) + u(state) 327 | This function will be executed many times during simulation, so high performance is necessary. 328 | :param state: 329 | 0 1 2 3 4 5 330 | p_x p_y p_z v_x v_y v_z 331 | 6 7 8 9 10 11 332 | roll pitch yaw v_roll v_pitch v_yaw 333 | :param action: u1(sum of thrust), u2(torque for roll), u3(pitch), u4(yaw) 334 | :return: derivatives of state inclfrom bokeh.plotting import figure 335 | """ 336 | # variable used repeatedly 337 | att_cos = np.cos(state[6:9]) 338 | att_sin = np.sin(state[6:9]) 339 | noise_pos = self.simPara.sysNoisePos * np.random.random(3) 340 | noise_att = self.simPara.sysNoiseAtt * np.random.random(3) 341 | 342 | dot_state = np.zeros([12]) 343 | # dynamic of position cycle 344 | dot_state[0:3] = state[3:6] 345 | # we need not to calculate the whole rotation matrix because just care last column 346 | dot_state[3:6] = action[0] / self.uavPara.uavM * np.array([ 347 | att_cos[2] * att_sin[1] * att_cos[0] + att_sin[2] * att_sin[0], 348 | att_sin[2] * att_sin[1] * att_cos[0] - att_cos[2] * att_sin[0], 349 | att_cos[0] * att_cos[1] 350 | ]) - np.array([0, 0, self.uavPara.g]) + noise_pos 351 | 352 | # dynamic of attitude cycle 353 | dot_state[6:9] = state[9:12] 354 | # Coriolis force on UAV from motor, this is affected by the direction of rotation. 355 | # Pay attention, it needs to be modify when the model of uav varies. 356 | # The signals of this equation should be same with toque for yaw 357 | rotor_rate_sum = (self.actuator.rotorRate[3] + self.actuator.rotorRate[2] 358 | - self.actuator.rotorRate[1] - self.actuator.rotorRate[0]) 359 | 360 | para = self.uavPara 361 | dot_state[9:12] = np.array([ 362 | state[10] * state[11] * (para.uavInertia[1] - para.uavInertia[2]) / para.uavInertia[0] 363 | - para.rotorInertia / para.uavInertia[0] * state[10] * rotor_rate_sum 364 | + para.uavL * action[1] / para.uavInertia[0], 365 | state[9] * state[11] * (para.uavInertia[2] - para.uavInertia[0]) / para.uavInertia[1] 366 | + para.rotorInertia / para.uavInertia[1] * state[9] * rotor_rate_sum 367 | + para.uavL * action[2] / para.uavInertia[1], 368 | state[9] * state[10] * (para.uavInertia[0] - para.uavInertia[1]) / para.uavInertia[2] 369 | + action[3] / para.uavInertia[2] 370 | ]) + noise_att 371 | 372 | ''' Just used for test 373 | temp1 = state[10] * state[11] * (para.uavInertia[1] - para.uavInertia[2]) / para.uavInertia[0] 374 | temp2 = - para.rotorInertia / para.uavInertia[0] * state[10] * rotor_rate_sum 375 | temp3 = + para.uavL * action[1] / para.uavInertia[0] 376 | print('dyanmic Test', temp1, temp2, temp3, action) 377 | ''' 378 | 379 | return dot_state 380 | 381 | def observe(self): 382 | """out put the system state, with sensor system or without sensor system""" 383 | if self.simPara.enableSensorSys: 384 | sensor_data = dict() 385 | for index, sensor in enumerate(self.sensorList): 386 | if isinstance(sensor, SensorBase.SensorBase): 387 | # name = str(index) + '-' + sensor.get_name() 388 | name = sensor.get_name() 389 | sensor_data.update({name: sensor.observe()}) 390 | return sensor_data 391 | else: 392 | return np.hstack([self.position, self.velocity, self.attitude, self.angular]) 393 | 394 | @property 395 | def state(self): 396 | return np.hstack([self.position, self.velocity, self.attitude, self.angular]) 397 | 398 | def is_finished(self): 399 | if (np.max(np.abs(self.position)) < self.simPara.maxPosition)\ 400 | and (np.max(np.abs(self.velocity) < self.simPara.maxVelocity))\ 401 | and (np.max(np.abs(self.attitude) < self.simPara.maxAttitude))\ 402 | and (np.max(np.abs(self.angular) < self.simPara.maxAngular)): 403 | return False 404 | else: 405 | return True 406 | 407 | def get_reward(self): 408 | reward = np.sum(np.square(self.position)) / 8 + np.sum(np.square(self.velocity)) / 20 \ 409 | + np.sum(np.square(self.attitude)) / 3 + np.sum(np.square(self.angular)) / 10 410 | return reward 411 | 412 | def rotor_distribute_dynamic(self, thrusts, torque): 413 | """ calculate torque according to the distribution of rotors 414 | :param thrusts: 415 | :param torque: 416 | :return: 417 | """ 418 | ''' The structure of quadrotor, left is '+' and the right is 'x' 419 | The 'x' 'y' in middle defines the positive direction X Y axi in body-frame, which is a right hand frame. 420 | The numbers inside the rotors indicate the index of the motors; 421 | The signals show the direction of rotation, positive is ccw while the negative is cw. 422 | --------------------------------------------------------------------------------------------------- 423 | ****** 424 | ** 3 ** **** **** 425 | ** - ** ** ** ** ** 426 | ** ** ** 3 ** ** 1 ** 427 | ** ** ** - ** ** + ** 428 | ** ** ** ** ** 429 | **** ** **** **** ** ** ** **** 430 | ** ** ** ** ** ** x(+) *** *** 431 | ** 2 ** ** ** ** 1 ** y(+) y(-) ** ** 432 | ** + ** ** ** ** + ** x(-) ** ** 433 | ** ** ****** ** ** * ** ** * 434 | **** ** **** **** ** ** **** 435 | ** ** ** ** ** 436 | ** ** ** 2 ** ** 4 ** 437 | ** ** ** + ** ** - ** 438 | ** 4 ** ** ** ** ** 439 | ** - ** **** **** 440 | ****** 441 | --------------------------------------------------------------------------------------------------- 442 | ''' 443 | forces = np.zeros(4) 444 | if self.uavPara.structureType == StructureType.quad_plus: 445 | forces[0] = np.sum(thrusts) 446 | forces[1] = thrusts[1] - thrusts[0] 447 | forces[2] = thrusts[3] - thrusts[2] 448 | forces[3] = torque[3] + torque[2] - torque[1] - torque[0] 449 | elif self.uavPara.structureType == StructureType.quad_x: 450 | forces[0] = np.sum(thrusts) 451 | forces[1] = -thrusts[0] + thrusts[1] + thrusts[2] - thrusts[3] 452 | forces[2] = -thrusts[0] + thrusts[1] - thrusts[2] + thrusts[3] 453 | forces[3] = -torque[0] - torque[1] + torque[2] + torque[3] 454 | else: 455 | forces = np.zeros(4) 456 | 457 | return forces 458 | 459 | def step(self, action: 'int > 0'): 460 | 461 | self.__ts += self.uavPara.ts 462 | # 1.1 Actuator model, calculate the thrust and torque 463 | thrusts, toques = self.actuator.step(action) 464 | 465 | # 1.2 Calculate the force distribute according to 'x' type or '+' type, assum '+' type 466 | forces = self.rotor_distribute_dynamic(thrusts, toques) 467 | 468 | # 1.3 Basic model, calculate the basic model, the u need to be given directly in test-mode for Matlab 469 | state_temp = np.hstack([self.position, self.velocity, self.attitude, self.angular]) 470 | state_next = rk4(self.dynamic_basic, state_temp, forces, self.uavPara.ts) 471 | [self.position, self.velocity, self.attitude, self.angular] = np.split(state_next, 4) 472 | # calculate the accelerate 473 | state_dot = self.dynamic_basic(state_temp, forces) 474 | self.acc = state_dot[3:6] 475 | 476 | # 2. Calculate Sensor sensor model 477 | if self.simPara.enableSensorSys: 478 | for index, sensor in enumerate(self.sensorList): 479 | if isinstance(sensor, SensorBase.SensorBase): 480 | sensor.update(np.hstack([state_next, self.acc]), self.__ts) 481 | ob = self.observe() 482 | 483 | # 3. Check whether finish (failed or completed) 484 | finish_flag = self.is_finished() 485 | 486 | # 4. Calculate a reference reward 487 | reward = self.get_reward() 488 | 489 | return ob, reward, finish_flag 490 | 491 | def get_controller_pid(self, state, ref_state=np.array([0, 0, 1, 0])): 492 | """ pid controller 493 | :param state: system state, 12 494 | :param ref_state: reference value for x, y, z, yaw 495 | :return: control value for four motors 496 | """ 497 | 498 | # position-velocity cycle, velocity cycle is regard as kd 499 | kp_pos = np.array([0.3, 0.3, 0.8]) 500 | kp_vel = np.array([0.15, 0.15, 0.5]) 501 | # decoupling about x-y 502 | phy = state[8] 503 | # de_phy = np.array([[np.sin(phy), -np.cos(phy)], [np.cos(phy), np.sin(phy)]]) 504 | # de_phy = np.array([[np.cos(phy), np.sin(phy)], [np.sin(phy), -np.cos(phy)]]) 505 | de_phy = np.array([[np.cos(phy), -np.sin(phy)], [np.sin(phy), np.cos(phy)]]) 506 | err_pos = ref_state[0:3] - np.array([state[0], state[1], state[2]]) 507 | ref_vel = err_pos * kp_pos 508 | err_vel = ref_vel - np.array([state[3], state[4], state[5]]) 509 | # calculate ref without decoupling about phy 510 | # ref_angle = kp_vel * err_vel 511 | # calculate ref with decoupling about phy 512 | ref_angle = np.zeros(3) 513 | ref_angle[0:2] = np.matmul(de_phy, kp_vel[0] * err_vel[0:2]) 514 | 515 | # attitude-angular cycle, angular cycle is regard as kd 516 | kp_angle = np.array([1.0, 1.0, 0.8]) 517 | kp_angular = np.array([0.2, 0.2, 0.2]) 518 | # ref_angle = np.zeros(3) 519 | err_angle = np.array([-ref_angle[1], ref_angle[0], ref_state[3]]) - np.array([state[6], state[7], state[8]]) 520 | ref_rate = err_angle * kp_angle 521 | err_rate = ref_rate - [state[9], state[10], state[11]] 522 | con_rate = err_rate * kp_angular 523 | 524 | # the control value in z direction needs to be modify considering gravity 525 | err_altitude = (ref_state[2] - state[2]) * 0.5 526 | con_altitude = (err_altitude - state[5]) * 0.25 527 | oil_altitude = 0.6 + con_altitude 528 | if oil_altitude > 0.75: 529 | oil_altitude = 0.75 530 | 531 | action_motor = np.zeros(4) 532 | if self.uavPara.structureType == StructureType.quad_plus: 533 | action_motor[0] = oil_altitude - con_rate[0] - con_rate[2] 534 | action_motor[1] = oil_altitude + con_rate[0] - con_rate[2] 535 | action_motor[2] = oil_altitude - con_rate[1] + con_rate[2] 536 | action_motor[3] = oil_altitude + con_rate[1] + con_rate[2] 537 | elif self.uavPara.structureType == StructureType.quad_x: 538 | action_motor[0] = oil_altitude - con_rate[2] - con_rate[1] - con_rate[0] 539 | action_motor[1] = oil_altitude - con_rate[2] + con_rate[1] + con_rate[0] 540 | action_motor[2] = oil_altitude + con_rate[2] - con_rate[1] + con_rate[0] 541 | action_motor[3] = oil_altitude + con_rate[2] + con_rate[1] - con_rate[0] 542 | else: 543 | action_motor = np.zeros(4) 544 | 545 | action_pid = action_motor 546 | return action_pid, oil_altitude 547 | 548 | 549 | if __name__ == '__main__': 550 | " used for testing this module" 551 | testFlag = 3 552 | 553 | if testFlag == 1: 554 | # test for actuator 555 | qp = QuadParas() 556 | ac0 = QuadActuator(qp, ActuatorMode.simple) 557 | print("QuadActuator Test") 558 | print("dynamic result0:", ac0.rotorRate) 559 | result1 = ac0.dynamic_actuator(ac0.rotorRate, np.array([0.2, 0.4, 0.6, 0.8])) 560 | 561 | print("dynamic result1:", result1) 562 | result2 = ac0.dynamic_actuator(np.array([400, 800, 1200, 1600]), np.array([0.2, 0.4, 0.6, 0.8])) 563 | print("dynamic result2:", result2) 564 | ac0.reset() 565 | ac0.step(np.array([0.2, 0.4, 0.6, 0.8])) 566 | print("dynamic result3:", ac0.rotorRate, ac0.outTorque, ac0.outThrust) 567 | print("QuadActuator Test Completed! ---------------------------------------------------------------") 568 | elif testFlag == 2: 569 | print("Basic model test: ") 570 | uavPara = QuadParas() 571 | simPara = QuadSimOpt(init_mode=SimInitType.fixed, actuator_mode=ActuatorMode.dynamic, 572 | init_att=np.array([0.2, 0.2, 0.2]), init_pos=np.array([0, 0, 0])) 573 | quad1 = QuadModel(uavPara, simPara) 574 | u = np.array([100., 20., 20., 20.]) 575 | stateTemp = np.array([1., 2., 3., 0.2, 0.3, 0.4, 0.3, 0.4, 0.5, 0.4, 0.5, 0.6]) 576 | result1 = quad1.dynamic_basic(stateTemp, np.array([100., 20., 20., 20.])) 577 | print("result1 ", result1) 578 | [quad1.pos, quad1.velocity, quad1.attitude, quad1.angular] = np.split(stateTemp, 4) 579 | result2 = quad1.step(u) 580 | print("result2 ", result2, quad1.pos, quad1.velocity, quad1.attitude, quad1.angular) 581 | elif testFlag == 3: 582 | import matplotlib.pyplot as plt 583 | import matplotlib as mpl 584 | print("PID controller test: ") 585 | uavPara = QuadParas(structure_type=StructureType.quad_x) 586 | simPara = QuadSimOpt(init_mode=SimInitType.fixed, enable_sensor_sys=False, 587 | init_att=np.array([10., -10., 30]), init_pos=np.array([5, -5, 0])) 588 | quad1 = QuadModel(uavPara, simPara) 589 | record = MemoryStore.DataRecord() 590 | record.clear() 591 | step_cnt = 0 592 | for i in range(1000): 593 | ref = np.array([0., 0., 1., 0.]) 594 | stateTemp = quad1.observe() 595 | action2, oil = quad1.get_controller_pid(stateTemp, ref) 596 | print('action: ', action2) 597 | action2 = np.clip(action2, 0.1, 0.9) 598 | quad1.step(action2) 599 | record.buffer_append((stateTemp, action2)) 600 | step_cnt = step_cnt + 1 601 | record.episode_append() 602 | 603 | print('Quadrotor structure type', quad1.uavPara.structureType) 604 | # quad1.reset_states() 605 | print('Quadrotor get reward:', quad1.get_reward()) 606 | data = record.get_episode_buffer() 607 | bs = data[0] 608 | ba = data[1] 609 | t = range(0, record.count) 610 | # mpl.style.use('seaborn') 611 | fig1 = plt.figure(1) 612 | plt.clf() 613 | plt.subplot(3, 1, 1) 614 | plt.plot(t, bs[t, 6] / D2R, label='roll') 615 | plt.plot(t, bs[t, 7] / D2R, label='pitch') 616 | plt.plot(t, bs[t, 8] / D2R, label='yaw') 617 | plt.ylabel('Attitude $(\circ)$', fontsize=15) 618 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 619 | plt.subplot(3, 1, 2) 620 | plt.plot(t, bs[t, 0], label='x') 621 | plt.plot(t, bs[t, 1], label='y') 622 | plt.ylabel('Position (m)', fontsize=15) 623 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 624 | plt.subplot(3, 1, 3) 625 | plt.plot(t, bs[t, 2], label='z') 626 | plt.ylabel('Altitude (m)', fontsize=15) 627 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 628 | plt.show() 629 | -------------------------------------------------------------------------------- /QuadrotorFlyTest.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """This file is used for testing the QuadrotorFly 4 | 5 | By xiaobo 6 | Contact linxiaobo110@gmail.com 7 | Created on 五月 06 17:13 2019 8 | """ 9 | 10 | # Copyright (C) 11 | # 12 | # This file is part of QuadrotorFly 13 | # 14 | # GWpy is free software: you can redistribute it and/or modify 15 | # it under the terms of the GNU General Public License as published by 16 | # the Free Software Foundation, either version 3 of the License, or 17 | # (at your option) any later version. 18 | # 19 | # GWpy is distributed in the hope that it will be useful, 20 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 21 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 22 | # GNU General Public License for more details. 23 | # 24 | # You should have received a copy of the GNU General Public License 25 | # along with GWpy. If not, see . 26 | 27 | 28 | import numpy as np 29 | import QuadrotorFlyModel as Qfm 30 | import QuadrotorFlyGui as Qfg 31 | import MemoryStore 32 | import matplotlib.pyplot as plt 33 | from enum import Enum 34 | import enum 35 | import StateEstimator 36 | import CamDown 37 | import time 38 | import cv2 39 | 40 | """ 41 | ******************************************************************************************************** 42 | **------------------------------------------------------------------------------------------------------- 43 | ** Compiler : python 3.6 44 | ** Module Name: QuadrotorFlyTest 45 | ** Module Date: 2019/5/6 46 | ** Module Auth: xiaobo 47 | ** Version : V0.1 48 | ** Description: 'Replace the content between' 49 | **------------------------------------------------------------------------------------------------------- 50 | ** Reversion : 51 | ** Modified By: 52 | ** Date : 53 | ** Content : 54 | ** Notes : 55 | ********************************************************************************************************/ 56 | """ 57 | 58 | 59 | class TestPara(Enum): 60 | Test_Module_Dynamic = enum.auto() 61 | Test_Module_Dynamic_Sensor = enum.auto() 62 | Test_Module_Dynamic_CamDown = enum.auto() 63 | 64 | 65 | D2R = Qfm.D2R 66 | testFlag = TestPara.Test_Module_Dynamic_Sensor 67 | 68 | if testFlag == TestPara.Test_Module_Dynamic: 69 | print("QuadrotorFly Dynamic Test: ") 70 | # define the quadrotor parameters 71 | uavPara = Qfm.QuadParas() 72 | # define the simulation parameters 73 | simPara = Qfm.QuadSimOpt(init_mode=Qfm.SimInitType.rand, 74 | init_att=np.array([10., 10., 0]), init_pos=np.array([0, 3, 0])) 75 | # define the data capture 76 | record = MemoryStore.DataRecord() 77 | record.clear() 78 | # define the first uav 79 | quad1 = Qfm.QuadModel(uavPara, simPara) 80 | # define the second uav 81 | quad2 = Qfm.QuadModel(uavPara, simPara) 82 | # gui init 83 | gui = Qfg.QuadrotorFlyGui([quad1, quad2]) 84 | 85 | # simulation begin 86 | for i in range(1000): 87 | # set the reference 88 | ref = np.array([0., 0., 1., 0.]) 89 | 90 | # update the first uav 91 | stateTemp = quad1.observe() 92 | action2, oil = quad1.get_controller_pid(stateTemp, ref) 93 | quad1.step(action2) 94 | # update the second uav 95 | action2, oil2 = quad2.get_controller_pid(quad2.observe(), ref) 96 | quad2.step(action2) 97 | 98 | # gui render 99 | gui.render() 100 | 101 | # store data 102 | record.buffer_append((stateTemp, action2)) 103 | 104 | # Data_recorder 0.3+ store episode data with independent deque 105 | record.episode_append() 106 | 107 | # draw result 108 | data = record.get_episode_buffer() 109 | bs = data[0] 110 | ba = data[1] 111 | t = range(0, record.count) 112 | fig1 = plt.figure(2) 113 | plt.clf() 114 | # draw position 115 | plt.subplot(3, 1, 1) 116 | plt.plot(t, bs[t, 6] / D2R, label='roll') 117 | plt.plot(t, bs[t, 7] / D2R, label='pitch') 118 | plt.plot(t, bs[t, 8] / D2R, label='yaw') 119 | plt.ylabel('Attitude $(\circ)$', fontsize=15) 120 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 121 | # draw position 122 | plt.subplot(3, 1, 2) 123 | plt.plot(t, bs[t, 0], label='x') 124 | plt.plot(t, bs[t, 1], label='y') 125 | plt.ylabel('Position (m)', fontsize=15) 126 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 127 | # draw altitude 128 | plt.subplot(3, 1, 3) 129 | plt.plot(t, bs[t, 2], label='z') 130 | plt.ylabel('Altitude (m)', fontsize=15) 131 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 132 | plt.show() 133 | 134 | elif testFlag == TestPara.Test_Module_Dynamic_Sensor: 135 | # from QuadrotorFly import QuadrotorFlyModel as Qfm 136 | q1 = Qfm.QuadModel(Qfm.QuadParas(), Qfm.QuadSimOpt(init_mode=Qfm.SimInitType.fixed, enable_sensor_sys=True, 137 | init_pos=np.array([5, -4, 0]), init_att=np.array([0, 0, 5]))) 138 | # init the estimator 139 | s1 = StateEstimator.KalmanFilterSimple() 140 | # set the init state of estimator 141 | s1.reset(q1.state) 142 | # simulation period 143 | t = np.arange(0, 30, 0.01) 144 | ii_len = len(t) 145 | stateRealArr = np.zeros([ii_len, 12]) 146 | stateEstArr = np.zeros([ii_len, 12]) 147 | meaArr = np.zeros([ii_len, 3]) 148 | 149 | # set the bias 150 | s1.gyroBias = q1.imu0.gyroBias 151 | s1.accBias = q1.imu0.accBias 152 | s1.magRef = q1.mag0.para.refField 153 | print(s1.gyroBias, s1.accBias) 154 | 155 | for ii in range(ii_len): 156 | # wait for start 157 | if ii < 100: 158 | sensor_data1 = q1.observe() 159 | _, oil = q1.get_controller_pid(q1.state) 160 | action = np.ones(4) * oil 161 | q1.step(action) 162 | stateEstArr[ii] = s1.update(sensor_data1, q1.ts) 163 | stateRealArr[ii] = q1.state 164 | else: 165 | sensor_data1 = q1.observe() 166 | action, oil = q1.get_controller_pid(s1.state, np.array([0, 0, 3, 0])) 167 | q1.step(action) 168 | stateEstArr[ii] = s1.update(sensor_data1, q1.ts) 169 | stateRealArr[ii] = q1.state 170 | import matplotlib.pyplot as plt 171 | plt.figure(1) 172 | ylabelList = ['roll', 'pitch', 'yaw', 'rate_roll', 'rate_pit', 'rate_yaw'] 173 | for ii in range(6): 174 | plt.subplot(6, 1, ii + 1) 175 | plt.plot(t, stateRealArr[:, 6 + ii] / D2R, '-b', label='real') 176 | plt.plot(t, stateEstArr[:, 6 + ii] / D2R, '-g', label='est') 177 | plt.legend() 178 | plt.ylabel(ylabelList[ii]) 179 | # plt.show() 180 | 181 | ylabelList = ['p_x', 'p_y', 'p_z', 'vel_x', 'vel_y', 'vel_z'] 182 | plt.figure(2) 183 | for ii in range(6): 184 | plt.subplot(6, 1, ii + 1) 185 | plt.plot(t, stateRealArr[:, ii], '-b', label='real') 186 | plt.plot(t, stateEstArr[:, ii], '-g', label='est') 187 | plt.legend() 188 | plt.ylabel(ylabelList[ii]) 189 | plt.show() 190 | elif testFlag == TestPara.Test_Module_Dynamic_CamDown: 191 | import matplotlib.pyplot as plt 192 | from QuadrotorFlyModel import QuadModel, QuadSimOpt, QuadParas, StructureType, SimInitType 193 | D2R = np.pi / 180 194 | video_write_flag = True 195 | 196 | print("PID controller test: ") 197 | uavPara = QuadParas(structure_type=StructureType.quad_x) 198 | simPara = QuadSimOpt(init_mode=SimInitType.fixed, enable_sensor_sys=False, 199 | init_att=np.array([5., -5., 0]), init_pos=np.array([5, -5, 0])) 200 | quad1 = QuadModel(uavPara, simPara) 201 | record = MemoryStore.DataRecord() 202 | record.clear() 203 | step_cnt = 0 204 | 205 | # init the camera 206 | cam1 = CamDown.CamDown(render_mode=CamDown.CamDownPara.Render_Mode_Gpu) 207 | cam1.load_ground_img() 208 | print('Load img completed!') 209 | if video_write_flag: 210 | v_format = cv2.VideoWriter_fourcc(*'MJPG') 211 | out1 = cv2.VideoWriter('Data/img/test.avi', v_format, 1 / quad1.uavPara.ts, (cam1.imgVertical, cam1.imgHorizon)) 212 | for i in range(1000): 213 | if i == 0: 214 | time_start = time.time() 215 | ref = np.array([0., 0., 3., 0.]) 216 | stateTemp = quad1.observe() 217 | # get image 218 | pos_0 = quad1.position * 1000 219 | att_0 = quad1.attitude 220 | img1 = cam1.get_img_by_state(pos_0, att_0) 221 | # file_name = 'Data/img/test_' + str(i) + '.jpg' 222 | # cv2.imwrite(file_name, img1) 223 | if video_write_flag: 224 | out1.write(img1) 225 | 226 | action2, oil = quad1.get_controller_pid(stateTemp, ref) 227 | print('action: ', action2) 228 | action2 = np.clip(action2, 0.1, 0.9) 229 | quad1.step(action2) 230 | record.buffer_append((stateTemp, action2)) 231 | step_cnt = step_cnt + 1 232 | time_end = time.time() 233 | print('time cost:', str(time_end - time_start)) 234 | record.episode_append() 235 | if video_write_flag: 236 | out1.release() 237 | 238 | print('Quadrotor structure type', quad1.uavPara.structureType) 239 | # quad1.reset_states() 240 | print('Quadrotor get reward:', quad1.get_reward()) 241 | data = record.get_episode_buffer() 242 | bs = data[0] 243 | ba = data[1] 244 | t = range(0, record.count) 245 | # mpl.style.use('seaborn') 246 | fig1 = plt.figure(1) 247 | plt.clf() 248 | plt.subplot(3, 1, 1) 249 | plt.plot(t, bs[t, 6] / D2R, label='roll') 250 | plt.plot(t, bs[t, 7] / D2R, label='pitch') 251 | plt.plot(t, bs[t, 8] / D2R, label='yaw') 252 | plt.ylabel('Attitude $(\circ)$', fontsize=15) 253 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 254 | plt.subplot(3, 1, 2) 255 | plt.plot(t, bs[t, 0], label='x') 256 | plt.plot(t, bs[t, 1], label='y') 257 | plt.ylabel('Position (m)', fontsize=15) 258 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 259 | plt.subplot(3, 1, 3) 260 | plt.plot(t, bs[t, 2], label='z') 261 | plt.ylabel('Altitude (m)', fontsize=15) 262 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 263 | plt.show() 264 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # QuadrotorFly四旋翼无人机动力学模型 2 | 3 | 主要目的是开发一个用于无人机动力学仿真的简单易用、功能相对齐全的仿真环境（也许是水论文环境）。 4 | 5 | # 0.3 版本主要更新 6 | 7 | 1. 加入了下视摄像头，假设在无人机的质心位置存在一个垂直机体并向下的摄像头，用来观测地面的图像和纹理，可以通过这个来试验光流等视觉定位算法 8 | 9 | 2. 下视摄像头图像的渲染提供了内存，cpu，gpu三种模式，内存就是把整个地面的图像全部放在内存里（会占用1G左右内存），cpu/gpu是通过贴图渲染，实验发现gpu渲染的效率是cpu的20倍左右（测试环境，i7-8086，gtx1080） 10 | 11 | 3. 调用的参考例子在Test目录下的Test_fullWithCamdown.py （有详细的注释） 12 | 13 | 4. 测试可能需要的地板图片和停机坪图片在这里 14 | 15 | [地板图片][http://qiniu.xiaobolin.cn/QuadrotorFlyGit/groundImgSmall.jpg] 16 | 17 | [停机坪图片][http://qiniu.xiaobolin.cn/QuadrotorFlyGit/landingMark.jpg] 18 | 19 | 5. 缺乏光流算法验证测试，可惜我不会呀。。。。。。。 20 | 21 | # 0.2 版本主要更新 22 | 详情见[传感器系统](Doc/SensorSystem.md) 23 | 1. 加入了传感器子系统（默认不使能），包含IMU（陀螺仪+加速度计），GPS，compass（磁力计）， 24 | 1. 加入了一个简单的卡尔曼滤波估计器。 25 | 1. 加入了时间系统，即无人机有一个自己的内部时间，每次迭代会累计采样周期，reset回导致时间归零。 26 | 2. 参考例子在StateEstimator.py和Test_fullWithSensor.py 27 | 3. 可以通过继承SensorBase来完成自定义传感器, 通过继承StateEstimatorBase来实现 28 | 29 | # 主要功能（已实现） 30 | 31 | ## 模型功能 32 | 33 | - [四旋翼基本动力学模型](#四旋翼基本动力学模型)，即电机推力到角速度、速度的动力学模型。 34 | - [电机动力学模型](#电机动力学模型)，简化成一阶惯性系统，控制输入为百分比（即0-1的小数）。 35 | - 系统干扰，作用在基本动力学模型上的均值可设定的随机噪声 36 | - 机型选择，**'x'** 型或 **'+'** 型。见[常见机型](#常见四旋翼无人机机型) 37 | - 提供了常见控制器以供参考，现有PD控制器 38 | - 提供了奖励函数以供学习算法使用 39 | - 状态边界检查，超出最大值后finish标志变成True 40 | - 传感器系统（imu、gps、compass） 41 | - 下视摄像头，可以用于视觉导航【新】 42 | - 时间戳 43 | 44 | ## 仿真功能 45 | 46 | - 支持随机初始位置、固定初始位置两种启动模式 47 | - 采样时间可设定，状态更新方式采用4阶龙格库塔方法 48 | - GUI动画，目前基于matplotlib搭建，支持多机 49 | 50 | # 环境与依赖 51 | ## 环境 52 | python3 + 你喜欢的编辑器 53 | 新手推荐anconda+spyder简单粗暴，入门建议anaconda+jupyter快捷稳定逼格高，想长期学习的建议anaconda+pycharm门槛高功能强大到难以想象。 54 | ## 需要的库 55 | - numpy 56 | - matplotlib 57 | - numba (0.3 后需要) 58 | - opencv (0.3 后需要) 59 | 60 | ## 我使用的环境 61 | - win10 + Anaconda （python 3.6）+ Pycharm 62 | 63 | # 使用教程 64 | 这些程序在pycharm、spyder下运行通过。jupyter的测试程序是工程目录下的TestInJupyter.ipynb，模型可以运行,不过gui动画不会动，暂时还没有修复。使用过程中有什么建议或者问题可以联系我729527658@qq.com. 65 | 66 | ## 测试 67 | 下载解压（建议使用git克隆）后运行其中的 **QuadrotorFlyTest.py**文件。 68 | git克隆指令 69 | ```bash 70 | git clone https://github.com/linxiaobo110/QuadrotorFly.git 71 | ``` 72 | 73 | 成功运行可以看到以下效果： 74 | ![test_3d](Doc/Images/test_3d.png) 75 | 运行结束后在画面单击可以看整个过程的飞行曲线 76 | ![test_2d](Doc/Images/test_2d.png) 77 | 78 | ## 最简实现 79 | 不画图和记录数据，就是了解整个调用流程。可以在当前目录里新建一个Test_simple.py（这个代码在Test文件夹里有），然后放入以下代码: 80 | ```python 81 | # 包含头文件 82 | import numpy as np 83 | import QuadrotorFlyModel as Qfm 84 | # 定义无人机参数 85 | uavPara = Qfm.QuadParas() 86 | # 定义仿真的参数 87 | simPara = Qfm.QuadSimOpt() 88 | # 使用参数新建一个无人机 89 | quad1 = Qfm.QuadModel(uavPara, simPara) 90 | 91 | # 仿真循环开始，总共1000步 92 | print("Simplest simulation begin!") 93 | for i in range(1000): 94 | # 设定目标，分别是x,y,z的位置，和偏航角的角度 95 | ref = np.array([0., 0., 1., 0.]) 96 | # 获取无人机的状态， 97 | # 共12个维度分别是，xyz位置，xyz速度，roll pitch yaw姿态角，roll pitch yaw 角速度 98 | stateTemp = quad1.observe() 99 | # 通过无人机状态计算控制量，这也是自行设计控制器应修改的地方 100 | # 这是一个内置的控制器，返回值是给各个电机的控制量即油门的大小 101 | action2, oil = quad1.get_controller_pid(stateTemp, ref) 102 | # 使用控制量更新无人机，控制量是4维向量，范围0-1 103 | quad1.step(action2) 104 | 105 | print("Simulation finish!") 106 | ``` 107 | 108 | 遇到“No module name ‘QuadrotorFlyGui’ ”或者 “No module name ‘QuadrotorFlyModel’”，见[工程环境设置错误](#工程环境设置错误)。 109 | 110 | ## 自定义仿真参数 111 | 在工程下新建Test_full.py(这个代码在Test文件夹里有)，然后复制以下代码： 112 | 113 | ```python 114 | # 包含文件 115 | import numpy as np 116 | import QuadrotorFlyModel as Qfm 117 | import QuadrotorFlyGui as Qfg 118 | import MemoryStore 119 | import matplotlib.pyplot as plt 120 | # 角度到弧度的转换 121 | D2R = Qfm.D2R 122 | # 仿真参数设置 123 | simPara = Qfm.QuadSimOpt( 124 | # 初值重置方式（随机或者固定）；姿态初值参数（随机就是上限，固定就是设定值）；位置初值参数（同上） 125 | init_mode=Qfm.SimInitType.rand, init_att=np.array([5., 5., 5.]), init_pos=np.array([1., 1., 1.]), 126 | # 仿真运行的最大位置，最大速度，最大姿态角（角度，不是弧度注意），最大角速度（角度每秒） 127 | max_position=8, max_velocity=8, max_attitude=180, max_angular=200, 128 | # 系统噪声，分别在位置环和速度环 129 | sysnoise_bound_pos=0, sysnoise_bound_att=0, 130 | #执行器模式，简单模式没有电机动力学， 131 | actuator_mode=Qfm.ActuatorMode.dynamic 132 | ) 133 | # 无人机参数设置，可以为不同的无人机设置不同的参数 134 | uavPara = Qfm.QuadParas( 135 | # 重力加速度；采样时间；机型plus或者x 136 | g=9.8, tim_sample=0.01, structure_type=Qfm.StructureType.quad_plus, 137 | # 无人机臂长（米）；质量（千克）；飞机绕三个轴的转动惯量（千克·平方米） 138 | uav_l=0.45, uav_m=1.5, uav_ixx=1.75e-2, uav_iyy=1.75e-2, uav_izz=3.18e-2, 139 | # 螺旋桨推力系数（牛每平方（弧度每秒）），螺旋桨扭力系数（牛·米每平方（弧度每秒）），旋翼转动惯量， 140 | rotor_ct=1.11e-5, rotor_cm=1.49e-7, rotor_i=9.9e-5, 141 | # 电机转速比例参数（度每秒），电机转速偏置参数（度每秒），电机相应时间（秒） 142 | rotor_cr=646, rotor_wb=166, rotor_t=1.36e-2 143 | ) 144 | # 使用参数新建无人机 145 | quad1 = Qfm.QuadModel(uavPara, simPara) 146 | 147 | # 初始化GUI，并添加无人机 148 | gui = Qfg.QuadrotorFlyGui([quad1]) 149 | 150 | # 初始化记录器，用于记录飞行数据 151 | record = MemoryStore.DataRecord() 152 | 153 | # 重置系统 154 | # 当需要跑多次重复实验时注意重置系统， 155 | quad1.reset_states() 156 | record.clear() 157 | 158 | # 仿真过程 159 | print("Simplest simulation begin!") 160 | for i in range(1000): 161 | # 模型更新 162 | # 设置目标 163 | ref = np.array([0., 0., 1., 0.]) 164 | # 获取无人机的状态，共12维的向量 165 | # 分别是，xyz位置，xyz速度，roll pitch yaw姿态角，roll pitch yaw 角速度 166 | stateTemp = quad1.observe() 167 | # 通过无人机状态计算控制量，这也是自行设计控制器应修改的地方 168 | # 这是一个内置的控制器，返回值是给各个电机的控制量即油门的大小 169 | action2, oil = quad1.get_controller_pid(stateTemp, ref) 170 | # 使用控制量更新无人机，控制量是4维向量，范围0-1 171 | quad1.step(action2) 172 | 173 | # 记录数据 174 | record.buffer_append((stateTemp, action2)) 175 | 176 | # 渲染GUI 177 | gui.render() 178 | 179 | record.episode_append() 180 | # 输出结果 181 | ## 获取记录中的状态 182 | data = record.get_episode_buffer() 183 | bs = data[0] 184 | ## 获取记录中的控制量 185 | ba = data[1] 186 | 187 | ## 生成时间序列 188 | t = range(0, record.count) 189 | ## 画图 190 | fig1 = plt.figure(2) 191 | plt.clf() 192 | ## 姿态图 193 | plt.subplot(3, 1, 1) 194 | plt.plot(t, bs[t, 6] / D2R, label='roll') 195 | plt.plot(t, bs[t, 7] / D2R, label='pitch') 196 | plt.plot(t, bs[t, 8] / D2R, label='yaw') 197 | plt.ylabel('Attitude $(\circ)$', fontsize=15) 198 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 199 | ## 位置图(xy) 200 | plt.subplot(3, 1, 2) 201 | plt.plot(t, bs[t, 0], label='x') 202 | plt.plot(t, bs[t, 1], label='y') 203 | plt.ylabel('Position (m)', fontsize=15) 204 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 205 | ## 高度图 206 | plt.subplot(3, 1, 3) 207 | plt.plot(t, bs[t, 2], label='z') 208 | plt.ylabel('Altitude (m)', fontsize=15) 209 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 210 | print("Simulation finish!") 211 | ``` 212 | 213 | 在spyder下的运行效果： 214 | ![spyder_effect](Doc/Images/spyder_effect.png) 215 | 216 | 如果遇到spyder运行时，图片在终端里显示（就是没有弹出新窗口），见[Spyder图形渲染设置](#spyder图形渲染设置)。 217 | 218 | # 基础知识 219 | 220 | ## 常见四旋翼无人机机型 221 | 222 | - **+** 型四旋翼无人机 223 | 224 | ![QuadRotorPlus](Doc/Images/QuadRotorPlus.png) 225 | 226 | - **X** 型四旋翼无人机 227 | 228 | ![QuadRotorX](Doc/Images/QuadRotorX.png) 229 | 230 | ## 四旋翼基本动力学模型 231 | 232 | 233 | 234 | 其中$p_x,p_y,p_z$ 是位置，$\varphi,\theta,\psi$是姿态，$\tau_{0，1,2,3}$分别是总体推力，绕x轴、y轴，z轴的扭力。 235 | 236 | ## 电机动力学模型 237 | 238 | 239 | 240 | 其中$\omega$是电机的转速；$u$是输入给电机的控制信号；$T,M$分别是电机产生的推力和扭力。 241 | 242 | ## 动力学中的力与螺旋桨产生的力关系 243 | 以十型举例 244 | 245 | 246 | 247 | # FAQ 248 | ## 工程环境设置错误 249 | 250 | - 现象： 251 | ModuleNotFoundError: No module named 'QuadrotorFlyGui' 252 | - 原因： 253 | 这是因为工程运行的目录不是QuadrotorFly目录，比如在Test里，或者是在一个上级的目录。 254 | - 解决办法 255 | 256 | 如果上级目录，修改import语句成 257 | ```python 258 | # 其他类似 259 | import QuadrotorFly.QuadrotorFlyGui 260 | ``` 261 | 262 | 如果是在Test里，或者不确定在哪里，直接修改程序运行目录 263 | 1. spyder编辑器 264 | 选择为每个文件单独指定配置文件 265 | ![dir_spyder](Doc/Images/dir_spyder.png) 266 | 选择指定文件夹 267 | ![dir_spyderSelect](Doc/Images/dir_spyderSelect.png) 268 | 选择QuadrotorFly目录 269 | ![dir_spyder_set](Doc/Images/dir_spyder_set.png) 270 | 271 | ## Spyder图形渲染设置 272 | 选择tool->preferences 273 | ![spyder_qt5set1](Doc/Images/spyder_qt5set1.png) 274 | 设置合适的渲染工具，不是inline 275 | ![spyder_qt5set2](Doc/Images/spyder_qt5set2.png) 276 | 277 | 278 | -------------------------------------------------------------------------------- /SensorBase.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """'abstract class for sensors, define the general call interface' 4 | 5 | By xiaobo 6 | Contact linxiaobo110@gmail.com 7 | Created on 六月 20 22:35 2019 8 | """ 9 | 10 | # Copyright (C) 11 | # 12 | # This file is part of QuadrotorFly 13 | # 14 | # GWpy is free software: you can redistribute it and/or modify 15 | # it under the terms of the GNU General Public License as published by 16 | # the Free Software Foundation, either version 3 of the License, or 17 | # (at your option) any later version. 18 | # 19 | # GWpy is distributed in the hope that it will be useful, 20 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 21 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 22 | # GNU General Public License for more details. 23 | # 24 | # You should have received a copy of the GNU General Public License 25 | # along with GWpy. If not, see . 26 | 27 | 28 | import numpy as np 29 | import enum 30 | from enum import Enum 31 | import abc 32 | 33 | """ 34 | ******************************************************************************************************** 35 | **------------------------------------------------------------------------------------------------------- 36 | ** Compiler : python 3.6 37 | ** Module Name: SensorBase 38 | ** Module Date: 2019/6/20 39 | ** Module Auth: xiaobo 40 | ** Version : V0.1 41 | ** Description: 'Replace the content between' 42 | **------------------------------------------------------------------------------------------------------- 43 | ** Reversion : 44 | ** Modified By: 45 | ** Date : 46 | ** Content : 47 | ** Notes : 48 | ********************************************************************************************************/ 49 | """ 50 | 51 | 52 | class SensorType(Enum): 53 | """Define the sensor types""" 54 | none = enum.auto() 55 | imu = enum.auto() 56 | compass = enum.auto() 57 | gps = enum.auto() 58 | 59 | 60 | class SensorBase(object, metaclass=abc.ABCMeta): 61 | """Define the abstract sensor_base class""" 62 | sensorType = SensorType.none 63 | 64 | def __init__(self): 65 | super(SensorBase, self).__init__() 66 | # the update tick of last one 67 | self._lastTick = 0 68 | self._isUpdated = False 69 | 70 | @property 71 | def last_tick(self): 72 | """the update tick of last one""" 73 | return self._lastTick 74 | 75 | @property 76 | def is_updated(self): 77 | return self._isUpdated 78 | 79 | def observe(self): 80 | """return the sensor data""" 81 | pass 82 | 83 | def update(self, real_state, ts): 84 | """Calculating the output data of sensor according to real state of vehicle, 85 | the difference between update and get_data is that this method will be called when system update, 86 | but the get_data is called when user need the sensor data. 87 | :param real_state: real system state from vehicle 88 | :param ts: the system tick 89 | """ 90 | pass 91 | 92 | def reset(self, real_state): 93 | """reset the sensor""" 94 | pass 95 | 96 | def get_name(self): 97 | """get the name of sensor, format: type:model-no""" 98 | pass 99 | -------------------------------------------------------------------------------- /SensorCompass.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """Implement the sensor details about compass 4 | 5 | By xiaobo 6 | Contact linxiaobo110@gmail.com 7 | Created on 六月 23 11:24 2019 8 | """ 9 | 10 | # Copyright (C) 11 | # 12 | # This file is part of QuadrotorFly 13 | # 14 | # GWpy is free software: you can redistribute it and/or modify 15 | # it under the terms of the GNU General Public License as published by 16 | # the Free Software Foundation, either version 3 of the License, or 17 | # (at your option) any later version. 18 | # 19 | # GWpy is distributed in the hope that it will be useful, 20 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 21 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 22 | # GNU General Public License for more details. 23 | # 24 | # You should have received a copy of the GNU General Public License 25 | # along with GWpy. If not, see . 26 | 27 | 28 | import numpy as np 29 | from SensorBase import SensorBase, SensorType 30 | import CommonFunctions as Cf 31 | 32 | """ 33 | ******************************************************************************************************** 34 | **------------------------------------------------------------------------------------------------------- 35 | ** Compiler : python 3.6 36 | ** Module Name: SensorCompass 37 | ** Module Date: 2019/6/23 38 | ** Module Auth: xiaobo 39 | ** Version : V0.1 40 | ** Description: 'Replace the content between' 41 | **------------------------------------------------------------------------------------------------------- 42 | ** Reversion : 43 | ** Modified By: 44 | ** Date : 45 | ** Content : 46 | ** Notes : 47 | ********************************************************************************************************/ 48 | """ 49 | 50 | 51 | class CompassPara(object): 52 | def __init__(self, max_update_frequency=50, start_delay=0, latency=0, name="compass", 53 | accuracy=0.5): 54 | """ 55 | :param max_update_frequency: max-update-frequency supported, Hz 56 | :param start_delay: the sensor start after this time, s 57 | :param latency: the state captured is indeed the state before, s 58 | :param name: the name of sensor, 59 | :param accuracy: the accuracy, uT 60 | """ 61 | self.minTs = 1 / max_update_frequency 62 | self.startDelay = start_delay 63 | self.latency = latency 64 | self.name = name 65 | self.accuracy = accuracy 66 | 67 | # the world-frame used in QuadrotorFly is East-North-Sky 68 | # varying magnetic filed or fixed one 69 | self.refFlagFixed = True 70 | self.refField = np.array([9.805, 34.252, -93.438]) 71 | 72 | 73 | class SensorCompass(SensorBase): 74 | 75 | def __init__(self, para=CompassPara()): 76 | """ 77 | :param para: 78 | """ 79 | SensorBase.__init__(self) 80 | self.para = para 81 | self.sensorType = SensorType.compass 82 | self.magMea = np.zeros(3) 83 | 84 | def observe(self): 85 | """return the sensor data""" 86 | return self._isUpdated, self.magMea 87 | 88 | def update(self, real_state, ts): 89 | """Calculating the output data of sensor according to real state of vehicle, 90 | the difference between update and get_data is that this method will be called when system update, 91 | but the get_data is called when user need the sensor data. 92 | the real_state here should be a 12 degree vector, 93 | :param real_state: 94 | 0 1 2 3 4 5 95 | p_x p_y p_z v_x v_y v_z 96 | 6 7 8 9 10 11 97 | roll pitch yaw v_roll v_pitch v_yaw 98 | :param ts: system tick now 99 | """ 100 | 101 | # process the update period 102 | if (ts - self._lastTick) >= self.para.minTs: 103 | self._isUpdated = True 104 | self._lastTick = ts 105 | else: 106 | self._isUpdated = False 107 | 108 | if self._isUpdated: 109 | # Magnetic sensor 110 | mag_world = self.para.refField 111 | rot_matrix = Cf.get_rotation_inv_matrix(real_state[6:9]) 112 | acc_body = np.dot(rot_matrix, mag_world) 113 | noise_mag = (1 * np.random.random(3) - 0.5) * np.sqrt(self.para.accuracy) 114 | self.magMea = acc_body + noise_mag 115 | else: 116 | # keep old 117 | pass 118 | 119 | return self.observe() 120 | 121 | def reset(self, real_state): 122 | """reset the sensor""" 123 | self._lastTick = 0 124 | 125 | def get_name(self): 126 | """get the name of sensor, format: type:model-no""" 127 | return self.para.name 128 | 129 | 130 | if __name__ == '__main__': 131 | " used for testing this module" 132 | D2R = Cf.D2R 133 | testFlag = 1 134 | if testFlag == 1: 135 | from QuadrotorFly import QuadrotorFlyModel as Qfm 136 | q1 = Qfm.QuadModel(Qfm.QuadParas(), Qfm.QuadSimOpt(init_mode=Qfm.SimInitType.fixed, 137 | init_att=np.array([5, 6, 8]))) 138 | s1 = SensorCompass() 139 | t = np.arange(0, 10, 0.01) 140 | ii_len = len(t) 141 | stateArr = np.zeros([ii_len, 12]) 142 | meaArr = np.zeros([ii_len, 3]) 143 | for ii in range(ii_len): 144 | state = q1.observe() 145 | action, oil = q1.get_controller_pid(state) 146 | q1.step(action) 147 | 148 | flag, meaArr[ii] = s1.update(state, q1.ts) 149 | stateArr[ii] = state 150 | 151 | estArr = np.zeros(ii_len) 152 | for i, x in enumerate(meaArr): 153 | temp = Cf.get_rotation_matrix(stateArr[i, 6:9]) 154 | ref_temp = np.dot(temp, np.array([0, 0, s1.para.refField[2]])) 155 | # estArr[i] = np.arctan2(temp[0], temp[1]) 156 | mea_temp = meaArr[i, :] 157 | # mea_temp[0] -= s1.para.refField[2] * np.sin(stateArr[i, 7]) 158 | # mea_temp[1] -= s1.para.refField[2] * np.sin(stateArr[i, 6]) 159 | print(mea_temp, ref_temp) 160 | mag_body1 = mea_temp[1] * np.cos(stateArr[i, 6]) - mea_temp[2] * np.sin(stateArr[i, 6]) 161 | mag_body2 = (mea_temp[0] * np.cos(stateArr[i, 7]) + 162 | mea_temp[1] * np.sin(stateArr[i, 6]) * np.sin(stateArr[i, 7]) + 163 | mea_temp[2] * np.cos(stateArr[i, 6]) * np.sin(stateArr[i, 7])) 164 | if (mag_body1 != 0) and (mag_body2 != 0): 165 | estArr[i] = np.arctan2(-mag_body1, mag_body2) + 90 * D2R - 16 * D2R 166 | 167 | print((estArr[100] - stateArr[100, 9]) / D2R) 168 | import matplotlib.pyplot as plt 169 | plt.figure(3) 170 | plt.plot(t, stateArr[:, 8] / D2R, '-b', label='real') 171 | plt.plot(t, estArr / D2R, '-g', label='mea') 172 | plt.show() 173 | # plt.plot(t, flagArr * 100, '-r', label='update flag') 174 | -------------------------------------------------------------------------------- /SensorGps.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """Implement the sensor details about Gps 4 | 5 | By xiaobo 6 | Contact linxiaobo110@gmail.com 7 | Created on 六月 21 22:59 2019 8 | """ 9 | 10 | # Copyright (C) 11 | # 12 | # This file is part of QuadrotorFly 13 | # 14 | # GWpy is free software: you can redistribute it and/or modify 15 | # it under the terms of the GNU General Public License as published by 16 | # the Free Software Foundation, either version 3 of the License, or 17 | # (at your option) any later version. 18 | # 19 | # GWpy is distributed in the hope that it will be useful, 20 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 21 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 22 | # GNU General Public License for more details. 23 | # 24 | # You should have received a copy of the GNU General Public License 25 | # along with GWpy. If not, see . 26 | 27 | 28 | import numpy as np 29 | from SensorBase import SensorBase, SensorType 30 | import queue 31 | 32 | """ 33 | ******************************************************************************************************** 34 | **------------------------------------------------------------------------------------------------------- 35 | ** Compiler : python 3.6 36 | ** Module Name: SensorGps 37 | ** Module Date: 2019/6/21 38 | ** Module Auth: xiaobo 39 | ** Version : V0.1 40 | ** Description: 'Replace the content between' 41 | **------------------------------------------------------------------------------------------------------- 42 | ** Reversion : 43 | ** Modified By: 44 | ** Date : 45 | ** Content : 46 | ** Notes : 47 | ********************************************************************************************************/ 48 | """ 49 | 50 | 51 | class GpsPara(object): 52 | def __init__(self, max_update_frequency=10, start_delay=1, latency=0.2, name="gps", 53 | accuracy_horizontal=2.5): 54 | """ 55 | :param max_update_frequency: max-update-frequency supported, Hz 56 | :param start_delay: the sensor start after this time, s 57 | :param latency: the state captured is indeed the state before, s 58 | :param name: the name of sensor, 59 | :param accuracy_horizontal: the accuracy, m 60 | """ 61 | self.minTs = 1 / max_update_frequency 62 | self.name = name 63 | self.startDelay = start_delay 64 | # important, the latency is assumed to be larger than minTs 65 | self.latency = latency 66 | self.accuracyHorizon = accuracy_horizontal 67 | 68 | 69 | class SensorGps(SensorBase): 70 | def __init__(self, para=GpsPara()): 71 | SensorBase.__init__(self) 72 | self.sensorType = SensorType.gps 73 | self.para = para 74 | self._posMea = np.zeros(3) 75 | # the history data is used to implement the latency 76 | self._posHisReal = queue.Queue() 77 | 78 | def observe(self): 79 | """return the sensor data""" 80 | return self._isUpdated, self._posMea 81 | 82 | def update(self, real_state, ts): 83 | """Calculating the output data of sensor according to real state of vehicle, 84 | the difference between update and get_data is that this method will be called when system update, 85 | but the get_data is called when user need the sensor data. 86 | the real_state here should be a 12 degree vector, 87 | :param real_state: 88 | 0 1 2 3 4 5 89 | p_x p_y p_z v_x v_y v_z 90 | 6 7 8 9 10 11 91 | roll pitch yaw v_roll v_pitch v_yaw 92 | :param ts: system tick now 93 | """ 94 | # process the latency 95 | real_state_latency = np.zeros(3) 96 | if ts < self.para.latency: 97 | self._posHisReal.put(real_state) 98 | else: 99 | self._posHisReal.put(real_state) 100 | real_state_latency = self._posHisReal.get() 101 | 102 | # process the star_time 103 | if ts < self.para.startDelay: 104 | self._posMea = np.zeros(3) 105 | self._isUpdated = False 106 | else: 107 | # process the update period 108 | if (ts - self._lastTick) >= self.para.minTs: 109 | self._isUpdated = True 110 | self._lastTick = ts 111 | else: 112 | self._isUpdated = False 113 | 114 | if self._isUpdated: 115 | noise_gps = (1 * np.random.random(3) - 0.5) * self.para.accuracyHorizon 116 | self._posMea = real_state_latency[0:3] + noise_gps 117 | else: 118 | # keep old 119 | pass 120 | 121 | return self.observe() 122 | 123 | def reset(self, real_state): 124 | """reset the sensor""" 125 | self._lastTick = 0 126 | self._posMea = np.zeros(3) 127 | if not self._posHisReal.empty(): 128 | self._posHisReal.queue.clear() 129 | 130 | def get_name(self): 131 | """get the name of sensor, format: type:model-no""" 132 | return self.para.name 133 | 134 | 135 | if __name__ == '__main__': 136 | " used for testing this module" 137 | testFlag = 1 138 | if testFlag == 1: 139 | s1 = SensorGps() 140 | t1 = np.arange(0, 5, 0.01) 141 | nums = len(t1) 142 | vel = np.sin(t1) 143 | pos = np.zeros([nums, 3]) 144 | posMea = np.zeros([nums, 3]) 145 | flagArr = np.zeros([nums, 3]) 146 | for ii in range(nums): 147 | if ii > 0: 148 | pos[ii] = pos[ii - 1] + vel[ii] 149 | 150 | for ii in range(nums): 151 | flagArr[ii], posMea[ii] = s1.update(np.hstack([pos[ii], np.zeros(9)]), t1[ii]) 152 | 153 | import matplotlib.pyplot as plt 154 | plt.figure(1) 155 | plt.plot(t1, pos, '-b', label='real') 156 | plt.plot(t1, posMea, '-g', label='measure') 157 | plt.plot(t1, flagArr * 100, '-r', label='update flag') 158 | plt.legend() 159 | plt.show() 160 | -------------------------------------------------------------------------------- /SensorImu.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """Implement the sensor details about imu 4 | 5 | By xiaobo 6 | Contact linxiaobo110@gmail.com 7 | Created on 六月 21 10:33 2019 8 | """ 9 | 10 | # Copyright (C) 11 | # 12 | # This file is part of QuadrotorFly 13 | # 14 | # GWpy is free software: you can redistribute it and/or modify 15 | # it under the terms of the GNU General Public License as published by 16 | # the Free Software Foundation, either version 3 of the License, or 17 | # (at your option) any later version. 18 | # 19 | # GWpy is distributed in the hope that it will be useful, 20 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 21 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 22 | # GNU General Public License for more details. 23 | # 24 | # You should have received a copy of the GNU General Public License 25 | # along with GWpy. If not, see . 26 | 27 | 28 | import numpy as np 29 | # import QuadrotorFly.SensorBase as SensorBase 30 | from SensorBase import SensorBase, SensorType 31 | import CommonFunctions as Cf 32 | 33 | """ 34 | ******************************************************************************************************** 35 | **------------------------------------------------------------------------------------------------------- 36 | ** Compiler : python 3.6 37 | ** Module Name: SensorImu 38 | ** Module Date: 2019/6/21 39 | ** Module Auth: xiaobo 40 | ** Version : V0.1 41 | ** Description: this module referenced the source of sensor part in AirSim, data sheet of mpu6050 42 | **------------------------------------------------------------------------------------------------------- 43 | ** Reversion : 44 | ** Modified By: 45 | ** Date : 46 | ** Content : 47 | ** Notes : 48 | ********************************************************************************************************/ 49 | """ 50 | 51 | D2R = Cf.D2R 52 | g = 9.8 53 | 54 | 55 | class ImuPara(object): 56 | def __init__(self, gyro_zro_tolerance_init=5, gyro_zro_var=30, gyro_noise_sd=0.01, min_time_sample=0.01, 57 | acc_zgo_tolerance=60, acc_zg_var_temp=1.5, acc_noise_sd=300, name='imu' 58 | ): 59 | """ 60 | zro is zero rate output, sd is spectral density, zgo is zero g output 61 | :param gyro_zro_tolerance_init: the zero-bias of gyro, \deg/s 62 | :param gyro_zro_var: the noise variation in normal temperature, \deg/s 63 | :param gyro_noise_sd: the rate noise spectral density, \deg/s/\sqrt(Hz) 64 | :param acc_zgo_tolerance: the zeros of acc, mg 65 | :param acc_zg_var_temp: the noise variation vs temperature (-40~85), mg/(\degC) 66 | :param acc_noise_sd: the rate noise spectral density, mg\sqrt(Hz) 67 | :param name: the name of sensor 68 | :param min_time_sample: min sample time 69 | """ 70 | # transfer the unit for general define 71 | # transfer to \rad/s 72 | self.gyroZroToleranceInit = gyro_zro_tolerance_init * D2R 73 | self.gyroZroVar = gyro_zro_var * (D2R**2) 74 | self.gyroNoiseSd = gyro_noise_sd # i do not understand it, in fact 75 | # transfer to m/(s^2) 76 | self.accZroToleranceInit = acc_zgo_tolerance / 1000 * g 77 | std_temp = 1 / 1000 * g * 60 # assumed the temperature is 20 (\degC) here 78 | self.accZroVar = acc_zg_var_temp * (std_temp**2) 79 | self.accNoiseSd = acc_noise_sd # i do not understand it, in fact 80 | self.name = name 81 | self.minTs = min_time_sample 82 | 83 | 84 | mpu6050 = ImuPara(5, 30, 0.01) 85 | 86 | 87 | class SensorImu(SensorBase): 88 | 89 | def __init__(self, imu_para=mpu6050): 90 | """ 91 | :param imu_para: 92 | """ 93 | SensorBase.__init__(self) 94 | self.para = imu_para 95 | self.sensorType = SensorType.imu 96 | self.angularMea = np.zeros(3) 97 | self.gyroBias = (1 * np.random.random(3) - 0.5) * self.para.gyroZroToleranceInit 98 | self.accMea = np.zeros(3) 99 | self.accBias = (1 * np.random.random(3) - 0.5) * self.para.accZroToleranceInit 100 | 101 | def observe(self): 102 | """return the sensor data""" 103 | return self._isUpdated, np.hstack([self.accMea, self.angularMea]) 104 | 105 | def update(self, real_state, ts): 106 | """Calculating the output data of sensor according to real state of vehicle, 107 | the difference between update and get_data is that this method will be called when system update, 108 | but the get_data is called when user need the sensor data. 109 | the real_state here should be a 12 degree vector, 110 | :param real_state: 111 | 0 1 2 3 4 5 112 | p_x p_y p_z v_x v_y v_z 113 | 6 7 8 9 10 11 12 13 14 114 | roll pitch yaw v_roll v_pitch v_yaw a_x a_y a_z 115 | :param ts: system tick now 116 | """ 117 | 118 | # process the update period 119 | if (ts - self._lastTick) >= self.para.minTs: 120 | self._isUpdated = True 121 | self._lastTick = ts 122 | else: 123 | self._isUpdated = False 124 | 125 | if self._isUpdated: 126 | # gyro 127 | noise_gyro = (1 * np.random.random(3) - 0.5) * np.sqrt(self.para.gyroZroVar) 128 | self.angularMea = real_state[9:12] + noise_gyro + self.gyroBias 129 | 130 | # accelerator 131 | acc_world = real_state[12:15] * 0.2 + np.array([0, 0, -g]) 132 | # acc_world = np.array([0, 0, -g]) 133 | rot_matrix = Cf.get_rotation_inv_matrix(real_state[6:9]) 134 | acc_body = np.dot(rot_matrix, acc_world) 135 | noise_acc = (1 * np.random.random(3) - 0.5) * np.sqrt(self.para.gyroZroVar) 136 | # print(real_state[12:15], acc_body) 137 | self.accMea = acc_body + noise_acc + self.accBias 138 | else: 139 | # keep old 140 | pass 141 | 142 | return self.observe() 143 | 144 | def reset(self, real_state): 145 | """reset the sensor""" 146 | self._lastTick = 0 147 | 148 | def get_name(self): 149 | """get the name of sensor, format: type:model-no""" 150 | return self.para.name 151 | 152 | 153 | if __name__ == '__main__': 154 | " used for testing this module" 155 | testFlag = 2 156 | if testFlag == 1: 157 | s1 = SensorImu() 158 | flag1, v1 = s1.update(np.random.random(12), 0.1) 159 | flag2, v2 = s1.update(np.random.random(12), 0.105) 160 | print(flag1, "val", v1, flag2, "val", v2) 161 | 162 | elif testFlag == 2: 163 | from QuadrotorFly import QuadrotorFlyModel as Qfm 164 | q1 = Qfm.QuadModel(Qfm.QuadParas(), Qfm.QuadSimOpt(init_mode=Qfm.SimInitType.fixed, 165 | init_att=np.array([15, -20, 5]))) 166 | s1 = SensorImu() 167 | t = np.arange(0, 10, 0.01) 168 | ii_len = len(t) 169 | stateArr = np.zeros([ii_len, 12]) 170 | meaArr = np.zeros([ii_len, 6]) 171 | for ii in range(ii_len): 172 | state = q1.observe() 173 | action, oil = q1.get_controller_pid(state) 174 | q1.step(action) 175 | 176 | flag, meaArr[ii] = s1.update(np.hstack([state, q1.acc]), q1.ts) 177 | stateArr[ii] = state 178 | 179 | estArr = np.zeros([ii_len, 3]) 180 | estArrAcc = np.zeros([ii_len, 3]) 181 | for ii in range(ii_len): 182 | if ii > 0: 183 | angle_dot = (meaArr[ii, 3:6] - s1.gyroBias) * q1.uavPara.ts 184 | estArr[ii] = estArr[ii - 1] + angle_dot 185 | meaAccTemp = meaArr[ii, 0:3] - s1.accBias 186 | acc_sum1 = np.sqrt(np.square(meaAccTemp[1]) + np.square(meaAccTemp[2])) 187 | acc_sum2 = np.sqrt(np.square(meaAccTemp[0]) + np.square(meaAccTemp[2])) 188 | estArrAcc[ii, 0] = -np.arctan2(meaAccTemp[0], acc_sum1) 189 | estArrAcc[ii, 1] = np.arctan2(meaAccTemp[1], acc_sum2) 190 | 191 | import matplotlib.pyplot as plt 192 | # plt.figure(1) 193 | for ii in range(3): 194 | plt.subplot(3, 1, ii + 1) 195 | plt.plot(t, stateArr[:, 6 + ii] / D2R, '-b', label='real') 196 | plt.plot(t, estArr[:, ii] / D2R, '-g', label='gyro angle') 197 | plt.plot(t, estArrAcc[:, ii] / D2R, '-m', label='acc angle') 198 | plt.show() 199 | # plt.figure(2) 200 | # plt.plot(t, stateArr[:, 3:6], '-b', label='real') 201 | # plt.plot(t, meaArr[:, 0:3], '-g', label='measure') 202 | # plt.show() 203 | # plt.plot(t, flagArr * 100, '-r', label='update flag') 204 | -------------------------------------------------------------------------------- /StateEstimator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """This module is design a state estimator for quadrotor according the data from sensors 4 | 5 | By xiaobo 6 | Contact linxiaobo110@gmail.com 7 | Created on 六月 24 19:17 2019 8 | """ 9 | 10 | # Copyright (C) 11 | # 12 | # This file is part of QuadrotorFly 13 | # 14 | # GWpy is free software: you can redistribute it and/or modify 15 | # it under the terms of the GNU General Public License as published by 16 | # the Free Software Foundation, either version 3 of the License, or 17 | # (at your option) any later version. 18 | # 19 | # GWpy is distributed in the hope that it will be useful, 20 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 21 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 22 | # GNU General Public License for more details. 23 | # 24 | # You should have received a copy of the GNU General Public License 25 | # along with GWpy. If not, see . 26 | 27 | 28 | import numpy as np 29 | import QuadrotorFlyModel as Qfm 30 | import abc 31 | import CommonFunctions as Cf 32 | 33 | """ 34 | ******************************************************************************************************** 35 | **------------------------------------------------------------------------------------------------------- 36 | ** Compiler : python 3.6 37 | ** Module Name: StateEstimator 38 | ** Module Date: 2019/6/24 39 | ** Module Auth: xiaobo 40 | ** Version : V0.1 41 | ** Description: 'Replace the content between' 42 | **------------------------------------------------------------------------------------------------------- 43 | ** Reversion : 44 | ** Modified By: 45 | ** Date : 46 | ** Content : 47 | ** Notes : 48 | ********************************************************************************************************/ 49 | """ 50 | 51 | 52 | class StateEstimatorBase(object, metaclass=abc.ABCMeta): 53 | def __init__(self): 54 | super(StateEstimatorBase, self).__init__() 55 | self._name = "State estimator" 56 | 57 | def update(self, sensor_data, ts): 58 | """Calculating the output data of sensor according to real state of vehicle, 59 | the difference between update and get_data is that this method will be called when system update, 60 | but the get_data is called when user need the sensor data. 61 | :param sensor_data real system state from vehicle 62 | :param ts: the system tick 63 | """ 64 | pass 65 | 66 | def reset(self, state_init): 67 | pass 68 | 69 | @property 70 | def name(self): 71 | return self._name 72 | 73 | 74 | class KalmanFilterSimple(StateEstimatorBase): 75 | def __init__(self): 76 | StateEstimatorBase.__init__(self) 77 | self.state = np.zeros(12) 78 | self.imuTickPre = 0 79 | self.gpsTickPre = 0 80 | self.magTickPre = 0 81 | self.gyroBias = np.zeros(3) 82 | self.accBias = np.zeros(3) 83 | self.magRef = np.zeros(3) 84 | 85 | def reset(self, state_init): 86 | self.state = state_init 87 | 88 | def update(self, sensor_data, ts): 89 | # print(sensor_data) 90 | # print(sensor_data['imu']) 91 | # print() 92 | data_imu = sensor_data['imu'][1] 93 | data_gps = sensor_data['gps'][1] 94 | data_mag = sensor_data['compass'][1] 95 | 96 | angle_mea = np.zeros(3) 97 | # pos_pct = np.zeros(3) 98 | # angle_pct = np.zeros(3) 99 | # pos_mea = np.zeros(3) 100 | 101 | if sensor_data['imu'][0]: 102 | period_temp = ts - self.imuTickPre 103 | self.imuTickPre = ts 104 | 105 | # attitude estimation 106 | angle_pct = self.state[6:9] + (data_imu[3:6] - self.gyroBias) * period_temp 107 | 108 | angle_acc = np.zeros(3) 109 | mea_acc_temp = data_imu[0:3] - self.accBias 110 | acc_sum1 = np.sqrt(np.square(mea_acc_temp[1]) + np.square(mea_acc_temp[2])) 111 | acc_sum2 = np.sqrt(np.square(mea_acc_temp[0]) + np.square(mea_acc_temp[2])) 112 | angle_acc[1] = np.arctan2(mea_acc_temp[0], acc_sum1) 113 | angle_acc[0] = -np.arctan2(mea_acc_temp[1], acc_sum2) 114 | angle_mea[0:2] = angle_pct[0:2] + 0.1 * (angle_acc[0:2] - angle_pct[0:2]) 115 | self.state[6:8] = angle_mea[0:2] 116 | self.state[9:12] = data_imu[3:6] 117 | self.state[8] = angle_pct[2] 118 | 119 | # velocity estimation 120 | rot_matrix = Cf.get_rotation_inv_matrix(self.state[6:9]) 121 | acc_ext = np.dot(rot_matrix, mea_acc_temp) + np.array([0, 0, 9.8]) 122 | # print(acc_ext, mea_acc_temp) 123 | acc_ext[2] = mea_acc_temp[2] / np.cos(self.state[6]) / np.cos(self.state[7]) + 9.8 124 | self.state[3:5] = self.state[3:5] + acc_ext[0:2] * period_temp * 0.5 125 | self.state[0:2] = self.state[0:2] + self.state[3:5] * period_temp * 0.5 126 | self.state[5] = self.state[5] + acc_ext[2] * period_temp * 5 127 | self.state[2] = self.state[2] + self.state[5] * period_temp 128 | 129 | if sensor_data['compass'][0]: 130 | roll_cos = np.cos(self.state[6]) 131 | roll_sin = np.sin(self.state[6]) 132 | pitch_cos = np.cos(self.state[7]) 133 | pitch_sin = np.sin(self.state[7]) 134 | mag_body1 = data_mag[1] * roll_cos - data_mag[2] * roll_sin 135 | mag_body2 = (data_mag[0] * pitch_cos + 136 | data_mag[1] * roll_sin * pitch_sin + 137 | data_mag[2] * roll_cos * pitch_sin) 138 | if (mag_body1 != 0) and (mag_body2 != 0): 139 | angle_mag = np.arctan2(-mag_body1, mag_body2) + 90 * Cf.D2R - 16 * Cf.D2R 140 | angle_mea[2] = self.state[8] + 0.1 * (angle_mag - self.state[8]) 141 | self.state[8] = angle_mea[2] 142 | # self.state[8] = angle_pct[2] 143 | 144 | if sensor_data['gps'][0]: 145 | period_temp = ts - self.gpsTickPre 146 | self.gpsTickPre = ts 147 | 148 | # position 149 | pos_pct = self.state[0:3] # + self.state[3:6] * period_temp # the predict has been done with acc 150 | pos_mea = pos_pct + 0.2 * (data_gps - pos_pct) 151 | 152 | # velocity 153 | vel_gps = (data_gps - self.state[0:3]) / period_temp 154 | vel_mea12 = self.state[3:5] + (pos_mea[0:2] - pos_pct[0:2]) * 0.1 + (vel_gps[0:2] - self.state[3:5]) * 0.00 155 | vel_mea3 = self.state[5] + (pos_mea[2] - pos_pct[2]) * 0.1 + (vel_gps[2] - self.state[5]) * 0.003 156 | 157 | self.state[0:3] = pos_mea[0:3] 158 | self.state[3:6] = np.hstack([vel_mea12, vel_mea3]) 159 | 160 | return self.state 161 | 162 | 163 | if __name__ == '__main__': 164 | " used for testing this module" 165 | D2R = Cf.D2R 166 | testFlag = 1 167 | if testFlag == 1: 168 | # from QuadrotorFly import QuadrotorFlyModel as Qfm 169 | q1 = Qfm.QuadModel(Qfm.QuadParas(), Qfm.QuadSimOpt(init_mode=Qfm.SimInitType.fixed, enable_sensor_sys=True, 170 | init_pos=np.array([5, -4, 0]), init_att=np.array([0, 0, 5]))) 171 | # init the estimator 172 | s1 = KalmanFilterSimple() 173 | # set the init state of estimator 174 | s1.reset(q1.state) 175 | # simulation period 176 | t = np.arange(0, 30, 0.01) 177 | ii_len = len(t) 178 | stateRealArr = np.zeros([ii_len, 12]) 179 | stateEstArr = np.zeros([ii_len, 12]) 180 | meaArr = np.zeros([ii_len, 3]) 181 | 182 | # set the bias 183 | s1.gyroBias = q1.imu0.gyroBias 184 | s1.accBias = q1.imu0.accBias 185 | s1.magRef = q1.mag0.para.refField 186 | print(s1.gyroBias, s1.accBias) 187 | 188 | for ii in range(ii_len): 189 | # wait for start 190 | if ii < 100: 191 | sensor_data1 = q1.observe() 192 | _, oil = q1.get_controller_pid(q1.state) 193 | action = np.ones(4) * oil 194 | q1.step(action) 195 | stateEstArr[ii] = s1.update(sensor_data1, q1.ts) 196 | stateRealArr[ii] = q1.state 197 | else: 198 | sensor_data1 = q1.observe() 199 | action, oil = q1.get_controller_pid(s1.state, np.array([0, 0, 4, 0])) 200 | q1.step(action) 201 | stateEstArr[ii] = s1.update(sensor_data1, q1.ts) 202 | stateRealArr[ii] = q1.state 203 | import matplotlib.pyplot as plt 204 | plt.figure(1) 205 | ylabelList = ['roll', 'pitch', 'yaw', 'rate_roll', 'rate_pit', 'rate_yaw'] 206 | for ii in range(6): 207 | plt.subplot(6, 1, ii + 1) 208 | plt.plot(t, stateRealArr[:, 6 + ii] / D2R, '-b', label='real') 209 | plt.plot(t, stateEstArr[:, 6 + ii] / D2R, '-g', label='est') 210 | plt.legend() 211 | plt.ylabel(ylabelList[ii]) 212 | # plt.show() 213 | 214 | ylabelList = ['p_x', 'p_y', 'p_z', 'vel_x', 'vel_y', 'vel_z'] 215 | plt.figure(2) 216 | for ii in range(6): 217 | plt.subplot(6, 1, ii + 1) 218 | plt.plot(t, stateRealArr[:, ii], '-b', label='real') 219 | plt.plot(t, stateEstArr[:, ii], '-g', label='est') 220 | plt.legend() 221 | plt.ylabel(ylabelList[ii]) 222 | plt.show() 223 | 224 | -------------------------------------------------------------------------------- /Test/Test_full.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """a more completed test example for QuadrotorFly 4 | 5 | By xiaobo 6 | Contact linxiaobo110@gmail.com 7 | Created on 五月 06 18:41 2019 8 | """ 9 | 10 | # Copyright (C) 11 | # 12 | # This file is part of quadrotorfly 13 | # 14 | # GWpy is free software: you can redistribute it and/or modify 15 | # it under the terms of the GNU General Public License as published by 16 | # the Free Software Foundation, either version 3 of the License, or 17 | # (at your option) any later version. 18 | # 19 | # GWpy is distributed in the hope that it will be useful, 20 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 21 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 22 | # GNU General Public License for more details. 23 | # 24 | # You should have received a copy of the GNU General Public License 25 | # along with GWpy. If not, see . 26 | 27 | 28 | import numpy as np 29 | import QuadrotorFlyModel as Qfm 30 | import QuadrotorFlyGui as Qfg 31 | import MemoryStore 32 | import matplotlib.pyplot as plt 33 | 34 | 35 | """ 36 | ******************************************************************************************************** 37 | **------------------------------------------------------------------------------------------------------- 38 | ** Compiler : python 3.6 39 | ** Module Name: Test_full 40 | ** Module Date: 2019/5/6 41 | ** Module Auth: xiaobo 42 | ** Version : V0.1 43 | ** Description: 'Replace the content between' 44 | **------------------------------------------------------------------------------------------------------- 45 | ** Reversion : 46 | ** Modified By: 47 | ** Date : 48 | ** Content : 49 | ** Notes : 50 | ********************************************************************************************************/ 51 | """ 52 | # 角度到弧度的转换 53 | D2R = Qfm.D2R 54 | # 仿真参数设置 55 | simPara = Qfm.QuadSimOpt( 56 | # 初值重置方式（随机或者固定）；姿态初值参数（随机就是上限，固定就是设定值）；位置初值参数（同上） 57 | init_mode=Qfm.SimInitType.rand, init_att=np.array([5., 5., 5.]), init_pos=np.array([1., 1., 1.]), 58 | # 仿真运行的最大位置，最大速度，最大姿态角（角度，不是弧度注意），最大角速度（角度每秒） 59 | max_position=8, max_velocity=8, max_attitude=180, max_angular=200, 60 | # 系统噪声，分别在位置环和速度环 61 | sysnoise_bound_pos=0, sysnoise_bound_att=0, 62 | #执行器模式，简单模式没有电机动力学， 63 | actuator_mode=Qfm.ActuatorMode.dynamic 64 | ) 65 | # 无人机参数设置，可以为不同的无人机设置不同的参数 66 | uavPara = Qfm.QuadParas( 67 | # 重力加速度；采样时间；机型plus或者x 68 | g=9.8, tim_sample=0.01, structure_type=Qfm.StructureType.quad_plus, 69 | # 无人机臂长（米）；质量（千克）；飞机绕三个轴的转动惯量（千克·平方米） 70 | uav_l=0.45, uav_m=1.5, uav_ixx=1.75e-2, uav_iyy=1.75e-2, uav_izz=3.18e-2, 71 | # 螺旋桨推力系数（牛每平方（弧度每秒）），螺旋桨扭力系数（牛·米每平方（弧度每秒）），旋翼转动惯量， 72 | rotor_ct=1.11e-5, rotor_cm=1.49e-7, rotor_i=9.9e-5, 73 | # 电机转速比例参数（度每秒），电机转速偏置参数（度每秒），电机相应时间（秒） 74 | rotor_cr=646, rotor_wb=166, rotor_t=1.36e-2 75 | ) 76 | # 使用参数新建无人机 77 | quad1 = Qfm.QuadModel(uavPara, simPara) 78 | 79 | # 初始化GUI，并添加无人机 80 | gui = Qfg.QuadrotorFlyGui([quad1]) 81 | 82 | # 初始化记录器，用于记录飞行数据 83 | record = MemoryStore.DataRecord() 84 | 85 | # 重置系统 86 | # 当需要跑多次重复实验时注意重置系统， 87 | quad1.reset_states() 88 | record.clear() 89 | 90 | # 仿真过程 91 | print("Simplest simulation begin!") 92 | for i in range(1000): 93 | # 模型更新 94 | # 设置目标 95 | ref = np.array([0., 0., 1., 0.]) 96 | # 获取无人机的状态，共12维的向量 97 | # 分别是，xyz位置，xyz速度，roll pitch yaw姿态角，roll pitch yaw 角速度 98 | stateTemp = quad1.observe() 99 | # 通过无人机状态计算控制量，这也是自行设计控制器应修改的地方 100 | # 这是一个内置的控制器，返回值是给各个电机的控制量即油门的大小 101 | action2, oil = quad1.get_controller_pid(stateTemp, ref) 102 | # 使用控制量更新无人机，控制量是4维向量，范围0-1 103 | quad1.step(action2) 104 | 105 | # 记录数据 106 | record.buffer_append((stateTemp, action2)) 107 | 108 | # 渲染GUI 109 | gui.render() 110 | 111 | record.episode_append() 112 | # 输出结果 113 | ## 获取记录中的状态 114 | data = record.get_episode_buffer() 115 | bs = data[0] 116 | ## 获取记录中的控制量 117 | ba = data[1] 118 | ## 生成时间序列 119 | t = range(0, record.count) 120 | ## 画图 121 | fig1 = plt.figure(2) 122 | plt.clf() 123 | ## 姿态图 124 | plt.subplot(3, 1, 1) 125 | plt.plot(t, bs[t, 6] / D2R, label='roll') 126 | plt.plot(t, bs[t, 7] / D2R, label='pitch') 127 | plt.plot(t, bs[t, 8] / D2R, label='yaw') 128 | plt.ylabel('Attitude $(\circ)$', fontsize=15) 129 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 130 | ## 位置图(xy) 131 | plt.subplot(3, 1, 2) 132 | plt.plot(t, bs[t, 0], label='x') 133 | plt.plot(t, bs[t, 1], label='y') 134 | plt.ylabel('Position (m)', fontsize=15) 135 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 136 | ## 高度图 137 | plt.subplot(3, 1, 3) 138 | plt.plot(t, bs[t, 2], label='z') 139 | plt.ylabel('Altitude (m)', fontsize=15) 140 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 141 | plt.show() 142 | print("Simulation finish!") 143 | -------------------------------------------------------------------------------- /Test/Test_fullWithCamdown.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """a more completed test example for QuadrotorFly with camdown module 4 | 5 | By xiaobo 6 | Contact linxiaobo110@gmail.com 7 | Created on 十二月 07 13:03 2019 8 | """ 9 | 10 | # Copyright (C) 11 | # 12 | # This file is part of QuadrotorFly 13 | # 14 | # GWpy is free software: you can redistribute it and/or modify 15 | # it under the terms of the GNU General Public License as published by 16 | # the Free Software Foundation, either version 3 of the License, or 17 | # (at your option) any later version. 18 | # 19 | # GWpy is distributed in the hope that it will be useful, 20 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 21 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 22 | # GNU General Public License for more details. 23 | # 24 | # You should have received a copy of the GNU General Public License 25 | # along with GWpy. If not, see . 26 | 27 | 28 | import numpy as np 29 | import QuadrotorFlyModel as Qfm 30 | import QuadrotorFlyGui as Qfg 31 | import MemoryStore 32 | import matplotlib.pyplot as plt 33 | import CamDown 34 | import os 35 | import cv2 36 | import time 37 | 38 | """ 39 | ******************************************************************************************************** 40 | **------------------------------------------------------------------------------------------------------- 41 | ** Compiler : python 3.6 42 | ** Module Name: Test_fullWithCamdown 43 | ** Module Date: 2019/12/7 44 | ** Module Auth: xiaobo 45 | ** Version : V0.1 46 | ** Description: 'Replace the content between' 47 | **------------------------------------------------------------------------------------------------------- 48 | ** Reversion : 49 | ** Modified By: 50 | ** Date : 51 | ** Content : 52 | ** Notes : 53 | ********************************************************************************************************/ 54 | """ 55 | 56 | # translate degree to rad 57 | D2R = Qfm.D2R 58 | # set the simulation para 59 | simPara = Qfm.QuadSimOpt( 60 | # initial mode（random or fixed, default is random）；init para for attitude（bound in random mode， 61 | # direct value in fixed mode）；init papa for position (like init_att) 62 | init_mode=Qfm.SimInitType.rand, init_att=np.array([5., 5., 5.]), init_pos=np.array([1., 1., 1.]), 63 | # max position(unit is m), max velocity(unit is m/s), max attitude(unit is degree), max angular(unit is deg/s) 64 | max_position=8, max_velocity=8, max_attitude=180, max_angular=200, 65 | # system noise (internal noisy, default is 0) 66 | sysnoise_bound_pos=0, sysnoise_bound_att=0, 67 | # mode for actuator(motor) (with dynamic or not), enable the sensor system (default is false) 68 | actuator_mode=Qfm.ActuatorMode.dynamic, enable_sensor_sys=False, 69 | ) 70 | # set the para for quadrotor 71 | uavPara = Qfm.QuadParas( 72 | # gravity accelerate；sample period；frame type (plus or x) 73 | g=9.8, tim_sample=0.01, structure_type=Qfm.StructureType.quad_plus, 74 | # length of arm(m)；mass(kg)；the moment of inertia around xyz（kg·m^2） 75 | uav_l=0.45, uav_m=1.5, uav_ixx=1.75e-2, uav_iyy=1.75e-2, uav_izz=3.18e-2, 76 | # para from motor speed to thrust (N/(rad/s)), papa from motor speed to torque (N ·m/(rad/s)), MoI of motor 77 | rotor_ct=1.11e-5, rotor_cm=1.49e-7, rotor_i=9.9e-5, 78 | # proportional parameter from u to motor speed (deg/s), bias parameters from u to motor (deg/s), response time 79 | rotor_cr=646, rotor_wb=166, rotor_t=1.36e-2 80 | ) 81 | 82 | # all above is the default parameters expect the enable_sensor_sys 83 | # crete the UAV with set parameters 84 | quad1 = Qfm.QuadModel(uav_para=uavPara, sim_para=simPara) 85 | 86 | # create the UAV camdown object 87 | cam1 = CamDown.CamDown( 88 | # the resolution and depth (RGB is 3, gray is 1) of the the sensor 89 | img_horizon=400, img_vertical=400, img_depth=3, 90 | # the physical size of the active area of the sensor (unit is mm), the focal of the lens (unit is mm) 91 | sensor_horizon=4., sensor_vertical=4., cam_focal=2.36, 92 | # the path of the ground image, used in MEM render mode, doesn't need in other mode 93 | ground_img_path='../Data/groundImgWood.jpg', 94 | # the path of the mapping image, used in other render mode except MEM 95 | small_ground_img_path='../Data/groundImgSmall.jpg', 96 | # the path of the mapping image(in the center), used in other render mode except MEM 97 | small_land_img_path='../Data/landingMark.jpg', 98 | # render mode, could be Render_Mode_Gpu, Render_Mode_Cpu, Render_Mode_Mem, GPU mode is recommended 99 | render_mode=CamDown.CamDownPara.Render_Mode_Gpu) 100 | # load ground image 101 | cam1.load_ground_img() 102 | 103 | # create path for video store 104 | if not os.path.exists('../Data/img'): 105 | os.mkdir('../Data/img') 106 | v_format = cv2.VideoWriter_fourcc(*'MJPG') 107 | out1 = cv2.VideoWriter('../Data/img/test.avi', v_format, 1 / quad1.uavPara.ts, (cam1.imgVertical, cam1.imgHorizon)) 108 | 109 | # create the gui with created UAV 110 | gui = Qfg.QuadrotorFlyGui([quad1]) 111 | 112 | # crate the record, used to record the data between UAV fly 113 | record = MemoryStore.DataRecord() 114 | 115 | # reset the system 116 | quad1.reset_states() 117 | record.clear() 118 | 119 | t = np.arange(0, 15, 0.01) 120 | ii_len = len(t) 121 | 122 | print('Camera module test begin:') 123 | time_start = time.time() 124 | # simulation process 125 | for ii in range(ii_len): 126 | # get the state of the quadrotor 127 | stateTemp = quad1.observe() 128 | # calculate image according the state 129 | pos_0 = quad1.position * 1000 130 | att_0 = quad1.attitude 131 | img1 = cam1.get_img_by_state(pos_0, att_0) 132 | 133 | # calculate the control value 134 | action, oil = quad1.get_controller_pid(stateTemp, np.array([0, 0, 3, 0])) 135 | # execute the dynamic of quadrotor 136 | quad1.step(action) 137 | print('action:', action) 138 | 139 | # store the real state and the estimated state 140 | record.buffer_append((stateTemp, action)) 141 | # write the video steam 142 | out1.write(img1) 143 | gui.render() 144 | out1.release() 145 | # gui.render() 146 | time_end = time.time() 147 | print('time cost:', str(time_end - time_start)) 148 | record.episode_append() 149 | 150 | # output the result 151 | # 1. get data form record 152 | data = record.get_episode_buffer() 153 | # 1.1 get the state 154 | bs = data[0] 155 | # 1.2 get the action 156 | ba = data[1] 157 | 158 | # 2. plot result 159 | t = range(0, record.count) 160 | fig1 = plt.figure(2) 161 | plt.clf() 162 | # 2.1 figure attitude 163 | plt.subplot(3, 1, 1) 164 | plt.plot(t, bs[t, 6] / D2R, label='roll') 165 | plt.plot(t, bs[t, 7] / D2R, label='pitch') 166 | plt.plot(t, bs[t, 8] / D2R, label='yaw') 167 | plt.ylabel('Attitude $(\circ)$', fontsize=15) 168 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 169 | # 2.2 figure position 170 | plt.subplot(3, 1, 2) 171 | plt.plot(t, bs[t, 0], label='x') 172 | plt.plot(t, bs[t, 1], label='y') 173 | plt.ylabel('Position (m)', fontsize=15) 174 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 175 | # 2.3 figure position 176 | plt.subplot(3, 1, 3) 177 | plt.plot(t, bs[t, 2], label='z') 178 | plt.ylabel('Altitude (m)', fontsize=15) 179 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 180 | plt.show() 181 | print("Simulation finish!") 182 | print('Saved video is under the ../Data/img/test.mp4') 183 | -------------------------------------------------------------------------------- /Test/Test_fullWithSensor.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """a more completed test example with sensor system for QuadrotorFly 4 | 5 | By xiaobo 6 | Contact linxiaobo110@gmail.com 7 | Created on 六月 29 21:26 2019 8 | """ 9 | 10 | # Copyright (C) 11 | # 12 | # This file is part of QuadrotorFly 13 | # 14 | # GWpy is free software: you can redistribute it and/or modify 15 | # it under the terms of the GNU General Public License as published by 16 | # the Free Software Foundation, either version 3 of the License, or 17 | # (at your option) any later version. 18 | # 19 | # GWpy is distributed in the hope that it will be useful, 20 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 21 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 22 | # GNU General Public License for more details. 23 | # 24 | # You should have received a copy of the GNU General Public License 25 | # along with GWpy. If not, see . 26 | 27 | 28 | import numpy as np 29 | import QuadrotorFlyModel as Qfm 30 | import QuadrotorFlyGui as Qfg 31 | import MemoryStore 32 | import matplotlib.pyplot as plt 33 | import StateEstimator 34 | 35 | """ 36 | ******************************************************************************************************** 37 | **------------------------------------------------------------------------------------------------------- 38 | ** Compiler : python 3.6 39 | ** Module Name: Test_fullWithSensor 40 | ** Module Date: 2019/6/29 41 | ** Module Auth: xiaobo 42 | ** Version : V0.1 43 | ** Description: 'Replace the content between' 44 | **------------------------------------------------------------------------------------------------------- 45 | ** Reversion : 46 | ** Modified By: 47 | ** Date : 48 | ** Content : 49 | ** Notes : 50 | ********************************************************************************************************/ 51 | """ 52 | 53 | # translate degree to rad 54 | D2R = Qfm.D2R 55 | # set the simulation para 56 | simPara = Qfm.QuadSimOpt( 57 | # initial mode（random or fixed, default is random）；init para for attitude（bound in random mode， 58 | # direct value in fixed mode）；init papa for position (like init_att) 59 | init_mode=Qfm.SimInitType.rand, init_att=np.array([5., 5., 5.]), init_pos=np.array([1., 1., 1.]), 60 | # max position(unit is m), max velocity(unit is m/s), max attitude(unit is degree), max angular(unit is deg/s) 61 | max_position=8, max_velocity=8, max_attitude=180, max_angular=200, 62 | # system noise (internal noisy, default is 0) 63 | sysnoise_bound_pos=0, sysnoise_bound_att=0, 64 | # mode for actuator(motor) (with dynamic or not), enable the sensor system (default is false) 65 | actuator_mode=Qfm.ActuatorMode.dynamic, enable_sensor_sys=True, 66 | ) 67 | # set the para for quadrotor 68 | uavPara = Qfm.QuadParas( 69 | # gravity accelerate；sample period；frame type (plus or x) 70 | g=9.8, tim_sample=0.01, structure_type=Qfm.StructureType.quad_plus, 71 | # length of arm(m)；mass(kg)；the moment of inertia around xyz（kg·m^2） 72 | uav_l=0.45, uav_m=1.5, uav_ixx=1.75e-2, uav_iyy=1.75e-2, uav_izz=3.18e-2, 73 | # para from motor speed to thrust (N/(rad/s)), papa from motor speed to torque (N ·m/(rad/s)), MoI of motor 74 | rotor_ct=1.11e-5, rotor_cm=1.49e-7, rotor_i=9.9e-5, 75 | # proportional parameter from u to motor speed (deg/s), bias parameters from u to motor (deg/s), response time 76 | rotor_cr=646, rotor_wb=166, rotor_t=1.36e-2 77 | ) 78 | 79 | # all above is the default parameters expect the enable_sensor_sys 80 | # crete the UAV with set parameters 81 | # quad1 = Qfm.QuadModel(uavPara, simPara) 82 | quad1 = Qfm.QuadModel(Qfm.QuadParas(), Qfm.QuadSimOpt(init_mode=Qfm.SimInitType.fixed, enable_sensor_sys=True, 83 | init_pos=np.array([5, -3, 0]), init_att=np.array([0, 0, 5]))) 84 | # create the estimator 85 | filter1 = StateEstimator.KalmanFilterSimple() 86 | 87 | # create the gui with created UAV 88 | gui = Qfg.QuadrotorFlyGui([quad1]) 89 | 90 | # crate the record, used to record the data between UAV fly 91 | record = MemoryStore.DataRecord() 92 | 93 | # reset the system 94 | quad1.reset_states() 95 | record.clear() 96 | filter1.reset(quad1.state) 97 | 98 | # set the real bias for filter directly, it is not real in fact, just a simplify 99 | filter1.gyroBias = quad1.imu0.gyroBias 100 | filter1.accBias = quad1.imu0.accBias 101 | print(filter1.gyroBias, filter1.accBias) 102 | 103 | t = np.arange(0, 30, 0.01) 104 | ii_len = len(t) 105 | 106 | # simulation process 107 | for ii in range(ii_len): 108 | # wait for sensor start 109 | if ii < 100: 110 | # get the sensor data 111 | sensor_data1 = quad1.observe() 112 | # the control value is calculated with real state before the sensor start completely 113 | _, oil = quad1.get_controller_pid(quad1.state) 114 | # just use the oil signal as control value 115 | action = np.ones(4) * oil 116 | # execute the dynamic of quadrotor 117 | quad1.step(action) 118 | # feed the filter with sensor data which will will return the estimated state 119 | state_est = filter1.update(sensor_data1, quad1.ts) 120 | # store the real state and the estimated state 121 | record.buffer_append((quad1.state, state_est)) 122 | # stateEstArr[ii] = filter1.update(sensor_data1, quad1.ts) 123 | # stateRealArr[ii] = quad1.state 124 | else: 125 | # get the sensor data 126 | sensor_data1 = quad1.observe() 127 | # calculate the system state based on the state estimated by kalman filter, not the real state 128 | action, oil = quad1.get_controller_pid(filter1.state, np.array([0, 0, 2, 0])) 129 | # execute the dynamic of quadrotor 130 | quad1.step(action) 131 | # feed the filter with sensor data which will will return the estimated state 132 | state_est = filter1.update(sensor_data1, quad1.ts) 133 | # print(state_est) 134 | # store the real state and the estimated state 135 | record.buffer_append((quad1.state, state_est.copy())) 136 | # stateEstArr[ii] = filter1.update(sensor_data1, quad1.ts) 137 | # stateRealArr[ii] = q1.state 138 | # gui.render() 139 | 140 | record.episode_append() 141 | # output the result 142 | # 1. get data form record 143 | data = record.get_episode_buffer() 144 | # 1.1 the real state 145 | bs_r = data[0] 146 | # 1.2 the estimated state 147 | bs_e = data[1] 148 | # 2. generate the time sequence 149 | t = range(0, record.count) 150 | # 3. draw figure 151 | # 3.1 draw the attitude and angular 152 | fig1 = plt.figure(2) 153 | yLabelList = ['roll', 'pitch', 'yaw', 'rate_roll', 'rate_pit', 'rate_yaw'] 154 | for ii in range(6): 155 | plt.subplot(6, 1, ii + 1) 156 | plt.plot(t, bs_r[:, 6 + ii] / D2R, '-b', label='real') 157 | plt.plot(t, bs_e[:, 6 + ii] / D2R, '-g', label='est') 158 | plt.legend() 159 | plt.ylabel(yLabelList[ii]) 160 | 161 | # 3.2 draw the position and velocity 162 | yLabelList = ['p_x', 'p_y', 'p_z', 'vel_x', 'vel_y', 'vel_z'] 163 | plt.figure(3) 164 | for ii in range(6): 165 | plt.subplot(6, 1, ii + 1) 166 | plt.plot(t, bs_r[:, ii], '-b', label='real') 167 | plt.plot(t, bs_e[:, ii], '-g', label='est') 168 | plt.legend() 169 | plt.ylabel(yLabelList[ii]) 170 | plt.show() 171 | print("Simulation finish!") 172 | -------------------------------------------------------------------------------- /Test/Test_simple.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """The simplest test example 4 | 5 | By xiaobo 6 | Contact linxiaobo110@gmail.com 7 | Created on 五月 06 17:34 2019 8 | """ 9 | 10 | # Copyright (C) 11 | # 12 | # This file is part of quadrotorfly 13 | # 14 | # GWpy is free software: you can redistribute it and/or modify 15 | # it under the terms of the GNU General Public License as published by 16 | # the Free Software Foundation, either version 3 of the License, or 17 | # (at your option) any later version. 18 | # 19 | # GWpy is distributed in the hope that it will be useful, 20 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 21 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 22 | # GNU General Public License for more details. 23 | # 24 | # You should have received a copy of the GNU General Public License 25 | # along with GWpy. If not, see . 26 | 27 | 28 | import numpy as np 29 | import QuadrotorFlyModel as Qfm 30 | 31 | """ 32 | ******************************************************************************************************** 33 | **------------------------------------------------------------------------------------------------------- 34 | ** Compiler : python 3.6 35 | ** Module Name: Test_simple 36 | ** Module Date: 2019/5/6 37 | ** Module Auth: xiaobo 38 | ** Version : V0.1 39 | ** Description: 'Replace the content between' 40 | **------------------------------------------------------------------------------------------------------- 41 | ** Reversion : 42 | ** Modified By: 43 | ** Date : 44 | ** Content : 45 | ** Notes : 46 | ********************************************************************************************************/ 47 | """ 48 | 49 | 50 | uavPara = Qfm.QuadParas() 51 | # define the simulation parameters 52 | simPara = Qfm.QuadSimOpt() 53 | # define the first uav 54 | quad1 = Qfm.QuadModel(uavPara, simPara) 55 | 56 | # simulation begin 57 | print("Simplest simulation begin!") 58 | for i in range(1000): 59 | # set target，i.e., position in x,y,z, and yaw 60 | ref = np.array([0., 0., 1., 0.]) 61 | # acquire the state of quadrotor， 62 | # they are 12 degrees' vector，position in xyz，velocity in xyz, 63 | # roll pitch yaw, and angular in roll pitch yaw 64 | stateTemp = quad1.observe() 65 | # calculate the control value; this is a pid controller example 66 | action2, oil = quad1.get_controller_pid(stateTemp, ref) 67 | # update the uav 68 | quad1.step(action2) 69 | 70 | print("Simulation finish!") 71 | -------------------------------------------------------------------------------- /UuvModel.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- encoding: utf-8 -*- 3 | ''' 4 | @File : UuvModel.py 5 | @Time : Mon Aug 17 2020 10:15:02 6 | @Author : xiaobo 7 | @Contact : linxiaobo110@gmail.com 8 | ''' 9 | 10 | # Copyright (C) 11 | # 12 | # This file is part of QuadrotorFly 13 | # 14 | # GWpy is free software: you can redistribute it and/or modify 15 | # it under the terms of the GNU General Public License as published by 16 | # the Free Software Foundation, either version 3 of the License, or 17 | # (at your option) any later version. 18 | # 19 | # GWpy is distributed in the hope that it will be useful, 20 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 21 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 22 | # GNU General Public License for more details. 23 | # 24 | # You should have received a copy of the GNU General Public License 25 | # along with GWpy. If not, see . 26 | 27 | # here put the import lib 28 | import numpy as np 29 | import MemoryStore 30 | ''' 31 | ******************************************************************************************************** 32 | **------------------------------------------------------------------------------------------------------- 33 | ** Compiler : python 3.6 34 | ** Module Name: UuvModel.py 35 | ** Module Date: 2020-08-17 36 | ** Module Auth: xiaobo 37 | ** Version : V0.1 38 | ** Description: create the module 39 | **------------------------------------------------------------------------------------------------------- 40 | ** Reversion : 41 | ** Modified By: 42 | ** Date : 43 | ** Content : 44 | ** Notes : 45 | ********************************************************************************************************/ 46 | ''' 47 | 48 | # definition of key constant 49 | D2R = np.pi / 180 50 | state_dim = 12 51 | action_dim = 4 52 | state_bound = np.array([10, 10, 10, 5, 5, 5, 80 * D2R, 80 * D2R, 180 * D2R, 100 * D2R, 100 * D2R, 100 * D2R]) 53 | action_bound = np.array([1, 1, 1, 1]) 54 | 55 | 56 | def rk4(func, x0, action, h): 57 | """Runge Kutta 4 order update function 58 | :param func: system dynamic 59 | :param x0: system state 60 | :param action: control input 61 | :param h: time of sample 62 | :return: state of next time 63 | """ 64 | k1 = func(x0, action) 65 | k2 = func(x0 + h * k1 / 2, action) 66 | k3 = func(x0 + h * k2 / 2, action) 67 | k4 = func(x0 + h * k3, action) 68 | # print(test) 69 | # print('rk4 debug: ', trans(k1), trans(k2), trans(k3), trans(k4)) 70 | x1 = x0 + h * (k1 + 2 * k2 + 2 * k3 + k4) / 6 71 | return x1 72 | 73 | def trans(xx): 74 | s2 = np.zeros(12) 75 | s2[0:3] = xx[3:6] 76 | s2[3:6] = xx[9:12] 77 | s2[6:9] = xx[6:9] 78 | s2[9:12] = xx[0:3] 79 | return s2 80 | 81 | 82 | class UuvParas(object): 83 | """Define the parameters of quadrotor model 84 | 85 | """ 86 | 87 | def __init__(self, g=9.8, tim_sample=0.01, 88 | uuv_b=14671, uuv_m=1840, uuv_dis_c=np.array([0.02, -0.005, 0.008]), uuv_j=np.array([289.3, 6771.8, 6771.8]), uuv_lu=1019.2, uuv_s=0.224, uuv_l=7.738, 89 | uuv_cx=0.141, uuv_mx_beta=0.00152, uuv_mx_dr=-0.000319, uuv_mx_dd=-0.0812, uuv_mx_wx=-0.0044, uuv_mx_wy=0.0008, uuv_mx_xp=0, 90 | uuv_cy_alfa=2.32, uuv_cy_de=0.51, uuv_cy_wz=1.17, uuv_my_beta=0.69, uuv_my_dr=-0.11, uuv_my_wx=0, uuv_my_wy=-0.61, 91 | uuv_cz_beta=-2.32, uuv_cz_dr=-0.2, uuv_cz_wy=-1.17, uuv_mz_alpha=0.69, uuv_mz_de=-0.28, uuv_mz_wz=-0.61, 92 | uuv_add_m_k11=0.0222, uuv_add_m_k22=1.1096, uuv_add_m_K44=0.1406, uuv_add_m_k55=3.8129, uuv_add_m_k26=-0.363): 93 | """init the quadrotor parameters 94 | These parameters are able to be estimation in web(https://flyeval.com/) if you do not have a real UAV. 95 | common parameters: 96 | -g : N/kg, acceleration gravity 97 | -tim_sample : s, sample time of system 98 | uuv: 99 | -uuv_b : N, floating force of uuv 100 | -uuv_m : kg, the mass of quadrotor 101 | -uuv_dis_c : m, distance from center of mass to center of floting 102 | -uuv_j : kg.m^2 the central principal moments of inertia of UAV in x,y,z 103 | -uuv_lu : kg/(m^3) the density of water 104 | -uuv_s : m^2 the max area of the uuv in direction x 105 | -uuv_l : m the length of the uuv 106 | -uuv_cx : the factor between drag-force and uuv_s 107 | -uuv_mx_beta: the factor between roll-force and beta 108 | -uuv_mx_dr: the factor between roll-force and delta_r 109 | -uuv_mx_dd: the factor between roll-force and delta_d 110 | -uuv_mx_wx: the factor between roll-force and omega_x 111 | -uuv_mx_wy: the factor between roll-force and omega_y 112 | -uuv_mx_xp: 113 | -uuv_cy_alfa: the factor between up-force and alpha 114 | -uuv_cy_de: the factor between up-force and delta_e 115 | -uuv_cy_wz: the factor between up-force and omega_z 116 | -uuv_my_beta: the factor between yaw-force and beta 117 | -uuv_my_dr: the factor between yaw-force and delta_r 118 | -uuv_my_wx: 119 | -uuv_my_wy: the factor between yaw-force and omega_y 120 | -uuv_cz_beta: the factor between right-force and beta 121 | -uuv_cz_dr: the factor between right-force and delta_r 122 | -uuv_cz_wy: the factor between right-force and omega_y 123 | -uuv_mz_alpha: the factor between pitch-force and alpha 124 | -uuv_mz_de: the factor between pitch-force and delta_e 125 | -uuv_mz_wz: the factor between pitch-force and omega_z 126 | -uuv_add_m_k11: the factor between added mass in x 127 | -uuv_add_m_k22: the factor between added mass in z 128 | -uuv_add_m_K44: the factor between added mass in roll 129 | -uuv_add_m_k55: the factor between added mass in pitch 130 | -uuv_add_m_k26: the factor between added mass in 131 | """ 132 | self.ts = tim_sample 133 | self.g = g 134 | 135 | self.B = uuv_b 136 | self.G = uuv_m * g # Gravity of uuv 137 | self.M = uuv_m 138 | self.Dis_c = uuv_dis_c 139 | self.J = uuv_j 140 | self.Lu = uuv_lu 141 | self.S = uuv_s 142 | self.L = uuv_l 143 | self.Cx = uuv_cx 144 | self.MxBeta = uuv_mx_beta 145 | self.MxDr = uuv_mx_dr 146 | self.MxDd = uuv_mx_dd 147 | self.MxWx = uuv_mx_wx 148 | self.MxWy = uuv_mx_wy 149 | self.MxP = 0 150 | self.CyAlpha = uuv_cy_alfa 151 | self.CyDe = uuv_cy_de 152 | self.CyWz = uuv_cy_wz 153 | self.MyBeta = uuv_my_beta 154 | self.MyDr = uuv_my_dr 155 | self.MyWy = uuv_my_wy 156 | self.CzBeta = uuv_cz_beta 157 | self.CzDr = uuv_cz_dr 158 | self.CzWy = uuv_cz_wy 159 | self.MzAlpha = uuv_mz_alpha 160 | self.MzDe = uuv_mz_de 161 | self.MzWz = uuv_mz_wz 162 | self.AddMk11 = uuv_add_m_k11 163 | self.AddMk22 = uuv_add_m_k22 164 | self.AddMk33 = uuv_add_m_k22 165 | self.AddMk44 = uuv_add_m_K44 166 | self.AddMk55 = uuv_add_m_k55 167 | self.AddMk66 = uuv_add_m_k55 168 | self.AddMk26 = uuv_add_m_k26 169 | self.AddMk35 = -uuv_add_m_k26 170 | 171 | V = self.B / (self.Lu * self.g) 172 | self.AddML11 = self.AddMk11 * self.Lu * V 173 | self.AddML22 = self.AddMk22 * self.Lu * V 174 | self.AddML33 = self.AddMk33 * self.Lu * V 175 | self.AddML44 = self.AddMk44 * self.Lu * np.power(V, 5./3) 176 | self.AddML55 = self.AddMk55 * self.Lu * np.power(V, 5./3) 177 | self.AddML66 = self.AddMk66 * self.Lu * np.power(V, 5./3) 178 | self.AddML26 = self.AddMk26 * self.Lu * np.power(V, 4./3) 179 | self.AddML35 = self.AddMk35 * self.Lu * np.power(V, 4./3) 180 | 181 | class UuvModel(object): 182 | def __init__(self, uuv_para: UuvParas): 183 | """init a uuv""" 184 | 185 | self.agtPara = uuv_para # the para of agent 186 | # states of uuv 187 | # -position, m 188 | self.position = np.array([0, 0, 0]) 189 | # -velocity, m/s 190 | self.velocity = np.array([0, 0, 0]) 191 | # -attitude, rad 192 | self.attitude = np.array([0, 0, 0]) 193 | # -angular, rad/s 194 | self.angular = np.array([0, 0, 0]) 195 | # accelerate, m/(s^2) 196 | self.acc = np.zeros(3) 197 | # time control, s 198 | self.__ts = 0 199 | 200 | def dynamic_basic(self, state, action, debug=False): 201 | """ calculate /dot(state) = f(state) + u(state) 202 | This function will be executed many times during simulation, so high performance is necessary. 203 | :param state: 204 | 0 1 2 3 4 5 205 | p_x p_y p_z v_x v_y v_z 206 | 6 7 8 9 10 11 207 | roll pitch yaw v_roll v_pitch v_yaw 208 | :param action: u1(sum of thrust), u2(torque for roll), u3(pitch), u4(yaw) 209 | :return: derivatives of state inclfrom bokeh.plotting import figure 210 | """ 211 | dot_state = np.zeros([12]) 212 | if debug: 213 | test = dict() 214 | p = self.agtPara 215 | a = np.array([ 216 | [p.M + p.AddML11, 0, 0, 0, p.M * p.Dis_c[2], -p.M * p.Dis_c[1]], 217 | [0, p.M + p.AddML22, 0, -p.M * p.Dis_c[2], 0, p.M * p.Dis_c[0] + p.AddML26], 218 | [0, 0, p.M + p.AddML33, p.M * p.Dis_c[1], p.AddML35 - p.M * p.Dis_c[0], 0], 219 | [0, -p.M * p.Dis_c[2], p.M * p.Dis_c[1], p.J[0] + p.AddML44, 0, 0], 220 | [p.M * p.Dis_c[2], 0, p.AddML35 - p.M * p.Dis_c[0], 0, p.J[1] + p.AddML55, 0], 221 | [-p.M * p.Dis_c[1], p.M * p.Dis_c[0] + p.AddML26, 0, 0, 0, p.J[2] + p.AddML66] 222 | ]) 223 | de = action[0] 224 | dr = action[1] 225 | dd = action[2] 226 | tl = action[3] 227 | # the sequence here is different from the book "yu lei hang xing xue" 228 | vx = state[3] 229 | vy = state[4] 230 | vz = state[5] 231 | wx = state[9] 232 | wy = state[10] 233 | wz = state[11] 234 | pesi = state[8] 235 | sita = state[7] 236 | fai = state[6] 237 | v2 = vx * vx + vy * vy + vz * vz 238 | # speed 239 | v = np.sqrt(v2) 240 | # attack angle 241 | alfa = np.arctan(-vy / vx) 242 | vxy = np.sqrt(vx * vx + vy * vy) 243 | # side angle 244 | beta = np.arctan(vz / vxy) 245 | 246 | salfa = np.sin(alfa) 247 | calfa = np.cos(alfa) 248 | sbeta = np.sin(beta) 249 | cbeta = np.cos(beta) 250 | spesi = np.sin(pesi) 251 | cpesi = np.cos(pesi) 252 | ssita = np.sin(sita) 253 | csita = np.cos(sita) 254 | sfai = np.sin(fai) 255 | cfai = np.cos(fai) 256 | 257 | cvb = np.array([ 258 | [calfa * cbeta, salfa, -calfa * sbeta], 259 | [-salfa * cbeta, calfa, salfa * sbeta], 260 | [sbeta, 0, cbeta] 261 | ]) 262 | 263 | ceb = np.array([ 264 | [csita * cpesi, ssita, -csita * spesi], 265 | [-ssita * cpesi * cfai + spesi * sfai, csita * cfai, ssita * spesi * cfai + cpesi * sfai], 266 | [ssita * cpesi * sfai + spesi * cfai, -csita * sfai, -ssita * spesi * sfai + cpesi * cfai] 267 | ]) 268 | if debug: 269 | test['cvb'] = cvb 270 | test['ceb'] = ceb 271 | # print('dynamic test 1: -----------------------') 272 | # print('cvb is ', cvb) 273 | # print('ceb is ', ceb) 274 | fs_v = np.array([-p.Cx * 0.5 * p.Lu * v2 * p.S, 275 | p.CyAlpha * 0.5 * p.Lu * v2 * p.S * alfa + p.CyWz * 0.5 * p.Lu * p.S * p.L * v * wz + p.CyDe * 0.5 * p.Lu * v2 * p.S * de, 276 | p.CzBeta * 0.5 * p.Lu * v2 * p.S * beta + p.CzWy * 0.5 * p.Lu * p.S * p.L * v * wy + p.CzDr * 0.5 * p.Lu * v2 * p.S * dr 277 | ]) 278 | # force 279 | fs = cvb.dot(fs_v) 280 | 281 | # torque 282 | ms = np.array([ 283 | p.MxBeta * 0.5 * p.Lu * p.S * p.L * v2 * beta + p.MxWx * 0.5 * p.Lu * p.S * (np.square(p.L)) * wx * v 284 | + p.MxDr * 0.5 * p.Lu * p.S * p.L * v2 * dr + p.MxDd * 0.5 * p.Lu * p.S * p.L * v2 * dd + p.MxP * v2, 285 | p.MyBeta * 0.5 * p.Lu * p.S * p.L * v2 * beta + p.MyWy * 0.5 * p.Lu * p.S * (np.square(p.L)) * wy * v + p.MyDr * 0.5 * p.Lu * p.S * p.L * v2 * dr, 286 | p.MzAlpha * 0.5 * p.Lu * p.S * p.L * v2 * alfa + p.MzWz * 0.5 * p.Lu * p.S * (np.square(p.L)) * wz * v + p.MzDe * 0.5 * p.Lu * p.S * p.L * v2 * de 287 | ]) 288 | # floating force 289 | fg = ceb.dot(np.array([0, p.B - p.G, 0])) 290 | if debug: 291 | test['fs'] = fs 292 | test['ms'] = ms 293 | test['fg'] = fg 294 | 295 | mg_m = np.array([ 296 | [0, -p.Dis_c[2], p.Dis_c[1]], 297 | [p.Dis_c[2], 0, -p.Dis_c[0]], 298 | [-p.Dis_c[1], p.Dis_c[0], 0] 299 | ]) 300 | # gravity 301 | mg = mg_m.dot(ceb.dot(np.array([0, -p.G, 0]))) 302 | ft = np.array([ 303 | [-p.M * (vz * wy - vy * wz + p.Dis_c[1] * wx * wy + p.Dis_c[2] * wx * wz - p.Dis_c[0] * (np.square(wy) + np.square(wz)))], 304 | [-p.M * (vx * wz - vz * wx + p.Dis_c[2] * wy * wz + p.Dis_c[0] * wy * wx - p.Dis_c[1] * (np.square(wz) + np.square(wx)))], 305 | [-p.M * (vy * wx - vx * wy + p.Dis_c[0] * wz * wx + p.Dis_c[1] * wz * wy - p.Dis_c[2] * (np.square(wx) + np.square(wy)))] 306 | ]) 307 | mt = np.array([ 308 | [-p.M * (p.Dis_c[1] * (vy * wx - vx * wy) + p.Dis_c[2] * (vz * wx - vx * wz)) - (p.J[2] - p.J[1]) * wy * wz], 309 | [-p.M * (p.Dis_c[2] * (vz * wy - vy * wz) + p.Dis_c[0] * (vx * wy - vy * wx)) - (p.J[0] - p.J[2]) * wz * wx], 310 | [-p.M * (p.Dis_c[0] * (vx * wz - vz * wx) + p.Dis_c[1] * (vy * wz - vz * wy)) - (p.J[1] - p.J[0]) * wx * wy] 311 | ]) 312 | if debug: 313 | test['mg'] = mg 314 | test['ft'] = ft 315 | test['mt'] = mt 316 | # print('dynamic test 3: -----------------------') 317 | # print('mg is ', mg) 318 | # print('ft is ', ft) 319 | # print('mt is ', mt) 320 | 321 | ftl = np.array([tl, 0, 0]) 322 | f = np.hstack([fs.reshape([3]) + fg.reshape([3]) + ft.reshape([3]) + ftl.reshape([3]), ms.reshape([3]) + mg.reshape([3]) + mt.reshape([3])]) 323 | df_speed = np.linalg.inv(a).dot(f) 324 | angle_rot = np.array([ 325 | [0, cfai / csita, -sfai / csita], 326 | [0, sfai, cfai], 327 | [1, -cfai * np.tan(sita), sfai * np.tan(sita)] 328 | ]) 329 | df_2 = angle_rot.dot(np.array([wx, wy, wz])) 330 | df_3 = ceb.dot([vx, vy, vz]) 331 | if debug: 332 | test['f'] = f 333 | test['a'] = a 334 | test['df_speed'] = df_speed 335 | test['df_angle'] = df_2 336 | 337 | dot_state[0:3] = df_3 338 | dot_state[3:6] = df_speed[0:3] 339 | dot_state[6:9] = df_2 340 | dot_state[9:12] = df_speed[3:6] 341 | if debug: 342 | return dot_state, test 343 | else: 344 | return dot_state 345 | 346 | def observe(self): 347 | """out put the system state, with sensor system or without sensor system""" 348 | # if self.simPara.enableSensorSys: 349 | # sensor_data = dict() 350 | # for index, sensor in enumerate(self.sensorList): 351 | # if isinstance(sensor, SensorBase.SensorBase): 352 | # # name = str(index) + '-' + sensor.get_name() 353 | # name = sensor.get_name() 354 | # sensor_data.update({name: sensor.observe()}) 355 | # return sensor_data 356 | # else: 357 | return np.hstack([self.position, self.velocity, self.attitude, self.angular]) 358 | 359 | def get_reward(self): 360 | reward = np.sum(np.square(self.position)) / 8 + np.sum(np.square(self.velocity)) / 20 \ 361 | + np.sum(np.square(self.attitude)) / 3 + np.sum(np.square(self.angular)) / 10 362 | return reward 363 | 364 | def step(self, action): 365 | 366 | self.__ts += self.agtPara.ts 367 | # # 1.1 Actuator model, calculate the thrust and torque 368 | # thrusts, toques = self.actuator.step(action) 369 | 370 | # # 1.2 Calculate the force distribute according to 'x' type or '+' type, assum '+' type 371 | # forces = self.rotor_distribute_dynamic(thrusts, toques) 372 | 373 | # 1.3 Basic model, calculate the basic model, the u need to be given directly in test-mode for Matlab 374 | state_temp = np.hstack([self.position, self.velocity, self.attitude, self.angular]) 375 | state_next = rk4(self.dynamic_basic, state_temp, action, self.agtPara.ts) 376 | [self.position, self.velocity, self.attitude, self.angular] = np.split(state_next, 4) 377 | # calculate the accelerate 378 | state_dot = self.dynamic_basic(state_temp, action) 379 | self.acc = state_dot[3:6] 380 | 381 | # 2. Calculate Sensor sensor model 382 | # if self.simPara.enableSensorSys: 383 | # for index, sensor in enumerate(self.sensorList): 384 | # if isinstance(sensor, SensorBase.SensorBase): 385 | # sensor.update(np.hstack([state_next, self.acc]), self.__ts) 386 | ob = self.observe() 387 | 388 | # 3. Check whether finish (failed or completed) 389 | finish_flag = False#self.is_finished() 390 | 391 | # 4. Calculate a reference reward 392 | reward = self.get_reward() 393 | 394 | return ob, reward, finish_flag 395 | 396 | if __name__ == '__main__': 397 | " used for testing this module" 398 | testFlag = 3 399 | 400 | if testFlag == 1: 401 | # test case 1 402 | # 使用简单的原始数据测试 403 | para = UuvParas() 404 | agent = UuvModel(para) 405 | xi = np.array([0, 0, 0, 10, 0, 0, 406 | 0, 0, 0, 0, 0, 0]) 407 | a0 = np.array([ 0, 0, 0, 10672]) 408 | dot_state, test = agent.dynamic_basic(xi, a0 ,debug=True) 409 | ## sec 1 ############################################################## 410 | r_cvb = np.diag(np.ones(3)) 411 | r_ceb = np.diag(np.ones(3)) 412 | ## sec 2 ############################################################## 413 | r_fs = np.array([-1.6095, 0, 0]) * 1000 414 | r_ms = np.array([0, 0, 0]) 415 | r_fg = np.array([0, -3361, 0]) 416 | ## sec 3 ############################################################## 417 | r_mg = np.array([144.2560, 0, -360.6400]) 418 | r_ft = np.array([0, 0, 0]) 419 | r_mt = np.array([0, 0, 0]) 420 | ## sec 4 ############################################################## 421 | r_f = 1.0e+04 * np.array([1.2282, -0.3361, 0, 0.0144, 0, -0.0361]) 422 | r_a = 1.0e+04 * np.array([ 423 | [0.1873, 0, 0, 0, 0.0015, 0.0009], 424 | [ 0, 0.3501, 0, -0.0015, 0, -0.0581], 425 | [ 0, 0, 0.3501, -0.0009, 0.0581, 0], 426 | [ 0, -0.0015, -0.0009, 0.0561, 0, 0], 427 | [0.0015, 0, 0.0581, 0, 1.4148, 0], 428 | [0.0009, -0.0581, 0, 0, 0, 1.4148] 429 | ]), 430 | r_df_speed = np.array([6.5567, -0.9706, 0.0018, 0.2316, -0.0069, -0.0696]) 431 | r_df_angle = np.array([0, 0, 0]) 432 | print('sec 1: ------------------------------------------------------------') 433 | print('cvb is ', test['cvb'], '. should be ', r_cvb) 434 | print('ceb is ', test['ceb'], '. should be ', r_ceb) 435 | print('sec 2: ------------------------------------------------------------') 436 | print('fs is ', test['fs'], '. should be ', r_fs) 437 | print('ms is ', test['ms'], '. should be ', r_ms) 438 | print('fg is ', test['fg'], '. should be ', r_fg) 439 | print('sec 3: ------------------------------------------------------------') 440 | print('mg is ', test['mg'], '. should be ', r_mg) 441 | print('ft is ', test['ft'], '. should be ', r_ft) 442 | print('mt is ', test['mt'], '. should be ', r_mt) 443 | print('sec 4 -------------------------------------------------------------') 444 | print('f is ', test['f'], '. should be ', r_f) 445 | print('a is ', test['a'], '. should be ', r_a) 446 | print('df_speed is ', test['df_speed'], '. should be ', r_df_speed) 447 | print('df_angle is ', test['df_angle'], '. should be ', r_df_angle) 448 | print('result is ', trans(dot_state)) 449 | 450 | elif testFlag == 2: 451 | # import matplotlib.pyplot as plt 452 | para = UuvParas() 453 | agent = UuvModel(para) 454 | s2 = np.zeros(12) 455 | agent.velocity[0] = 10 456 | xi = np.array([0, 0, 0, 10, 0, 0, 457 | 0, 0, 0, 0, 0, 0]) 458 | a0 = np.array([ 0, 0, 0, 10672]) 459 | # record = MemoryStore.DataRecord() 460 | # record.clear() 461 | step_cnt = 0 462 | action2 = np.zeros(4) 463 | action2[3] = 10672 464 | print('action: ', action2) 465 | agent.step(action2) 466 | s1 = agent.observe() 467 | s2[0:3] = s1[3:6] 468 | s2[3:6] = s1[9:12] 469 | s2[6:9] = s1[6:9] 470 | s2[9:12] = s1[3:6] 471 | print('1', s2) 472 | elif testFlag == 3: 473 | import matplotlib.pyplot as plt 474 | para = UuvParas() 475 | agent = UuvModel(para) 476 | agent.velocity[0] = 10 477 | xi = np.array([0, 0, 0, 10, 0, 0, 478 | 0, 0, 0, 0, 0, 0]) 479 | a0 = np.array([ 0, 0, 0, 10672]) 480 | record = MemoryStore.DataRecord() 481 | record.clear() 482 | step_cnt = 0 483 | for i in range(2000): 484 | ref = np.array([0., 0., 1., 0.]) 485 | stateTemp = agent.observe() 486 | 487 | print(stateTemp) 488 | action2 = np.zeros(4) 489 | action2[3] = 10672 490 | # action2, oil = quad1.get_controller_pid(stateTemp, ref) 491 | print('action: ', action2) 492 | # action2 = np.clip(action2, 0.1, 0.9) 493 | agent.step(action2) 494 | record.buffer_append((stateTemp, action2)) 495 | step_cnt = step_cnt + 1 496 | record.episode_append() 497 | 498 | 499 | data = record.get_episode_buffer() 500 | bs = data[0] 501 | ba = data[1] 502 | t = range(0, record.count) 503 | # mpl.style.use('seaborn') 504 | fig1 = plt.figure(1) 505 | plt.clf() 506 | plt.subplot(3, 1, 1) 507 | plt.plot(t, bs[t, 6] / D2R, label='roll') 508 | plt.plot(t, bs[t, 7] / D2R, label='pitch') 509 | plt.plot(t, bs[t, 8] / D2R, label='yaw') 510 | plt.ylabel('Attitude $(\circ)$', fontsize=15) 511 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 512 | plt.subplot(3, 1, 2) 513 | plt.plot(t, bs[t, 4], label='y') 514 | plt.plot(t, bs[t, 5], label='z') 515 | plt.ylabel('Position (m)', fontsize=15) 516 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 517 | plt.subplot(3, 1, 3) 518 | plt.plot(t, bs[t, 3], label='x') 519 | plt.ylabel('Forward (m)', fontsize=15) 520 | plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05)) 521 | plt.show() --------------------------------------------------------------------------------