├── .gitignore ├── .idea ├── Otter-USV-AUV-glider-tracking.iml ├── inspectionProfiles │ ├── Project_Default.xml │ └── profiles_settings.xml ├── misc.xml ├── modules.xml ├── shelf │ ├── Uncommitted_changes_before_rebase_[Changes] │ │ └── shelved.patch │ └── Uncommitted_changes_before_rebase__Changes_.xml ├── vcs.xml └── workspace.xml ├── AiControllerTrainer.py ├── LICENSE ├── README.md ├── RL_controller ├── AiControllerTesting.py ├── TargetTrackingAIController.py └── gym │ └── training_env │ └── MarineVehicleTargetTrackingEnv.py ├── config ├── __init__.py └── config.yml ├── main.py ├── mpc ├── TargetTrackingMPC.py ├── casadi_otter_model_3DOF.py └── casadi_sim.py ├── otter └── otter.py ├── results ├── TargetTrackingAI │ ├── Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-21 │ │ ├── AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_NED.pdf │ │ ├── AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_compute_time.pdf │ │ ├── AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_controls.pdf │ │ ├── AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_error.pdf │ │ ├── AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_velocities.pdf │ │ └── copy_config.yml │ ├── Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-59 │ │ ├── AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_NED.pdf │ │ ├── AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_compute_time.pdf │ │ ├── AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_controls.pdf │ │ ├── AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_error.pdf │ │ ├── AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_velocities.pdf │ │ └── copy_config.yml │ ├── Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-24 │ │ ├── AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_NED.pdf │ │ ├── AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_compute_time.pdf │ │ ├── AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_controls.pdf │ │ ├── AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_error.pdf │ │ ├── AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_velocities.pdf │ │ └── copy_config.yml │ ├── Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-38 │ │ ├── AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_NED.pdf │ │ ├── AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_compute_time.pdf │ │ ├── AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_controls.pdf │ │ ├── AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_error.pdf │ │ ├── AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_velocities.pdf │ │ └── copy_config.yml │ ├── Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-19 │ │ ├── AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_NED.pdf │ │ ├── AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_compute_time.pdf │ │ ├── AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_controls.pdf │ │ ├── AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_error.pdf │ │ ├── AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_velocities.pdf │ │ └── copy_config.yml │ └── Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-59 │ │ ├── AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_NED.pdf │ │ ├── AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_compute_time.pdf │ │ ├── AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_controls.pdf │ │ ├── AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_error.pdf │ │ ├── AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_velocities.pdf │ │ └── copy_config.yml └── TargetTrackingMPC │ ├── 2023-04-29 13-31-25_case 1 │ ├── .png │ ├── NED.pdf │ ├── config.yml │ ├── control_compute_time.pdf │ ├── control_forces.pdf │ ├── control_rpms.pdf │ ├── error.pdf │ ├── simData.csv │ ├── sim_time.csv │ ├── target_trajectory.csv │ ├── time_controller_computation.csv │ ├── vel.pdf │ └── velocities.pdf │ └── 2023-04-29 15-17-51_ case 2 │ ├── config.yml │ ├── res_mpc_case2_NED.pdf │ ├── res_mpc_case2_control_compute_time.pdf │ ├── res_mpc_case2_control_forces.pdf │ ├── res_mpc_case2_control_rpms.pdf │ ├── res_mpc_case2_error.pdf │ ├── res_mpc_case2_velocities.pdf │ ├── simData.csv │ ├── sim_time.csv │ ├── target_trajectory.csv │ └── time_controller_computation.csv ├── simulator.py ├── thesis.pdf └── utils.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ 130 | 131 | #dirs 132 | trash 133 | tests 134 | /tests/ 135 | /trash/ 136 | /AI_controller/logs/ 137 | /RL_controller/logs/ 138 | -------------------------------------------------------------------------------- /.idea/Otter-USV-AUV-glider-tracking.iml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 14 | 15 | 17 | -------------------------------------------------------------------------------- /.idea/inspectionProfiles/Project_Default.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | -------------------------------------------------------------------------------- /.idea/inspectionProfiles/profiles_settings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | -------------------------------------------------------------------------------- /.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /.idea/shelf/Uncommitted_changes_before_rebase__Changes_.xml: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /.idea/workspace.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 20 | 21 | 27 | 28 | 29 | 34 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 50 | 51 | 53 | 54 | 55 | 58 | { 59 | "keyToString": { 60 | "RunOnceActivity.OpenProjectViewOnStart": "true", 61 | "RunOnceActivity.ShowReadmeOnStart": "true", 62 | "WebServerToolWindowFactoryState": "false", 63 | "codeWithMe.voiceChat.enabledByDefault": "false", 64 | "last_opened_file_path": "C:/Users/aksel/OneDrive/ACIT-Robotikk/Master Thesis/Code/Otter-USV-AUV-glider-tracking/AI_controller/archive", 65 | "node.js.detected.package.eslint": "true", 66 | "node.js.selected.package.eslint": "(autodetect)", 67 | "settings.editor.selected.configurable": "editor.preferences.completion" 68 | } 69 | } 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 109 | 110 | 111 | 131 | 132 | 133 | 153 | 154 | 155 | 175 | 176 | 177 | 197 | 198 | 199 | 219 | 220 | 221 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 1673357621198 265 | 309 | 310 | 1674472685097 311 | 316 | 317 | 1674644089273 318 | 323 | 324 | 1676361179463 325 | 330 | 331 | 1678369723444 332 | 337 | 338 | 1683890389290 339 | 344 | 345 | 1683893202342 346 | 351 | 352 | 1683893270556 353 | 358 | 359 | 1683893337767 360 | 365 | 366 | 1683893514337 367 | 372 | 373 | 1683894184271 374 | 379 | 380 | 1683894664844 381 | 386 | 387 | 1683895748603 388 | 393 | 394 | 1683895788450 395 | 400 | 403 | 404 | 406 | 407 | 442 | 447 | 448 | 449 | 450 | 452 | 453 | 454 | 455 | 456 | 457 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 467 | 468 | -------------------------------------------------------------------------------- /AiControllerTrainer.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from stable_baselines3.common.callbacks import CheckpointCallback, StopTrainingOnNoModelImprovement, EvalCallback 3 | from stable_baselines3.common.env_util import make_vec_env 4 | 5 | import utils 6 | from AI_controller.gym.env.MarineVehicleTargetTrackingEnv import TargetTrackingEnv 7 | from otter.otter import otter 8 | from stable_baselines3 import PPO 9 | import os 10 | from config import config 11 | from datetime import datetime 12 | from stable_baselines3.common.logger import configure 13 | import torch as th 14 | from stable_baselines3.common.vec_env import DummyVecEnv, VecNormalize, \ 15 | VecFrameStack, SubprocVecEnv 16 | from stable_baselines3.common.vec_env.base_vec_env import VecEnv, VecEnvStepReturn, VecEnvWrapper 17 | 18 | # import modifiers 19 | #np.seterr(all='raise') 20 | def make_my_env(): 21 | temp_env = TargetTrackingEnv() 22 | vehicle = otter(0, 0, 0, 0, 0) 23 | temp_env.vehicle = vehicle 24 | return temp_env 25 | # fetch config 26 | th.autograd.set_detect_anomaly(True) 27 | continue_training = config['continue_training'] 28 | debug = config['env']['debug'] 29 | vehicle = otter(0, 0, 0, 0, 0) 30 | n_stack = config['n_stacked_frames'] 31 | 32 | architecture = config['architecture'] 33 | starting_learning_rate = config['starting_learning_rate'] 34 | 35 | 36 | log_dir = os.path.join('AI_controller', 'trash/logs', config['model'], 37 | f"{config['env']['time_out']}_" 38 | f"{config['env']['target_spawn_region']}_" 39 | f"{config['env']['target_confident_region']}_" 40 | f"{config['env']['random_target']}_" 41 | f"{config['env']['c1']}_" 42 | f"{config['env']['c2']}_" 43 | f"{config['env']['c3']}_" 44 | f"{config['env']['c4']}_" 45 | f"{config['env']['c5']}_" 46 | f"{config['env']['c6']}_" 47 | f"{config['env']['c7']}_" 48 | f"{config['starting_learning_rate']}", 49 | datetime.now().strftime('%Y-%m-%d %H-%M-%S')) 50 | model_dir = os.path.join(log_dir, 'model') 51 | 52 | model_prefix = config['model'] + '_' 53 | 54 | if debug: 55 | model_dir = os.path.join('AI_controller', 'models', 'debug', config['model'], 56 | datetime.now().strftime('%Y-%m-%d %H-%M-%S')) 57 | log_dir = os.path.join('AI_controller', 'trash/logs', 'debug', config['model']) 58 | model_prefix = config['model'] + '_' 59 | 60 | if not os.path.exists('./' + log_dir): 61 | os.makedirs(log_dir) 62 | 63 | 64 | 65 | #create training environment 66 | env = TargetTrackingEnv() 67 | env.vehicle = vehicle 68 | env = make_vec_env(lambda: make_my_env(), n_envs=config['n_env']) 69 | env = VecFrameStack(env, n_stack=n_stack) 70 | env = VecNormalize(venv=env) 71 | env.reset() 72 | 73 | # Separate evaluation env 74 | eval_env = TargetTrackingEnv() 75 | eval_env.vehicle = otter(0, 0, 0, 0, 0) # define vehicle for environment (should be passed when initializing) 76 | eval_env = make_vec_env(lambda: eval_env, n_envs=1) 77 | eval_env = VecFrameStack(eval_env, n_stack=n_stack) 78 | eval_env = VecNormalize(venv=eval_env) 79 | eval_env.reset() 80 | 81 | def lrsched(alpha0): 82 | def reallr(progress): 83 | return alpha0 * np.exp(-(1 - progress)) 84 | 85 | return reallr 86 | 87 | 88 | policy_kwargs = None 89 | 90 | if continue_training: 91 | print("Loading trained model_and continuing training") 92 | # finding existing model 93 | tmp_log_dir = config['log_dir'] 94 | #model_dir = 'AI_controller/logs/PPO/120_100_1_True_0_100_1_1_100_5/2023-03-08 17-36-18/' 95 | continue_from_model = 'best_model' 96 | model = PPO.load(f"{tmp_log_dir}/{continue_from_model}", env=env) 97 | reset_num_timesteps = True 98 | 99 | else: 100 | print("Starting new training session") 101 | policy_kwargs = dict(activation_fn=th.nn.ReLU, 102 | net_arch=dict(pi=architecture, vf=architecture)) 103 | reset_num_timesteps = True 104 | model = PPO("MlpPolicy", env=env, verbose=1, tensorboard_log=log_dir, learning_rate=lrsched(starting_learning_rate), 105 | policy_kwargs=policy_kwargs, device='cuda') 106 | 107 | # Defining callbacks 108 | 109 | checkpoint_callback = CheckpointCallback( 110 | save_freq= max(100_000 // config['n_env'], 1), 111 | save_path=log_dir+'/models', 112 | name_prefix=model_prefix, 113 | save_replay_buffer=True, 114 | save_vecnormalize=True, 115 | # verbose=1 116 | ) 117 | # Stop training if there is no improvement after more than 5 evaluations 118 | stop_train_callback = StopTrainingOnNoModelImprovement(max_no_improvement_evals=4, min_evals=5, verbose=1) 119 | 120 | eval_callback = EvalCallback(eval_env, eval_freq=max(100_000 // config['n_env'], 1), 121 | #callback_after_eval=stop_train_callback, 122 | n_eval_episodes=10, 123 | best_model_save_path=log_dir, 124 | deterministic= True, 125 | render= False, 126 | verbose=1, ) 127 | utils.copy_config(log_dir)#copy the config file for documentation 128 | print(model.policy) 129 | 130 | model.learn(total_timesteps=config['total_timesteps'], 131 | reset_num_timesteps=reset_num_timesteps, 132 | tb_log_name=f"PPO_arch_{str(architecture)}", 133 | callback=[checkpoint_callback, eval_callback]) # 134 | 135 | # save the normalization statistics (to be used in inference) 136 | env.save(log_dir+f"\\norm.pickle") 137 | 138 | # test model 139 | 140 | obs = env.reset() 141 | trajectory = [] 142 | target = vehicle.target 143 | done = False 144 | while not done: 145 | trajectory = env.get_attr('trajectory')[0] # we must call this first because the stacked env is reseting the environment when done. 146 | action, _states = model.predict(obs, deterministic=True) 147 | obs, reward, done, info = env.step(action) 148 | #env.render() 149 | 150 | 151 | utils.plot_trajectory(trajectory= trajectory, target=target) 152 | env.close() 153 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 akseljohan 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Otter-USV-glider-tracking 2 | Master thesis repo for implementation of Model Predictive Controller and Reinforcement Learning (RL) controller for an Otter USV. 3 | 4 | image 5 | 6 | 7 | The repo make use of a modifyed version of Fossen Vehicle Simulator with edited files. Casadi is used for implementing a 3DOF model of the Otter together with an IPOPT solver and RungeKutta 4-dim for diskretisation. 8 | 9 | 10 | 11 | 12 | 13 | The thesis may be found here: https://oda.oslomet.no/oda-xmlui/handle/11250/3100591 14 | -------------------------------------------------------------------------------- /RL_controller/AiControllerTesting.py: -------------------------------------------------------------------------------- 1 | import shutil 2 | import time 3 | 4 | import numpy as np 5 | from matplotlib import pyplot as plt 6 | from stable_baselines3.common.callbacks import CheckpointCallback 7 | from stable_baselines3.common.env_util import make_vec_env 8 | from stable_baselines3.common.vec_env import VecFrameStack, VecNormalize 9 | 10 | import utils 11 | from AI_controller.gym.env.MarineVehicleTargetTrackingEnv import TargetTrackingEnv 12 | from otter.otter import otter 13 | from stable_baselines3 import PPO 14 | import os 15 | from config import config 16 | from datetime import datetime 17 | from stable_baselines3.common.logger import configure 18 | import torch as th 19 | 20 | debug = False 21 | 22 | models_dir = "../trash/models/PPO/" 23 | file_name = "best_model" 24 | 25 | # mod3el_path =f"AI_controller/logs/PPO_end_when_reaching_target_true/1000_150_5_True_0_1_1_1_0_1/2023-03-25 19-43-22/best_model.zip" 26 | # f"AI_controller/logs/PPO_end_when_reaching_target_true/1000_150_5_True_0_1_1_1_0_1/2023-03-25 19-43-25/best_model.zip" 27 | model_path = "AI_controller/logs/PPO_moving_target_normalized/1000_30_1_True_0_2_1_1_0_1/2023-04-09 17-13-42/best_model.zip" 28 | #f"AI_controller/logs/PPO_moving_target_normalized/1000_50_1_True_1_0_0.01_0.1_0_0_1/2023-04-16 17-35-34/models/PPO_moving_target_normalized__78900000_steps.zip" 29 | #f"AI_controller/logs/PPO_moving_target_normalized/1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06/2023-04-17 17-54-54/models/PPO_moving_target_normalized__29500000_steps.zip" 30 | #f"AI_controller/logs/PPO_moving_target_normalized/1000_50_1_True_0_1_0.01_0.1_0_1_1/2023-04-16 22-31-27/models/PPO_moving_target_normalized__67700000_steps.zip" 31 | # 32 | # 33 | 34 | #these have plotted results: 35 | #f"AI_controller/logs/PPO_moving_target_normalized/1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06/2023-04-17 17-54-54/models/PPO_moving_target_normalized__29500000_steps.zip" 36 | #f"AI_controller/logs/PPO_moving_target_normalized/1000_50_1_True_0_1_0.01_0.1_0_1_1/2023-04-16 22-31-27/models/PPO_moving_target_normalized__67700000_steps.zip" 37 | #these have not plotted results 38 | #f"AI_controller/logs/PPO_moving_target_normalized/1000_50_1_True_1_0_0.01_0.1_0_0_0/2023-04-15 15-17-49/models/PPO_moving_target_normalized__69000000_steps.zip" 39 | #f"AI_controller/logs/PPO_moving_target_normalized/1000_50_1_True_1_0_0.01_0.1_0_0_1/2023-04-16 17-35-34/models/PPO_moving_target_normalized__64200000_steps.zip" 40 | # f"AI_controller/logs/PPO_moving_target_normalized/1000_50_1_True_1_0_0.01_0.1_0_0_0/2023-04-15 15-17-49/models/PPO_moving_target_normalized__69000000_steps.zip" 41 | # f"AI_controller/logs/PPO_moving_target_normalized/1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06/2023-04-17 17-54-54/models/PPO_moving_target_normalized__17800000_steps.zip" 42 | # f"AI_controller/logs/PPO_moving_target_normalized/1000_50_1_True_1_0_0.1_0.1_0_0_0/2023-04-13 16-43-39/models/PPO_moving_target_normalized__8000000_steps.zip" 43 | # "AI_controller\\logs\\PPO_moving_target_normalized\\1000_50_1_True_1_0_0.1_0.1_0_0_0\\2023-04-11 22-09-23\\models\\PPO_moving_target_normalized__3500000_steps.zip" 44 | # f"AI_controller/logs/PPO_moving_target_normalized/1000_50_5_True_1_0_0.1_0.1_0_0_0/2023-04-11 12-21-40/models/PPO_moving_target_normalized__2000000_steps.zip" 45 | # f"AI_controller/logs/PPO_moving_target_normalized/1000_30_5_False_1_0_0_0.1_0_0_0/2023-04-11 09-43-59/models/PPO_moving_target_normalized__2000000_steps.zip" 46 | # f"AI_controller/logs/PPO_moving_target_normalized/1000_30_5_False_0_1_0_0.1_0_1_1/2023-04-11 08-26-41/models/PPO_moving_target_normalized__1000000_steps.zip" 47 | 48 | vec_normalized_path = f"AI_controller/logs/PPO_moving_target_normalized/1000_50_1_True_1_0_0.01_0.1_0_0_1/2023-04-16 17-35-34/models/PPO_moving_target_normalized__vecnormalize_78900000_steps.pkl" 49 | #f"AI_controller/logs/PPO_moving_target_normalized/1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06/2023-04-17 17-54-54/models/PPO_moving_target_normalized__vecnormalize_29500000_steps.pkl" 50 | #f"AI_controller/logs/PPO_moving_target_normalized/1000_50_1_True_0_1_0.01_0.1_0_1_1/2023-04-16 22-31-27/models/PPO_moving_target_normalized__vecnormalize_67700000_steps.pkl" 51 | #f"AI_controller/logs/PPO_moving_target_normalized/1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06/2023-04-17 17-54-54/models/PPO_moving_target_normalized__vecnormalize_29500000_steps.pkl" 52 | # 53 | #the models below has ploted results 54 | #f"AI_controller/logs/PPO_moving_target_normalized/1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06/2023-04-17 17-54-54/models/PPO_moving_target_normalized__vecnormalize_29500000_steps.pkl" 55 | # 56 | 57 | os.path.dirname(vec_normalized_path) 58 | #f"AI_controller/logs/PPO_moving_target_normalized/1000_50_1_True_1_0_0.01_0.1_0_0_0/2023-04-15 15-17-49/models/PPO_moving_target_normalized__vecnormalize_69000000_steps.pkl" 59 | 60 | #f"AI_controller/logs/PPO_moving_target_normalized/1000_50_1_True_1_0_0.01_0.1_0_0_1/2023-04-16 17-35-34/models/PPO_moving_target_normalized__vecnormalize_64200000_steps.pkl" 61 | # f"AI_controller/logs/PPO_moving_target_normalized/1000_50_1_True_1_0_0.01_0.1_0_0_0/2023-04-15 15-17-49/models/PPO_moving_target_normalized__vecnormalize_69000000_steps.pkl" 62 | # f"AI_controller/logs/PPO_moving_target_normalized/1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06/2023-04-17 17-54-54/models/PPO_moving_target_normalized__vecnormalize_17800000_steps.pkl" 63 | # f"AI_controller/logs/PPO_moving_target_normalized/1000_50_1_True_1_0_0.1_0.1_0_0_0/2023-04-13 16-43-39/models/PPO_moving_target_normalized__vecnormalize_8000000_steps.pkl" 64 | # f"AI_controller\\logs\\PPO_moving_target_normalized\\1000_50_1_True_1_0_0.1_0.1_0_0_0\\2023-04-11 22-09-23\\models\\PPO_moving_target_normalized__vecnormalize_3500000_steps.pkl" 65 | # This is good: 66 | # f"AI_controller/logs/PPO_moving_target_normalized/1000_50_5_True_1_0_0.1_0.1_0_0_0/2023-04-11 12-21-40/models/PPO_moving_target_normalized__vecnormalize_2000000_steps.pkl" 67 | # f"AI_controller/logs/PPO_moving_target_normalized/1000_30_5_False_1_0_0_0.1_0_0_0/2023-04-11 09-43-59/models/PPO_moving_target_normalized__vecnormalize_2000000_steps.pkl" 68 | # f"AI_controller/logs/PPO_moving_target_normalized/1000_30_5_False_0_1_0_0.1_0_1_1/2023-04-11 08-26-41/models/PPO_moving_target_normalized__vecnormalize_1000000_steps.pkl" 69 | 70 | 71 | vehicle = otter('TargetTrackingAI', 0.0, 0.0, 0, 000.0) 72 | 73 | env = TargetTrackingEnv() 74 | # env.render_mode = 'human' 75 | env.vehicle = vehicle 76 | env.vehicle.target = config['env']['fixed_target'] 77 | env = make_vec_env(lambda: env, n_envs=1) 78 | n_stack = config['n_stacked_frames'] 79 | env = VecFrameStack(env, n_stack=n_stack) 80 | env = VecNormalize.load(load_path=vec_normalized_path, venv=env) # load normalization paramaters from training 81 | sampleTime = config['sample_time'] 82 | # env.reset() 83 | 84 | 85 | # loading the model 86 | model = PPO.load(model_path, env=env) 87 | print(model.policy) 88 | print(f"model.observation.shape: {model.observation_space.shape}") 89 | print(f"model.n_steps: {model.n_steps}") 90 | print(f"model.action_noise: {model.action_noise}") 91 | print(f"model.num_timesteps: {model.num_timesteps}") 92 | episodes = 1 93 | time_compute_controller_signals = [] 94 | for ep in range(episodes): 95 | print("New episode") 96 | # env.set_attr(attr_name='cycle_counter', value= 0) 97 | obs = env.reset() 98 | # env.set_attr(attr_name='cycle_counter', value=0, indices=0) 99 | # print(env.obs_rms.var) 100 | #print(obs) 101 | done = False 102 | trajectory = np.array([[0], [0], [0]]) 103 | # print(trajectory) 104 | rewards = 0 105 | rew_tot = 0 106 | target = vehicle.target 107 | rew_trajectory = [] 108 | 109 | while not done: 110 | trajectory = env.get_attr('trajectory')[ 111 | 0] # when using vectorized environments it is reset when done, therfore we must fetch the trajectory before the last run 112 | 113 | action, _states = model.predict(obs, deterministic=True) 114 | start_time = time.time() 115 | obs, rewards, done, info = env.step(action) 116 | end_time = time.time() # end timing how long it takes to compute controller signals 117 | time_compute_controller_signals.append(end_time - start_time) # log 118 | 119 | rew_trajectory.append(rewards) 120 | 121 | rew_tot += rewards 122 | dir_name = os.path.dirname(os.path.dirname(vec_normalized_path)) 123 | hyper_params = os.path.basename(os.path.dirname(dir_name)) 124 | result_dir = os.path.join('results', vehicle.controlMode, 125 | f"{hyper_params} {datetime.now().strftime('%Y-%m-%d %H-%M-%S')}") # create a log dir for the results 126 | if not os.path.exists(result_dir): 127 | os.makedirs(result_dir) 128 | #copy the config file 129 | shutil.copy(src=f'{os.path.dirname(os.path.dirname(vec_normalized_path))}\config.yml', dst=f"{result_dir}/copy_config.yml") # copy the config file for documentation 130 | 131 | print(f"rew_tot: {rew_tot}") 132 | vehicle = env.get_attr('vehicle')[0] 133 | plt.plot(list(range(0, len(rew_trajectory))), rew_trajectory, label='reward history') 134 | plt.title(f"run: {ep}") 135 | plt.show() 136 | target_trajectory = trajectory[[6, 7], :] 137 | # print results 138 | 139 | pos_3dof = trajectory[[0, 1, 2, 3, 4, 5], :] 140 | utils.plot_trajectory(trajectory=pos_3dof, target=target_trajectory, 141 | file_path=f'{result_dir}/AI_{hyper_params}_NED.pdf') 142 | 143 | # plot error plot 144 | utils.plot_error(trajectory[[0, 1], :].T, pos_target=target_trajectory.T, path=f'{result_dir}/AI_{hyper_params}_error.pdf', 145 | sample_time=sampleTime) 146 | 147 | # plot the controller signals 148 | """utils.plot_controls(u_control=simData[:, [12, 13]], 149 | u_actual=simData[:, [14, 15]], 150 | sample_time=sampleTime, 151 | file_path=f'{result_dir}/controls.pdf')""" 152 | print(trajectory.shape) 153 | utils.plot_veloceties(vel_matrix_3DOF=trajectory[[3, 4, 5], :], 154 | #action_matrix=trajectory[[14, 15], :], 155 | sample_time=sampleTime, 156 | file_path=f'{result_dir}/AI_{hyper_params}_velocities.pdf') 157 | 158 | utils.plot_solv_time(solv_time_data=time_compute_controller_signals, sample_time=sampleTime, file_path=f'{result_dir}/AI_{hyper_params}_compute_time.pdf') 159 | 160 | utils.plot_control_forces(tau=trajectory[[14, 15], :].T, sample_time=sampleTime, file_path=f'{result_dir}/AI_{hyper_params}_controls.pdf') 161 | plt.show() -------------------------------------------------------------------------------- /RL_controller/TargetTrackingAIController.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from matplotlib import pyplot as plt 3 | 4 | import utils 5 | from stable_baselines3 import PPO 6 | import os 7 | from datetime import datetime 8 | from stable_baselines3.common.logger import configure 9 | import math 10 | import yaml 11 | 12 | # velocity 13 | class TargetTrackingAIController: 14 | def __init__(self, model_name, config_name): 15 | """ 16 | NOT WORKING YET! This class is not used to test teh AI controllers. The AIControllerTesting is used. 17 | 18 | A self-contained controller. Requier a config file in the sam dir as the model. 19 | Must also have access to the normalisation statistics for the observations. 20 | 21 | :param model_dir: path to dir where model and file is stored 22 | :param congif_name: path and to config file .yaml 23 | """ 24 | 25 | self.model_name = model_name 26 | self.config = yaml.safe_load(open((config_name))) 27 | self.eta = None 28 | # self.target = config['env']['fixed_target'] 29 | self.model = self.load_PPO_model(model_path=model_name) 30 | self.observation_shape = self.model.observation_space.shape 31 | self.n_stacked_frames = self.config['n_stacked_frames'] 32 | self.obs_size = self.observation_shape[ 33 | 0] / self.n_stacked_frames # the amount of stacked observations for the agent to make its predictions o 34 | self.stacked_observations = self.__initiate_stacked_observations() #stacked observations for the model to make its predictions on 35 | print(f"observations.shape:{self.stacked_observations.shape}") 36 | self.trajectory = np.zeros((int(self.obs_size), 1)) # the trajectory of the observations 37 | self.sample_time = self.config['sample_time'] 38 | self.sim_length = self.config['env']['sim_length'] 39 | self.radius = self.config['env']['target_confident_region'] 40 | self.time_out = self.config['env']['time_out'] 41 | self.action = [] 42 | 43 | def get_psi_d(self, eta, target): 44 | """Returns the angle between of the line from the vessel to the target in NED""" 45 | return math.atan2(target[1] - eta[1], target[0] - eta[0]) 46 | 47 | def get_smalles_sign_angle(self, x=None, y=None, angle=None): # page 413. in Fossen 2021 48 | 49 | if angle is not None: 50 | return (angle + math.pi) % (2 * math.pi) - math.pi 51 | if x is not None and y is not None: 52 | a = (x - y) % (2 * np.pi) 53 | b = (y - x) % (2 * np.pi) 54 | return -a if a < b else b 55 | else: 56 | raise ValueError("you must define either x and y, or angle") 57 | 58 | def get_euclidean_distance(self, eta, target): 59 | return np.linalg.norm(np.array([eta[0], eta[1]]) - target) 60 | 61 | def get_speed_towards_target(self, trajectory, target): 62 | 63 | if trajectory.shape[1] > 1: 64 | pos_k2 = np.array([trajectory[0, -2], trajectory[1, -2]]) 65 | pos_k1 = np.array([trajectory[0, -1], trajectory[1, -1]]) 66 | return (np.linalg.norm(pos_k2 - target) - np.linalg.norm( 67 | pos_k1 - target)) / self.sim_length 68 | else: 69 | return 0 70 | 71 | def __initiate_stacked_observations(self): 72 | return np.zeros(shape=self.model.observation_space.shape) 73 | 74 | def stack_observations(self, observation_stack, new_observation): 75 | """ 76 | :param new_observations: new observation elements (18,1) to be added to the stacked observations 77 | :return: the new stacked_observation matrix 78 | """ 79 | #shifted_mat = np.roll(self.stacked_observations, -self.obs_size, axis=0) #shifts the 80 | 81 | tmp = np.hstack((observation_stack, new_observation)) 82 | tmp = tmp[int(self.obs_size) :] 83 | if tmp.shape == self.model.observation_space.shape: 84 | #print(f"tmp.shape_after removal:{tmp.shape}") 85 | return tmp 86 | else: 87 | print(f"the new observation stack({tmp.shape}), does not match the model observation shape ({self.model.observation_space.shape})") 88 | return observation_stack 89 | 90 | def load_PPO_model(self, model_path): 91 | # loading the model 92 | try: 93 | return PPO.load(model_path, env=None) 94 | except Exception as e: 95 | print(e) 96 | 97 | def get_delta_pos_target(self, eta, target): 98 | delta_x = abs(eta[0] - target[0]) 99 | delta_y = abs(eta[1] - target[1]) 100 | return delta_x, delta_y 101 | 102 | def get_action_dot(self, sample_time): 103 | return (np.linalg.norm(self.trajectory[14:16, -1]) - np.linalg.norm(self.trajectory[14:16, -2])) / sample_time 104 | 105 | def set_up(self, initial_state, target): 106 | print("Setting up controller)") 107 | self.t = 0 # time is set to zero 108 | eta = initial_state[:3] # internal 3DOF eta 109 | nu = initial_state[3:] # internal 3DOF nu 110 | #print(self.model.predict(self.stacked_observations)) 111 | # calculate and add observations to trajectory 112 | #self.action_trajectory = np.zeros((self.observation_space.shape[0],)) 113 | # set observations 114 | observation = np.array([eta[0], # x (N) 115 | eta[1], # y (E) 116 | eta[2], # psi (angle from north) 117 | nu[0], # surge vel. 118 | nu[1], # sway vel. 119 | nu[2], # yaw vel. 120 | target[0], # x, target 121 | target[1], # y, target 122 | self.get_delta_pos_target(eta, target)[0], # distance between vehicle and target in x 123 | self.get_delta_pos_target(eta, target)[1], # distance between vehicle and target in y 124 | self.get_euclidean_distance(eta, target), # euclidean distance vehicle and target 125 | 0, # difference in the two previous actions 126 | 0, 127 | self.get_smalles_sign_angle(angle=(self.get_psi_d(eta=eta, target=target) - eta[2])), 128 | # the angle between the target and the heading of the vessle 129 | 0, # action surge 130 | 0, # action sway 131 | 0, # time awareness, 132 | self.radius # aware of the region around the target 133 | ], dtype=np.float32) 134 | self.trajectory = np.append(self.trajectory, np.array([observation]).T, 135 | axis=1) # appends a trajectory of observations# initiate observations 136 | 137 | def get_controller_action(self, initial_state, target, sample_time, t ): 138 | #print(f" t: {t}, self.time_out: {self.time_out},t/self.sample_time :{t/self.sample_time}, t % self.time_out: {cc}") 139 | #print(t % self.time_out) 140 | if t ==0: 141 | self.set_up(initial_state=initial_state, target= target) 142 | elif (t/self.sample_time )% self.time_out ==0: 143 | print("Training time period reached") 144 | self.set_up(initial_state=initial_state, target=target) 145 | 146 | 147 | #if t> self.time_out: 148 | # self.set_up() 149 | eta = initial_state[:3] # internal 3DOF eta 150 | nu = initial_state[3:] # internatl 3DOF nu 151 | #print(self.model.predict(self.stacked_observations)) 152 | # calculate and add observations to trajectory 153 | 154 | #predict action absed on last timestep: 155 | norm_action = self.model.predict(observation=self.stacked_observations, deterministic=True)[0] 156 | norm_action = norm_action 157 | # set observations (just so that the action is tace 158 | observation = np.array([eta[0], # x (N) 159 | eta[1], # y (E) 160 | eta[2], # psi (angle from north) 161 | nu[0], # surge vel. 162 | nu[1], # sway vel. 163 | nu[2], # yaw vel. 164 | target[0], # x, target 165 | target[1], # y, target 166 | self.get_delta_pos_target(eta, target)[0], # distance between vehicle and target in x 167 | self.get_delta_pos_target(eta, target)[1], # distance between vehicle and target in y 168 | self.get_euclidean_distance(eta, target), # euclidean distance vehicle and target 169 | self.get_action_dot(sample_time=sample_time), # difference in the two previous actions 170 | self.get_speed_towards_target(trajectory=self.trajectory, target=target), 171 | self.get_smalles_sign_angle(angle=(self.get_psi_d(eta=eta, target=target) - eta[2])), 172 | # the angle between the target and the heading of the vessle 173 | norm_action[0], # action surge 174 | norm_action[1], # action sway 175 | t / self.time_out, # time awareness, #TODO set up a modulus for calculaing this since the simulation time can be larger than the training horizon? 176 | 1,#self.radius # aware of the region around the target 177 | ], dtype=np.float32) 178 | np.set_printoptions(linewidth= 2000,precision=3, formatter={'float': '{: 0.3f}'.format}) 179 | #print(observation) 180 | self.trajectory = np.append(self.trajectory, np.array([observation]).T, 181 | axis=1) # appends a trajectory of observations 182 | self.stacked_observations = self.stack_observations(self.stacked_observations, new_observation=observation) 183 | 184 | 185 | #print(f"norm_actions: {norm_action}") 186 | tau_X = utils.denormalize(self.config['constraints']['forces']['X']['lower'], self.config['constraints']['forces']['X']['upper'], norm_action[0]) 187 | # print(f"tau_X: {tau_X}") 188 | tau_N = utils.denormalize(self.config['constraints']['forces']['N']['lower'], self.config['constraints']['forces']['N']['upper'], norm_action[1]) 189 | #print(tau_X, tau_N) 190 | tau_X = tau_X 191 | tau_N = tau_N 192 | return tau_X, tau_N 193 | 194 | 195 | if __name__ == '__main__': 196 | # loading the model 197 | from otter.otter import otter 198 | #Tests: 199 | model_name = f"AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-28 09-17-53/best_model.zip" 200 | config_name = f"AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-28 09-17-53/config.yml" 201 | 202 | controller = TargetTrackingAIController( model_name=model_name, config_name=config_name) 203 | test_obs = np.linspace(0,18,18) 204 | #print(test_obs.shape) 205 | #print(controller.stack_observations(observation_stack=controller.stacked_observations, new_observation=test_obs)) 206 | #print(controller.stacked_observations) 207 | #controller.set_up(initial_state=[0, 0, 0, 0, 0, 0], target=[1, 1]) 208 | print(controller.get_controller_action(initial_state=[0, 0, 0, 0, 0, 0], target=[1, 1], sample_time=0.2, t =0)) 209 | print(controller.get_controller_action(initial_state=[0, 0, 0, 0, 0, 0], target=[1, 1], sample_time=0.2, t=1)) 210 | print(controller.get_controller_action(initial_state=[0, 0, 0, 0, 0, 0], target=[1, 1], sample_time=0.2, t=3)) 211 | """ 212 | print(model.policy) 213 | print(model.observation_space) 214 | print(model.n_steps) 215 | print(model.action_noise) 216 | print(model.num_timesteps) 217 | episodes = 5 218 | 219 | 220 | for ep in range(episodes): 221 | obs = env.reset() 222 | done = False 223 | trajectory = np.array([[0],[0], [0]]) 224 | #print(trajectory) 225 | rewards = 0 226 | rew_tot = 0 227 | target = vehicle.target 228 | rew_trajectory = [] 229 | 230 | while not done: 231 | trajectory = env.get_attr('trajectory')[0] # when using vectorized environments it is reset when done, therfore we must fetch the trajectory before the last run 232 | #print(obs) 233 | action, _states = model.predict(obs, deterministic=True ) 234 | #print(f"model prediction returns: {model.predict(obs)}") 235 | #print(f"action: {action}" ) 236 | #print(f"states: {_states}") 237 | #print(vehicle.target) 238 | obs, rewards, done, info = env.step(action) 239 | rew_trajectory.append(rewards) 240 | #env.render() 241 | #print(f"rewards: {rewards}" 242 | # f"done: {done}") 243 | #trajectory = np.append(trajectory, np.reshape(obs[:1, -12:-9][0], (3,1)), axis = 1) 244 | #print(trajectory) 245 | #np.set_printoptions(suppress= True, linewidth= 30000) 246 | rew_tot += rewards 247 | #print(trajectory.shape) 248 | print(f"rew_tot: {rew_tot}") 249 | vehicle = env.get_attr('vehicle')[0] 250 | plt.plot(list(range(0,len(rew_trajectory))), rew_trajectory, label = 'reward history') 251 | plt.title(f"run: {ep}") 252 | plt.show() 253 | #print(trajectory.shape) 254 | utils.plot_trajectory(trajectory = trajectory[:, :-1], target= target ) 255 | plt.show() 256 | """ 257 | -------------------------------------------------------------------------------- /RL_controller/gym/training_env/MarineVehicleTargetTrackingEnv.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | import gym 4 | import numpy as np 5 | from gym import spaces 6 | import random 7 | from matplotlib import pyplot as plt 8 | import utils 9 | from config import config 10 | from otter.otter import otter # modfyed from the original Fossen Vehivle Simulator 11 | from python_vehicle_simulator.lib import gnc # fossen vehicle simulator library 12 | 13 | 14 | class TargetTrackingEnv(gym.Env): 15 | 16 | # static variables are defined upon compiling 17 | 18 | def __init__(self): 19 | super(TargetTrackingEnv, self).__init__() 20 | # Define action and observation space 21 | # They must be gym.spaces objects #the actions are continuous and normalized 22 | 23 | self.cycle_counter = 0 24 | self.target_radius = None 25 | self.within_confidence_radius = False 26 | self.time_within_confidence_radius = 0 27 | self.radius = config['env']['target_confident_region'] # defines the region around the target 28 | self.tota_reward_list = [] 29 | self.time_out = config['env']['time_out'] 30 | self.action_trajectory = None 31 | self.total_training_time = 0 32 | self.n_done_tot = 0 33 | self.psi_d = None 34 | self.action_dot = 0 35 | 36 | self.action = None 37 | self.action_space = spaces.Box(low=np.array([-1, -1], dtype=np.float64), 38 | high=np.array([1, 1], dtype=np.float64), 39 | dtype=np.float64) 40 | # The observations are: 41 | self.observation_space = spaces.Box(low=-np.inf, 42 | high=np.inf, 43 | shape=(18,), 44 | dtype=np.float64) 45 | 46 | self.u_actual = [0, 0] 47 | self.trajectory = np.array([]) 48 | self.eta = None # the actual position of the vehicle in the NED-frame 49 | self.nu = None # the actual velocity of the vehicle in the BODY-frame 50 | self.done = None # record if the training is done or not 51 | self.vehicle = None # the otter in this case (must provide a function to calculate the dynamics in the BODY frame) 52 | self.sample_time = config['sample_time'] 53 | self.sim_length = config['env']['sim_length'] 54 | self.n_steps = range(int(self.sim_length / self.sample_time)) 55 | print(f"sample_time {self.sample_time}") 56 | self.metadata = {"render.modes": ["human", "plot"]} 57 | self.boundaries = config['constraints']['forces'] # boundaries 58 | self.surge_boundaries = self.boundaries['X'] 59 | self.torque_boundaries = self.boundaries['N'] 60 | self.world_box = config['env']['world_size'] 61 | self.previous_actions = [] 62 | self.reward = 0 63 | self.t = 0 64 | self.render_mode = config['env']['render_mode'] 65 | self.debug = config['env']['debug'] 66 | self.initial_target_pos = None 67 | self.target_vel = None 68 | 69 | def __str__(self): 70 | return str(self.__dict__) 71 | 72 | def step(self, action): 73 | """ 74 | Step takes in an action from the Agent between. The value are denormalized by utilizing the ranges defined in the config file. 75 | :param action: 76 | :return: Observations, stop signal and rewards 77 | """ 78 | self.action = action 79 | self.previous_actions.append(action) 80 | 81 | # denormalize action values and calculate truster commands before simulator 82 | # in this architecture the controller learns the Normalized forces in surge and moment among yaw 83 | tau_X = utils.denormalize(self.surge_boundaries['lower'], self.surge_boundaries['upper'], action[0]) 84 | 85 | tau_N = utils.denormalize(self.torque_boundaries['lower'], self.torque_boundaries['upper'], action[1]) 86 | 87 | u_control = np.array(self.vehicle.controlAllocation( 88 | tau_X=tau_X, 89 | tau_N=tau_N), float) # convert forces to control signals (the Agent could learn this directly) 90 | 91 | # this loop is taken from Fossen vehicle simulator: 92 | for t_step in self.n_steps: # by adjusting the values in the config file you can simulate several timesteps with the same controller action. 93 | self.nu, self.u_actual = self.vehicle.dynamics(eta=self.eta, nu=self.nu, 94 | u_actual=self.u_actual, 95 | u_control=u_control, 96 | sampleTime=self.sample_time) # The dynamics is the Fossen- pythonVehicleSimulator 97 | self.eta = gnc.attitudeEuler(self.eta, self.nu, self.sample_time) 98 | 99 | self.t += 1 # increase time counter 100 | self.total_training_time += self.sim_length # increase the total time used in the env 101 | 102 | self.action_dot = (np.linalg.norm(self.previous_actions[-1]) - np.linalg.norm(self.previous_actions[-2])) / len( 103 | self.n_steps) 104 | 105 | self.vehicle.target = utils.simulate_circular_target_motion(initial_position=self.initial_target_pos, 106 | velocity=self.target_vel, 107 | radius=self.target_radius, 108 | sim_time=self.total_training_time) 109 | 110 | self.psi_d = self.get_psi_d() # desired heading towards target 111 | 112 | # set observations 113 | observation = np.array([self.eta[0], # x (N) 114 | self.eta[1], # y (E) 115 | self.eta[5], # psi (angle from north) 116 | self.nu[0], # surge vel. 117 | self.nu[1], # sway vel. 118 | self.nu[5], # yaw vel. 119 | self.vehicle.target[0], # x, target 120 | self.vehicle.target[1], # y, target 121 | self.get_delta_pos_target()[0], # distance between vehicle and target in x 122 | self.get_delta_pos_target()[1], # distance between vehicle and target in y 123 | self.get_euclidean_distance(), # euclidean distance vehicle and target 124 | self.action_dot, 125 | self.get_speed_towards_target(), 126 | self.get_smalles_sign_angle(angle=(self.get_psi_d() - self.eta[5])), 127 | # the angle between the target and the heading of the vessel 128 | self.action[0], 129 | self.action[1], 130 | self.t / self.time_out, # time awareness, 131 | self.radius # aware of the region around the target 132 | ], dtype=np.float64) 133 | self.trajectory = np.append(self.trajectory, np.array([observation]).T, 134 | axis=1) # appends a trajectory of observations 135 | 136 | # get rewards 137 | self.reward, done, euclidean_distance = self.get_reward() 138 | 139 | if self.render_mode == 'human' \ 140 | and abs(self.total_training_time % 10_000) <= self.time_out \ 141 | and self.trajectory.shape[1] > self.time_out: # check if the episode is finished 142 | print("Rendering") 143 | print(f"trajectory shape: {self.trajectory.shape}") 144 | self.render() 145 | 146 | if self.debug: 147 | print(f"observations: {observation} \n" 148 | f"agent action: {action}\n" 149 | f"tau (denormalized action): {[tau_X, tau_N]}\n" 150 | f"control signal: {u_control}" 151 | f"u_actual:{self.u_actual}\n" 152 | f"time: {self.t}\n" 153 | f"psi_d: {self.psi_d}\n" 154 | f"Smallest sign angle: {self.get_smalles_sign_angle(self.get_psi_d(), self.eta[5])}") 155 | 156 | info = {"observations": observation, 157 | "agent_action": action, 158 | "tau": [tau_X, tau_N], 159 | "u_control": u_control, 160 | "u_actual": self.u_actual, 161 | "time": self.t, 162 | "psi_d": self.psi_d, 163 | "Smallest sign angle": self.get_smalles_sign_angle(self.get_psi_d(), self.eta[5])} 164 | return observation, self.reward, done, info 165 | 166 | def reset(self): 167 | 168 | self.previous_actions = [0, 0] 169 | self.action_trajectory = np.zeros((12,)) 170 | self.action = None 171 | self.done = False 172 | self.reward = 0 173 | self.t = 0 174 | self.total_reward_list = [] 175 | # reset all values to initial states 176 | self.eta = [0, 0, 0, 0, 0, 0] 177 | self.nu = [0, 0, 0, 0, 0, 0] 178 | self.u_actual = [0, 0] 179 | self.time_within_confidence_radius = 0 # time within confidence radius 180 | self.target_radius = config['env']['moving_target']['radius'] 181 | self.total_training_time = 0 182 | 183 | if self.cycle_counter > config['env'][ 184 | 'target_cycles'] or self.cycle_counter < 1: # if it is the first tiume or the last time in the cycle 185 | # print(f"cycle_counter: {self.cycle_counter}: {self.cycle_counter > config['env']['target_cycles'] or self.cycle_counter < 1}") 186 | # define new values for moving or fixed target 187 | self.cycle_counter = 0 188 | if config['env']['random_target']: 189 | self.vehicle.target = utils.get_random_target_point( 190 | config['env']['target_spawn_region']) # define new random target 191 | # define new target pos, and velocity 192 | else: 193 | self.vehicle.target = config['env']['fixed_target'] 194 | 195 | self.initial_target_pos = self.vehicle.target # set new target start point 196 | 197 | vel_range = config['env']['moving_target']['velocity'] 198 | self.target_vel = random.uniform(vel_range[0], vel_range[1]) # set new target velocity 199 | 200 | self.cycle_counter += 1 201 | self.psi_d = self.get_psi_d() # get the desired heading 202 | # self.trajectory = np.zeros( 203 | # (self.observation_space.shape[0], 1)) # initialized with zeros so the get speed has an 204 | observation = np.array([self.eta[0], # x (N) 205 | self.eta[1], # y (E) 206 | self.eta[5], # psi (angle from north) heading 207 | self.nu[0], # surge vel. 208 | self.nu[1], # sway vel. 209 | self.nu[5], # yaw vel. 210 | self.vehicle.target[0], # x, target 211 | self.vehicle.target[1], # y, target 212 | self.get_delta_pos_target()[0], # distance between vehicle and target in x 213 | self.get_delta_pos_target()[1], # distance between vehicle and target in y 214 | self.get_euclidean_distance(), 215 | 0, # action_dot 216 | 0, # self.get_speed_towards_target(), 217 | self.get_smalles_sign_angle(angle=(self.get_psi_d() - self.eta[5])), 218 | 0, 219 | 0, 220 | 0, # time 221 | self.radius # radius around target 222 | ], dtype=np.float64) 223 | self.trajectory = np.array(np.array([observation]).T) # initialized with two zeros so that the 224 | 225 | if self.debug: 226 | print("Env reset") 227 | print(f"target: {self.vehicle.target}") 228 | print(f"reset_trajectory:{self.trajectory}") 229 | return observation # reward, done, info can't be included 230 | 231 | def render(self, mode="human"): 232 | # print(self.trajectory) 233 | # print(f"trajectory: {self.trajectory}") 234 | if mode == "human": 235 | utils.plot_trajectory(self.trajectory, target=self.vehicle.target) 236 | 237 | def close(self): 238 | None 239 | 240 | def get_psi_d(self): 241 | """Returns the angle between of the line from the vessel to the target in NED""" 242 | return math.atan2(self.vehicle.target[1] - self.eta[1], self.vehicle.target[0] - self.eta[0]) 243 | 244 | def get_smalles_sign_angle(self, x=None, y=None, angle=None): # page 413. in Fossen 2021 245 | 246 | if angle is not None: 247 | return (angle + math.pi) % (2 * math.pi) - math.pi 248 | if x is not None and y is not None: 249 | a = (x - y) % (2 * np.pi) 250 | b = (y - x) % (2 * np.pi) 251 | return -a if a < b else b 252 | else: 253 | raise ValueError("you must define either x and y, or angle") 254 | 255 | def get_euclidean_distance(self): 256 | return np.linalg.norm(np.array([self.eta[0], self.eta[1]]) - self.vehicle.target, ) 257 | 258 | def get_speed_towards_target(self): 259 | 260 | if self.trajectory.shape[1] > 1: 261 | pos_k2 = np.array([self.trajectory[0, -2], self.trajectory[1, -2]]) 262 | pos_k1 = np.array([self.trajectory[0, -1], self.trajectory[1, -1]]) 263 | return (np.linalg.norm(pos_k2 - self.vehicle.target) - np.linalg.norm( 264 | pos_k1 - self.vehicle.target)) / self.sim_length 265 | else: 266 | return 0 267 | 268 | def get_delta_pos_target(self): 269 | delta_x = abs(self.eta[0] - self.vehicle.target[0]) 270 | delta_y = abs(self.eta[1] - self.vehicle.target[1]) 271 | return delta_x, delta_y 272 | 273 | def get_reward(self): 274 | r = self.radius # objective area around the target/otter 275 | a = 1 # amplitude for the reward function 276 | b = 0 # displacement of bell curve 277 | c = config['env']['target_spawn_region'] / 2 # the with of the bell-curve 278 | c1 = config['env']['c1'] # tuning coefficient for distance reward 279 | c2 = config['env']['c2'] # tuning coefficient for target reached reward 280 | c3 = config['env']['c3'] # tuning coefficient for controller penalty 281 | c4 = config['env']['c4'] # tuning coefficient for time penalty 282 | c5 = config['env']['c5'] # tuning coefficient for time out penalty 283 | c6 = config['env']['c6'] # tuning coefficient for distance change rate 284 | c7 = config['env']['c7'] # tuning coefficient for desired heading error 285 | # initiating the reward and penalty variables 286 | distance_reward = 0 287 | target_reached_reward = 0 288 | controller_action_penalty = 0 289 | time_penalty = 0 290 | total_reward = 0 291 | time_out_penalty = 0 292 | eucledian_distance = self.get_euclidean_distance() # (0.5)*(np.tanh(20 * i+2)-1) 293 | speed_towards_target = c6 * ((np.tanh(3 * self.get_speed_towards_target()))) 294 | heading_reward = -1 + 2 * np.exp( 295 | -((self.get_smalles_sign_angle(angle=(self.get_psi_d() - self.eta[5]))) ** 2 / (2 * 1) ** 2)) 296 | # (1 * np.exp(-((self.get_smalles_sign_angle(angle=(self.get_psi_d() - self.eta[5]))) ** 2 / (2 * 0.2) ** 2))) 297 | 298 | # reward for reducing distance to target 299 | distance_reward = (a * np.exp(-((eucledian_distance - b) ** 2 / ( 300 | c) ** 2))) # gaussian reward function returning a value up reaching the confident area 301 | 302 | # penalties 303 | # for time usage 304 | time_penalty = -1 # -self.t / self.time_out #-(-1 + np.exp((self.t / self.time_out) ** 2)) # exponential function that gives a negative revard as a function of the ratio between time and max time 305 | # controller action penalties 306 | controller_action_penalty = -abs(self.action_dot) 307 | 308 | # check if target is reached and award agent respectively 309 | done = False 310 | 311 | # check for termination states (either agent state) 312 | # check if aget has reach wanted state(Standing relativly calm within the boundary), within the time at hand 313 | 314 | if eucledian_distance < r: 315 | self.within_confidence_radius = True 316 | 317 | else: 318 | self.within_confidence_radius = False 319 | self.time_within_confidence_radius = 0 320 | 321 | if self.within_confidence_radius: # and -0.5 < self.nu[0] < 0.5 and self.nu[1] < 0.5: 322 | target_reached_reward += 1 323 | self.time_within_confidence_radius += 1 324 | if self.time_within_confidence_radius * self.sample_time > 60: 325 | # if the agent has been within the radius of confidence more than 60 seconds the wanted state is reached 326 | print("Reached target for 60 sec!") 327 | self.done = True 328 | done = True 329 | 330 | # check if the time has passed 331 | if self.t >= self.time_out: 332 | self.done = True 333 | done = True 334 | time_out_penalty = -1 335 | print('time out penalty') 336 | 337 | """#Another function 338 | if eucledian_distance < r: 339 | # print("target reached!") 340 | target_reached_reward += 1 341 | target_reached = True 342 | #self.done = True 343 | #done = True 344 | if self.t >= self.time_out and eucledian_distance < r: 345 | # print("time_out!") 346 | self.done = True 347 | done = True""" 348 | 349 | total_reward = c1 * distance_reward \ 350 | + c2 * target_reached_reward \ 351 | + c3 * controller_action_penalty \ 352 | + c4 * time_penalty \ 353 | + c5 * time_out_penalty \ 354 | + c6 * speed_towards_target \ 355 | + c7 * heading_reward 356 | self.total_reward_list = [c1 * distance_reward, 357 | c2 * target_reached_reward, 358 | c3 * controller_action_penalty, 359 | c4 * time_penalty, 360 | c5 * time_out_penalty, 361 | c6 * speed_towards_target, 362 | c7 * heading_reward] 363 | reward = total_reward 364 | if self.debug: 365 | print(f"reward_debug_details:\n" 366 | f"pos: {self.eta[0], self.eta[1]} \n" 367 | f"heading: {self.eta[2]}\n" 368 | f"target at: {self.vehicle.target}\n" 369 | f"Total_reward: {reward} \n" 370 | f"(distance_reward : {c1 * distance_reward}, speed_towards_target_reward: {c6 * speed_towards_target}, " 371 | f"target_reached_reward : {c2 * target_reached_reward},controller_action_penalty: " 372 | f"{c3 * controller_action_penalty}, time_penalty: {c4 * time_penalty}, time_out_penalty:" 373 | f"{c5 * time_out_penalty} " 374 | f":heading_reward: {c7 * heading_reward})" 375 | ) 376 | 377 | return reward, done, eucledian_distance 378 | 379 | 380 | if __name__ == '__main__': 381 | from stable_baselines3.common.env_checker import check_env 382 | 383 | np.set_printoptions(linewidth=2000) 384 | vehicle = otter(0.0, 0.0, 0, 0, 0) 385 | vehicle.target = [50, 50] 386 | env = TargetTrackingEnv() 387 | env.vehicle = vehicle 388 | 389 | print("Checking env, with Stable Baseline 3 env-checker:") 390 | print(check_env(env)) 391 | env.reset() 392 | print() 393 | print("Env initialisation details: ") 394 | print(env) 395 | # print(env.reset()) 396 | # print(env.observation_space) 397 | # It will check your custom environment and output additional warnings if needed 398 | print("Testing environment") 399 | for i in range(0, 1): 400 | env.reset() 401 | env.vehicle.target = [10, 0] 402 | temp_rew = [] 403 | temp_dist = [] 404 | done = False 405 | i = 0 406 | temp_rew_trajectory_list = [] 407 | while not done: 408 | obs, rewards, done, info = env.step([1, 0]) 409 | i = 1 410 | temp_rew.append(rewards) 411 | temp_rew_trajectory_list.append(env.total_reward_list) 412 | temp_dist.append(obs[9]) 413 | # print(f"obs_shape: {obs.shape}") 414 | # env.render() 415 | trajectory = env.trajectory 416 | # target = vehicle.target 417 | print(f"self.initial_target_pos: {env.initial_target_pos}") 418 | print(f"target vel: {env.target_vel}") 419 | print(f"target radius: {env.target_radius}") 420 | print(trajectory[6, :], trajectory[7, :]) 421 | plt.plot(trajectory[6, :], trajectory[7, :], label="target_trajectory") 422 | plt.legend() 423 | plt.show() 424 | fig1 = utils.plot_trajectory(trajectory, target=None) 425 | c1 = config['env']['c1'] # tuning coefficient for distance reward 426 | c2 = config['env']['c2'] # tuning coefficient for target reached reward 427 | c3 = config['env']['c3'] # tuning coefficient for controller penalty 428 | c4 = config['env']['c4'] # tuning coefficient for time penalty 429 | c5 = config['env']['c5'] # tuning coefficient for time out penalty 430 | c6 = config['env']['c6'] # tuning coefficient for distance change rate 431 | c7 = config['env']['c7'] # tuning coefficient for desired heading error 432 | 433 | fig = plt.figure(figsize=[10, 8]) 434 | ax = plt.subplot(111) 435 | cycler = plt.cycler(linestyle=[':', ':', ':', ':', ':', ':', ':', '-'], 436 | color=['black', 'blue', 'brown', 'orange', 'red', 'green', 'purple', 'olive']) 437 | ax.set_prop_cycle(cycler) 438 | lines = ax.plot(list(range(0, len(temp_rew_trajectory_list))), temp_rew_trajectory_list, 439 | label=[f'Euclidean distance reward', 440 | f'Target reached reward', 441 | f'Controller penalty', 442 | f'Intermediate time penalty', 443 | f'Time out penalty', 444 | f'Speed towards target', 445 | f'Heading reward'] 446 | ) 447 | lines = ax.plot(list(range(0, len(temp_rew))), temp_rew, label='Total rewards') 448 | # plt.plot(list(range(0, len(temp_rew))), temp_dist, label='euclidean distance') 449 | 450 | box = ax.get_position() 451 | 452 | ax.set_position([box.x0, box.y0 + box.height * 0.1, 453 | box.width, box.height * 0.9]) 454 | 455 | ax.legend(loc='upper center', bbox_to_anchor=(0.5, -0.05), 456 | fancybox=True, shadow=False, ncol=2, borderaxespad=0.1) 457 | ax.set_title('Reward function') 458 | plt.savefig('rew_func_example.pdf', pad_inches=0.1, bbox_inches='tight') 459 | 460 | plt.show() 461 | 462 | print(f"vehiclen_min/max: {vehicle.n_min}/{vehicle.n_max}") 463 | -------------------------------------------------------------------------------- /config/__init__.py: -------------------------------------------------------------------------------- 1 | import yaml 2 | from pathlib import Path 3 | 4 | path = Path(__file__).parent 5 | 6 | config = yaml.safe_load(open((path / 'config.yml'))) 7 | 8 | __all__ = [config] -------------------------------------------------------------------------------- /config/config.yml: -------------------------------------------------------------------------------- 1 | 2 | #simulation config 3 | sample_time : 0.1 4 | 5 | #Vehicle parameters 6 | constraints: 7 | forces: #(according to SNAME) 8 | X: #surge 9 | upper: 150 10 | lower: -116 11 | N: #torque among yaw 12 | upper: 73 13 | lower: -73 14 | 15 | 16 | #MPC controller parameters 17 | MPC: 18 | debug: True 19 | prediction_horizon: 10 #Prediction for the MPCThmodel 20 | sample_time: 0.1 #teh sampling time for the MPC optimization Casadi 3 DOF model 21 | solver : 'ipopt' 22 | R: [0.05,0 ,0.05] #weight matrix for the controller part of the matrix (sway is set to be zero) 23 | Q: [1000, 1000] #weight matrix for the state-element in the cost function 24 | nlp_options: #tip: https://github-wiki-see.page/m/casadi/casadi/wiki/FAQ%3A-how-to-pass-options-to-solvers#:~:text=Options%20for%20nonlinear%20programming%20solvers%20are%20passed%20in,%3D%20nlpsol%20%28%27solver_name%27%2C%27ipopt%27%2Cnlp%2Coptions%29%20Using%20Opti%20%3A%20opti.solver%20%28%27ipopt%27%2Coptions%29 25 | print_time: False 26 | 27 | options: 28 | ipopt: #ipopt options: https://coin-or.github.io/Ipopt/OPTIONS.html 29 | print_level : 2 # can be between 0-12 and will print different at each solution 30 | max_iter: 10000 31 | linear_solver: 32 | output_file: # 'log.csv' 33 | file_print_level: # 1 34 | print_timing_statistics: # 'yes' 35 | 36 | #AI-controller parameters 37 | env: #invironment parameters 38 | debug: False 39 | render_mode: #'human' #'human' #will render a plot every 1000 epoch (not working as it should) 40 | sim_length: 0.1 # how long to run the simulation between each action during training (seconds) 41 | sample_time: 0.1 # how long integration steps are used in the simulation 42 | observation_space: # used to normalize the observations to the neural network 43 | lower: 0 44 | higher: 600 # represents the 2 x radius range of the positioning system 45 | world_size: [-200,200] #represent min and max in a quadratic box shape (for now this does not 46 | time_out: 1000 # how many for each episode 47 | random_target: False 48 | fixed_target: [20,20] 49 | target_spawn_region: 30 #defines the radius of the region where the target will randomly appear, used in training if the AI 50 | target_confident_region: 1 # radius, defines a confident region around the target, 51 | target_cycles: 100 #how many times the target is appearing in the same spot 52 | moving_target: 53 | pattern: c #None, Linear(l) or Circular(c) 54 | velocity: [0.25,0.25] #randomly 55 | radius: 200 #the path radius if circular movement 56 | 57 | 58 | 59 | #Tuning coefficients for the reward function in the environment 60 | c1: 1 # tuning coefficient for distance to target reward 61 | c2: 1 # tuning coefficient for target reached reward 62 | c3: 1 # tuning coefficient for controller penalty 63 | c4: 1 # tuning coefficient for intermediate time penalty 64 | c5: 1 # tuning parameter for time out penalty 65 | c6: 1 # tuning parameter for distance towards target change rate (speed towards target) 66 | c7: 1 # tuning coefficient for desired heading error 67 | 68 | architecture: [128,128] 69 | n_env: 10 #how many env to run in parallel 70 | n_stacked_frames: 8 # how many stacked frames to use, makes a simple "Memory" for the agent 71 | total_timesteps: 8_000_000 72 | starting_learning_rate: 0.000003 #all up until 27.03.2023 has been 0.0003, but I want to se the affect of using 0.003 in a 128x128 network 73 | model: 'PPO_moving_target_normalized' #prefix of the model 74 | #continue training paramaters 75 | continue_training: False 76 | log_dir: 'AI_controller/logs/PPO_moving_target_normalized/1000_30_5_False_1_0_0_0.1_0_0_1/2023-04-10 22-32-24' 77 | #'AI_controller/logs/PPO_moving_target_transferred_lr/1000_30_1_True_1_2_1_1_0_1/2023-04-07 23-16-45' 78 | #'AI_controller/logs/PPO_moving_target_transferred_lr/1000_30_1_True_1_2_1_1_0_1/2023-04-07 18-10-38' 79 | #'AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-28 09-17-53' 80 | #'AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-26 12-15-55' # relative path for the continued training 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | main.py: Main program for the Python Vehicle Simulator, which can be used 5 | to simulate and test guidance, navigation and control (GNC) systems. 6 | """ 7 | import os 8 | # import webbrowser 9 | # import matplotlib 10 | # matplotlib.use("TkAgg") 11 | # import matplotlib.pyplot as plt 12 | import pandas as pd 13 | # import mpc.casadi_otter_model_3DOF 14 | import simulator 15 | 16 | # from python_vehicle_simulator.vehicles import * 17 | from python_vehicle_simulator.lib import * 18 | 19 | from AI_controller.TargetTrackingAIController import TargetTrackingAIController 20 | from otter import otter 21 | from mpc.casadi_otter_model_3DOF import Casadi3dofOtterModel 22 | from mpc.TargetTrackingMPC import TargetTrackingMPC 23 | import utils 24 | from config import config 25 | from datetime import datetime 26 | import numpy as np 27 | 28 | plt.rcParams.update({ 29 | "font.family": "serif", 30 | 'lines.linewidth': 1}) 31 | 32 | # Simulation parameters: 33 | sampleTime = config['sample_time'] # sample time 34 | N = 8000 # number of samples 35 | 36 | # 3D plot and animation parameters where browser = {firefox,chrome,safari,etc.} 37 | # numDataPoints = 50 # number of 3D data points 38 | # FPS = 10 # frames per second (animated GIF) 39 | # filename = '3D_animation.gif' # data file for animated GIF 40 | # browser = 'safari' # browser for visualization of animated GIF 41 | 42 | ############################################################################### 43 | # Vehicle constructors 44 | ############################################################################### 45 | printSimInfo() 46 | # select 1 for MPC test, or '2' for AI test 47 | no = '1' 48 | match no: # the match statement requires Python >= 3.10 49 | 50 | case '1': 51 | vehicle = otter.otter('TargetTrackingMPC', 0.0, 0.0, 0, 000.0) 52 | otter_6_dof_model = vehicle 53 | otter_3_dof_model = Casadi3dofOtterModel(otter_6_dof_model) # implmentere 3DOF uavhengig av 6DOF 54 | mpc = TargetTrackingMPC(otter_3_dof_model, N=config['MPC']['prediction_horizon'], 55 | sample_time=None) # N is the prediction horizon for the MPC 56 | vehicle.set_target_tracking_mpc(mpc) 57 | case '2': 58 | vehicle = otter.otter('TargetTrackingAI', 0.0, 0.0, 0, 000.0) 59 | 60 | # f"AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-28 09-17-53/best_model.zip" #this is not done training 61 | model_dir = f"AI_controller/logs/PPO_moving_target_normalized/1000_30_1_True_0_2_1_1_0_1/2023-04-09 17-17-17/" 62 | # f"AI_controller/logs/PPO_moving_target_transferred_lr/1000_30_1_True_0_2_1_1_0_1/2023-04-08 18-32-40/" 63 | # f"AI_controller/logs/PPO_moving_target_transferred_lr/1000_30_1_True_0_2_1_1_0_1/2023-04-08 17-18-52/" 64 | # f"AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-28 09-17-53/" 65 | # f"AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-27 08-03-59/"#this is slow 66 | model_name = model_dir + f"best_model.zip" 67 | config_name = model_dir + f"config.yml" 68 | # f"AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-27 08-03-59/config.yml" 69 | # f"AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-28 09-17-53/config.yml" #this is not converged 70 | ai_controller = TargetTrackingAIController(model_name=model_name, config_name=config_name) 71 | vehicle.set_target_tracking_ai_controller(ai_controller=ai_controller) 72 | case '5': 73 | vehicle = otter.otter('manual', 0.0, 0.0, 0, 000.0) 74 | print('manual_controls') 75 | printVehicleinfo(vehicle, sampleTime, N) 76 | 77 | 78 | ############################################################################### 79 | # Main simulation loop 80 | ############################################################################### 81 | def main(): 82 | # add the functionality to stop the simulation while running 83 | break_program = False 84 | target = [20,20] 85 | vehicle.target = target 86 | 87 | result_dir = os.path.join('results', vehicle.controlMode, 88 | datetime.now().strftime('%Y-%m-%d %H-%M-%S')) # create a log dir for the results 89 | if not os.path.exists(result_dir): 90 | os.makedirs(result_dir) 91 | utils.copy_config(result_dir) # copy the config file for documentation 92 | 93 | def on_press(key): 94 | if key == keyboard.Key.end: 95 | # print('end pressed') 96 | break_program = True 97 | return False 98 | 99 | with keyboard.Listener(on_press=on_press) as listener: 100 | while not break_program: 101 | print('Running simulation...') 102 | simTime, simData, target_trajectory, tau_trajectory, time_compute_controller_signals = simulator.simulate(N, sampleTime, vehicle) 103 | print(("sim done")) 104 | break_program = True # stop sim 105 | listener.stop() 106 | listener.join() 107 | 108 | # save the data in teh result (for future treatment of plots and research) 109 | np.savetxt(fname=result_dir + '/sim_time.csv', X=simTime, delimiter=",", fmt="%f") 110 | np.savetxt(fname=result_dir + '/simData.csv', X=simData, delimiter=",", fmt="%f") 111 | np.savetxt(fname=result_dir + '/target_trajectory.csv', X=target_trajectory, delimiter=",", fmt="%f") 112 | np.savetxt(fname=result_dir + '/time_controller_computation.csv', X=time_compute_controller_signals, delimiter=",", fmt="%f") 113 | np.savetxt(fname=result_dir + '/tau_trajectory.csv', X=tau_trajectory, delimiter=",", fmt="%f") 114 | 115 | # plot target and vehicle trajectory 116 | pos_3dof = simData[:, [0, 1, 5, 6, 7, 11]] 117 | utils.plot_trajectory(trajectory=pos_3dof.T, target=target_trajectory.T, 118 | file_path=f'{result_dir}/NED.pdf') 119 | 120 | # plot error plot 121 | utils.plot_error(simData[:, [0, 1]], pos_target=target_trajectory, path=f'{result_dir}/error.pdf', 122 | sample_time=sampleTime) 123 | 124 | # plot the controller signals 125 | utils.plot_controls(u_control=simData[:, [12, 13]], 126 | u_actual=simData[:, [14, 15]], 127 | sample_time=sampleTime, 128 | file_path=f'{result_dir}/control_rpms.pdf') 129 | 130 | utils.plot_veloceties(vel_matrix_3DOF=simData[:, [6,7,11]].T, 131 | action_matrix=simData[:, [12, 13]].T, 132 | sample_time=sampleTime, 133 | file_path=f'{result_dir}/velocities.pdf') 134 | 135 | utils.plot_control_forces(tau_trajectory[[0,2],:], sample_time=sampleTime, file_path=f'{result_dir}/control_forces.pdf') 136 | plt.show() 137 | 138 | utils.plot_solv_time(solv_time_data=time_compute_controller_signals, sample_time=sampleTime, file_path=f'{result_dir}/control_compute_time.pdf') 139 | plt.show() 140 | main() 141 | -------------------------------------------------------------------------------- /mpc/TargetTrackingMPC.py: -------------------------------------------------------------------------------- 1 | import casadi as ca 2 | import matplotlib.pyplot as plt 3 | 4 | from config import config 5 | from mpc import casadi_otter_model_3DOF 6 | import numpy as np 7 | 8 | 9 | class TargetTrackingMPC: 10 | 11 | def __init__(self, model, N, sample_time=None, solver=None): 12 | """ 13 | This is the MPC class for the otter takes the model from casadi_otter_model_3DOF 14 | The model must provide: 15 | ode = casadi_otter_model_3DOF.ode 16 | x = Casadi symobl defined in the state derivative in the ode 17 | u = Control inputs (must present in the ODE) 18 | inspiration: https://web.casadi.org/blog/ocp/ 19 | """ 20 | self.model = model 21 | self.p = None 22 | self.latest_solv = None 23 | self.ode = model.ode 24 | self.x = model.x 25 | self.u = model.u 26 | self.current_target = None # holds the trajectory of the target, relative to the NED-frame 27 | self.previous_target = None 28 | self.solv = None 29 | self.opt_response = None 30 | self.opt_controls = None 31 | self.solv_target = None 32 | self.target = None 33 | self.N = N 34 | 35 | if not sample_time: 36 | self.sample_time = config['MPC']['sample_time'] 37 | else: 38 | self.sample_time = sample_time 39 | 40 | self.solver = solver 41 | self.solver_options = None 42 | 43 | self.opti = self.__initiate_mpc() 44 | 45 | self.set_solver() 46 | print(f"solver and options set to: {self.get_solver_options()}") 47 | # initiate the MPC 48 | 49 | def set_target(self, target): 50 | self.opti.set_value(self.target, target) 51 | 52 | def set_solver(self, solver: str): 53 | self.solver = solver 54 | 55 | def __initiate_mpc(self): 56 | 57 | # define local variables_ 58 | ode = self.ode 59 | N = self.N 60 | sample_time = self.sample_time 61 | 62 | # Define integrator for #next_state = x_next 63 | # Integrator definitions and options 64 | integrator_options = {'tf': sample_time, 65 | 'simplify': True, 66 | 'number_of_finite_elements': 4 67 | } 68 | solver = 'rk' # ipopt' #runge kutta 4 69 | 70 | # DAE problem structure 71 | f = ca.Function('f_1', [self.x, self.u], [ode], ['x', 'u'], ['xdot']) 72 | dae = {'x': self.x, 'p': self.u, 'ode': f(self.x, self.u)} 73 | 74 | # oae = ca.Function('ode', [x, u], ode, ['x','u'], ['eta_dot', 'nu_r_dot'] ) 75 | # defining the next state via runge kutta method, provided by Casadi. 76 | intg = ca.integrator('intg', 'rk', dae, integrator_options) 77 | 78 | x_next = intg(x0=self.x, p=self.u)['xf'] 79 | # print(x_next) 80 | # print(f"x_next= {ca.evalf(intg(x0=x_0, p=u_0)['xf'])}") 81 | 82 | F = ca.Function('F', [self.x, self.u], [x_next]) # function for the next state 83 | 84 | # define optimization environment: 85 | opti = ca.Opti() 86 | self.opti = opti 87 | 88 | # Define optimization paramaters: 89 | """Q = ca.MX(ca.vertcat(ca.horzcat(1, 0), 90 | ca.horzcat(0, 1))) 91 | R = ca.MX(ca.vertcat(ca.horzcat(1, 1), 92 | ca.horzcat(0, 1)))""" 93 | # cf= # cost function 94 | 95 | x = opti.variable(6, N + 1) # we only return one state-matrix that contains eta, and nu, after discretization 96 | u = opti.variable(3, N + 1) # control variable 97 | p = opti.parameter(6) # used to fix the first state 98 | target = opti.parameter(2) 99 | Q = opti.parameter(2, 2) 100 | R = opti.parameter(3, 3) 101 | I = opti.parameter(3,3) 102 | cf = 0 103 | # define cost function 104 | # for k in range(N): #multiple shooting method 105 | # # opti.subject_to(x[:,k+1] == F(x[:, k], u[:, k])) 106 | # cf = (((x[:2, k] - target).T @ Q @ (x[:2, k] - target)) + \ 107 | # (u[:, k].T @ R @ u[:, k])) 108 | # cf = (ca.sumsqr((x[:2, :] - target).T @ Q @ (x[:2, :] - target)) + (ca.sumsqr(u.T @ R @ u))) 109 | 110 | 111 | for k in range(N): 112 | opti.subject_to(x[:, k + 1] == F(x[:, k], u[:, k])) #to repect the dynamics of the system 113 | cf = cf +((x[:2, k] - target).T @ Q @ (x[:2, k] - target) + ((u[:, k]-u[:, k-1]).T @ I @ (u[:, k]-u[:, k+1]))+(u[:, k]).T @ R @ (u[:, k])) 114 | #cf = ((x[:2, k] - target).T @ Q @ (x[:2, k] - target) + ((u[:, k] - u[:, k - 1]).T @ I @ (u[:, k] - u[:, k - 1])) + ca.eig((u[:, k]).T @ R)) 115 | #cf = ((ca.norm_2((x[:2, k] - target).T @ Q)) + (ca.norm_2((u[:, k]).T @ R))) 116 | # cf = (((x[:2, k] - target).T @ Q @ (x[:2, k] - target)) + ((u[:, k] -u[:, k+1]).T @ R @ (u[:, k] -u[:, k+1]))) #delta u 117 | 118 | cost_function = ca.Function('cost_function', [x, u, target,R,Q], [cf]) 119 | 120 | # create an optimization environment with a Multiple shooting discretization, 121 | # that means that the whole state and control trajectory is the decition variables 122 | 123 | opti.minimize(cost_function(x, u, target, R, Q)) # objective function this is primarily the distance between the state and the reference (target) 124 | # opti.subject_to(x == F(x, u)) 125 | 126 | opti.subject_to(opti.bounded(-150, u[0, :], 150)) # surge lower and upper bound 127 | opti.subject_to(u[1, :] == 0) # yaw controls == 0 always 128 | opti.subject_to(opti.bounded(-80, u[2, :], 80)) # max 50 Nm torque among yaw 129 | opti.subject_to(x[:, 0] == p) # initial states must always be the initial states 130 | opti.set_value(Q, np.diag(config['MPC']['Q'])) 131 | opti.set_value(R, np.diag(config['MPC']['R'])) 132 | opti.set_value(I, np.diag([1,1,1])) 133 | # solver_options = {"ipopt": {"max_iter": 10, "print_level": 5}} 134 | 135 | 136 | 137 | self.p = p 138 | self.x = x 139 | self.u = u 140 | self.target = target 141 | 142 | return opti 143 | 144 | def get_solver_options(self): 145 | options = config['MPC']['nlp_options'] 146 | options[config['MPC']['solver']] = config['MPC']['options']['ipopt'] 147 | return options 148 | 149 | def set_solver(self): 150 | self.solver = config['MPC']['solver'] 151 | options = self.get_solver_options() 152 | if options: 153 | self.opti.solver(self.solver, self.get_solver_options()) 154 | else: 155 | self.opti.solver(self.solver) 156 | 157 | def solve_optimal_control(self, initial_state, target): 158 | 159 | if target: 160 | self.opti.set_value(self.target, target) 161 | if config['MPC']['debug']: 162 | self.opti.debug.value 163 | #print(f"target: {target}") 164 | self.opti.set_value(self.p, initial_state) 165 | # self.opti.set_initial(self.x, self.x) 166 | if self.latest_solv is not None: 167 | #self.opti.set_initial(self.x, self.latest_solv.value(self.x)) 168 | self.opti.set_initial(self.u, self.latest_solv.value(self.u)) 169 | else: 170 | self.opti.set_initial(self.u, 0) 171 | solv = self.opti.solve() 172 | #print(solv.stats()) 173 | #print(solv) 174 | self.latest_solv = solv 175 | 176 | 177 | return solv.value(self.u)[:, 0] # only returns the first in the trajectory 178 | 179 | def write_latest_results(self): 180 | opt_controls = self.latest_solv.value(self.u) # collects the numerical values from the solve-object 181 | opt_response = self.latest_solv.value(self.x) 182 | solv_target = self.latest_solv.value(self.target) 183 | print(f"sim time: {self.N * self.sample_time} sec") 184 | n = int(len(opt_controls[0]) * 1) 185 | x = list(range(0, n + 1)) 186 | fig_1 = plt.plot(opt_response[0, :n], opt_response[1, :n], label='x,y (NED)') 187 | fig_1 = plt.plot(opt_response[0][0], opt_response[1][0], label='Start', marker='o') 188 | fig_1 = plt.plot(solv_target[0], solv_target[1], label='Target', marker='x') 189 | plt.legend() 190 | plt.show() 191 | 192 | fig_2, (ax1, ax2, ax3) = plt.subplots(nrows=3, ncols=1) 193 | ax1.stairs(values=opt_controls[0], edges=x, label='Surge (N)') 194 | ax1.legend() 195 | ax2.stairs(values=opt_controls[1], edges=x, label='Sway (N)') 196 | ax2.legend() 197 | ax3.stairs(values=opt_controls[2], edges=x, label='Yaw (Nm)') 198 | ax3.legend() 199 | fig_2.show() 200 | 201 | def __str__(self) -> str: 202 | return f"solver: {self.solver}" \ 203 | f"Prediction horizon: {self.N}" \ 204 | f"sampleing time : {self.sample_time}" 205 | 206 | 207 | if __name__ == '__main__': 208 | """ 209 | Example of usage: 210 | 211 | """ 212 | # collecting the state, control and ode definitions from the 3DOF-model 213 | import otter.otter as otter 214 | 215 | fossen_6_dof_model = otter.otter(0, 0, 0, 216 | 0) # Now the 6DOF model is used to extract the M-matrix, the D matrix and other constants. 217 | 218 | # TODO implement a clean variant of Otter_model_3DOF, where otter 6DOF is not used. 219 | model = casadi_otter_model_3DOF.Casadi3dofOtterModel(fossen_6_dof_otter_model=fossen_6_dof_model) 220 | 221 | MPC = TargetTrackingMPC(model=model, N=100, sample_time=0.2) 222 | # MPC.set_target(n) 223 | control_force = MPC.solve_optimal_control(initial_state=[0, 0, 0, 0, 0, 0], target=[10, 10]) 224 | # print(control_force[:, 0]) 225 | MPC.write_latest_results() 226 | -------------------------------------------------------------------------------- /mpc/casadi_otter_model_3DOF.py: -------------------------------------------------------------------------------- 1 | """ 2 | This is the 3-DOF Otter Model implemented in the Casadi-framework. 3 | The matrix coefficients are collected directly from the Otter by Fossen 2021 4 | """ 5 | #TODO: remove the vehicle as an istance variable, and implement the matrecies directly in the model 6 | # TODO Clean the casadi_otteR_model_3DOF file, to only contin the nessecary details for the 3DOF-model. 7 | import casadi as ca 8 | from casadi import * 9 | import numpy as np 10 | #import otter.otter as otter 11 | #from python_vehicle_simulator.lib import gnc 12 | #from casadi import tools as cat 13 | #from python_vehicle_simulator.lib import gnc # some functions from Fossen 14 | #import matplotlib 15 | #from matplotlib import pyplot as plt 16 | import utils 17 | 18 | 19 | # otter = otter.otter(0, 0, 0, 0) 20 | 21 | class Casadi3dofOtterModel: 22 | def __init__(self, fossen_6_dof_otter_model): 23 | otter = fossen_6_dof_otter_model 24 | self.ode = None 25 | self.x = None 26 | self.u = None 27 | self.f = None 28 | 29 | # States 30 | # x1 = MX.sym('x1',3,1) # [x,y,psi] position 31 | # x2 = MX.sym('x2',3,1) # v velocity vector 32 | # x =[x1,x2] 33 | x = MX.sym('x', 6, 1) # [x1, x2] # state vector where x = [[x,y,psi].T , [u,v,r].T].T 34 | 35 | # controls 36 | u = MX.sym('u', 3, 1) # Controls of the otter this is [t1+t2,0,-l1*t1-l2*t2] 37 | 38 | # symbolic representations 39 | # Rpsi = MX.sym('R(psi)', 3, 3) # rotation matrix from BODY to NED (north-east-down) 40 | # p1 = MX.sym('p1', 1, 6) # elimination matrix from the to extract yaw-rate 41 | # p2 = MX.sym('p2', 6, 3) # elimination matrix to extract velocity matrix 42 | # M = MX.sym('M', 3,3) # mass matrix 43 | # Minv = MX.sym('Minv', 3, 3) # inverse mass matrix M^-1 44 | # MRB = MX.sym('MRB', 3,3) # rigid body mass 45 | # MA = MX.sym('MA',3,3) # Mass matrix added mass 46 | # D = num.sym('D', 3, 3) # the Otter Damping matrix 47 | 48 | psi = MX.sym('psi') # heading angle 49 | m_tot = MX.sym('m_tot') # total mass of otter 50 | SO3 = MX.sym('SO3', 3, 3) # 51 | r = MX.sym('r') # angular velocity yaw = x[5] 52 | g_x = MX.sym('g_x') # vector from the CO to CG chap. 3.3 in Fossen 2021 53 | Xh = SX.sym('Xh') 54 | Yh = SX.sym('Yh') 55 | Nh = SX.sym('Nh') 56 | Xu = SX.sym('Xh') 57 | Yv = SX.sym('Yv') 58 | Nr = SX.sym('Nr') 59 | nu_c = MX.sym('nu_c', 6, 1) # symbol for currents 60 | nu_r = MX.sym('nu_r', 6, 1) # relative velocity taking currents into considerations 61 | 62 | # Define constants: 63 | m_tot = otter.m_total 64 | MA = ca.MX(utils.convert_6DOFto3DOF(otter.MA)) 65 | # print(f"MA:{MA}") 66 | MRB = ca.MX(utils.convert_6DOFto3DOF(otter.MRB)) 67 | M = MA + MRB 68 | D = ca.MX(utils.convert_6DOFto3DOF(otter.D)) # "dette er "negativ D 69 | # print(f"D: {D}") 70 | Minv = ca.MX(np.linalg.inv(utils.convert_6DOFto3DOF(otter.M))) 71 | # nu_r = x - nu_c 72 | G = ca.MX((otter.G)) # we dont need this, due to the restoring forces only act in Have, roll and pitch 73 | g_x = otter.rg[0] # x-component in the g-matrix 74 | # print(f"otter rg[]0: {g_x}") 75 | # functions 76 | CRB_v = MX.sym('C', 3, 3) # Rigid body Corlious and centripetal matrix (absolut velocity in the body) 77 | CA_vr = MX.sym('CA_vr', 3, 3) # added Coriolis and Centripetal matrix (relative velocity, due to currents) 78 | D_vr = MX.sym('D_vr', 3, 3) # Linear surge damping + nonlinear yaw-damping 79 | 80 | # p1 = np.array([0, 0, 1, 0, 0, 0, 0]) 81 | # p2 = np.array([[0, 0, 0, 1, 0, 0], 82 | # [0, 0, 0, 0, 1, 0], 83 | # [0, 0, 0, 0, 0, 1]]) 84 | 85 | # Rotation Matrix function from BODY to NED 86 | b2ned = ca.vertcat(ca.horzcat(ca.cos(psi), -ca.sin(psi), 0), 87 | ca.horzcat(ca.sin(psi), ca.cos(psi), 0), 88 | ca.horzcat(0, 0, 1)) 89 | 90 | # define function for R(psi) 91 | Rpsi = Function('R_psi', [psi], [b2ned]) 92 | 93 | """ 94 | Function to calculate rigid body and added mass Coriolis and centripetal matrices 95 | x[5] = yaw-rate and is the sith element in the state matrix 96 | CRB_v is - Quality controlled twoards eq. 6.99 in Fossen 97 | """ 98 | CRB_v_matrix = ca.vertcat(ca.horzcat(0, -m_tot * x[5], -m_tot * g_x * x[5]), 99 | ca.horzcat(m_tot * x[5], 0, 0), 100 | ca.horzcat(m_tot * g_x * x[5], 0, 0)) 101 | # CRB_v_matrix = Rpsi(x[2]).T @ CRB_v_matrix @ Rpsi(x[2]) #rotation about 102 | # defining function to calculate the CRB matrix 103 | CRB_v = Function('CRB_v', [x], [CRB_v_matrix]) 104 | 105 | """ 106 | CRB alternative from fossen: 107 | CRB_CG = np.zeros((6, 6)) 108 | CRB_CG[0:3, 0:3] = self.m_total * Smtrx(nu[3:6]) 109 | CRB_CG[3:6, 3:6] = -Smtrx(np.matmul(self.Ig, nu[3:6])) 110 | CRB = self.H_rg.T @ CRB_CG @ self.H_rg # transform CRB from CG to CO(Body) 111 | 112 | CRB_CG = np.zeros((6, 6)) 113 | CRB_CG[0:3, 0:3] = otter.m_total * gnc.Smtrx([0,0,x[5]]) 114 | CRB_CG[3:6, 3:6] = -gnc.Smtrx(np.matmul(otter.Ig, [0,0,x[5]])) 115 | CRB = otter.H_rg.T @ CRB_CG @ otter.H_rg # transform CRB from CG to CO(Body) 116 | CRB_v2 = Function('CRB_v', [x], [CRB]) 117 | 118 | """ 119 | 120 | # Matematically expression to calculate added mass Coriolis and centripetal matrices 121 | # 3-DOF model (surge, sway and yaw) 122 | # C = np.zeros( (3,3) ) 123 | # C[0,2] = -M[1,1] * nu[1] - M[1,2] * nu[2] 124 | # C[1,2] = M[0,0] * nu[0] 125 | 126 | """ 127 | CA_vr assumes that the Munk moment (CA_vr[2,0]) in yaw can be neglected and that CA[2,1] is zero (due to nonlinear-damping) 128 | x = [x (x-coord),y (y-coor),psi(heading),u(surge), v(sway), r (yaw rate)] 129 | CA_vr is - Quality controlled twoards eq. 6.99 in Fossen 130 | MA = [Xudot(0,0) 131 | Yvdot(1,1) 132 | Zwdot(2,2) 133 | Kpdot(3,3) 134 | Mqdot(4,4) 135 | Nrdot (5,5)] 136 | 137 | CA_vr_mtrx = ca.vertcat(ca.horzcat(0, 0, MA[1, 1] * x[4] + MA[1, 2] * x[5]), 138 | ca.horzcat(0, 0, - 0 * MA[0, 0] * x[3]), 139 | ca.horzcat((-MA[1, 1] * x[4] - MA[1, 2] * x[5]), (MA[0, 0] * x[3]), 0))""" 140 | 141 | CA_vr_mtrx = utils.m2c(MA, x[3:]) 142 | CA_vr_mtrx[2, 0] = 0 # neglecting Munk moment 143 | CA_vr_mtrx[2, 1] = 0 # negleting som damping stuff tha need better damping terms in order to be used 144 | CA_vr = Function('CA_vr', [x], [CA_vr_mtrx]) 145 | 146 | # Hydrodynamic linear(surge) damping + nonlinear yaw damping tau_damo[2,2] 147 | tau_damp = ca.mtimes(D, x[3:]) 148 | tau_damp[2] = (D[2, 2] * x[5]) + (10 * D[2, 2] * ca.fabs(x[5]) * x[5]) # Same expression as in the Otter script 149 | """ 150 | tau_damp = -np.matmul(self.D, nu_r) 151 | tau_damp[5] = tau_damp[5] - 10 * self.D[5, 5] * abs(nu_r[5]) * nu_r[5] 152 | """ 153 | 154 | # print(tau_damp) 155 | Dv = Function('Dn_vr', [x], [tau_damp]) 156 | 157 | """#Def symbols 158 | Yh = ca.SX.sym('Yh') 159 | Nh = ca.SX.sym('Nh') 160 | xL = ca.SX.sym('xL') 161 | Ucf = ca.SX.sym(('Ucf')) 162 | """ 163 | 164 | # Cross-flow drag 165 | tau_cf_x = ca.Function('tau_cf', [x], [utils.crossFlowDrags3DOF(otter.L, otter.B, otter.T, x)]) 166 | 167 | # define input variables: 168 | eta_0 = ca.SX([0, 0, 0]) # starter i x_n = 1, y_n = 1 psi = 0 (heading rett nord) 169 | x_0 = ca.MX([0, 0, 0, 0, 0, 0]) # initial states 170 | u_0 = ca.MX([0, 0, 0]) # initial controls 171 | nu_c_in = ca.MX([0, 0, 0, 0, 0, 0]) # velocity of current 172 | x_r = x - nu_c # relative velocity vector 173 | Dv_val = ca.evalf(Dv(x_0)) 174 | # print("Checking the ode equations") 175 | # print(f"val_MA: {MA}, type: {type(MA)}") 176 | # print(f"val_MRB: {MRB}, type: {type(MRB)}") 177 | # print(f"val_M: {M}, type: {type(M)}") 178 | # print(f"val_D: {D}, type: {type(D)}") 179 | # print(f"val_G: {G}, type: {type(G)}") 180 | # print(f"val_D(v): {ca.evalf(Dv([0, 0, 0, 10, 10, 10]))}") 181 | # print(f"CRB([0,0,0,0,0,0]): {ca.evalf(CRB_v([0, 0, 0, 0, 0, 0]))}") 182 | # print(f"CA(v): {ca.evalf(CA_vr(x_0))}") 183 | # print(f"C: {ca.evalf(CRB_v(x_0) + CA_vr(x_0))}") 184 | 185 | # Forward time integration method -> Integrator to discretize the system using casadi integrators 186 | # x_dot --> x(k+1) 187 | 188 | sampleTime = 0.02 189 | N = 10000 190 | simTime = N * sampleTime 191 | integrator_options = {'tf': sampleTime, 192 | 'simplify': True, 193 | 'number_of_finite_elements': 4 194 | } 195 | solver = 'rk' # ipopt' 196 | 197 | # DAE problem structure 198 | # x = MX.sym('x') 199 | # u = MX.sym('u') 200 | p = MX.sym('p') 201 | 202 | damp_sway = ca.Function('damp_sway', [x], [ca.vertcat(0, -x[4] * 10, 0)]) 203 | 204 | ode = ca.vertcat((Rpsi(x[2])) @ (x[3:]), 205 | (Minv @ (u - (CRB_v(x) - CA_vr(x)) @ x[3:] - Dv(x) - tau_cf_x(x)))) # tau_cf_cx = crossflow 206 | 207 | f = ca.Function('f_1', [x, u], [ode], ['x', 'u'], ['xdot']) 208 | 209 | self.ode = ode 210 | self.x = x 211 | self.u = u 212 | self.f = f -------------------------------------------------------------------------------- /mpc/casadi_sim.py: -------------------------------------------------------------------------------- 1 | import casadi_otter_model_3DOF 2 | import casadi as ca 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | """ 6 | This is a simulator using the 3DOF model and runge kutta integrator to obtain the position in NED and BODY velocities 7 | """ 8 | 9 | # Forward time integration method -> Integrator to discretize the system using casadi integrators 10 | # x_dot --> x(k+1) 11 | 12 | class OtterSimulator: 13 | def __init__(self, model, N = None, sample_time = None): 14 | self.x = model.x 15 | self.u = model.u 16 | self.ode = model.ode 17 | self.trajectory = None 18 | self.N = N 19 | self.sample_time = sample_time 20 | 21 | def simulate(self, x_initial, u_controls, N, sample_time): 22 | if not sample_time: 23 | sample_time = self.sample_time 24 | if not N: 25 | N = self.N 26 | 27 | x = self.x 28 | u = self.u 29 | ode = self.ode 30 | 31 | #collecting the state, control and ode definitions from the 3DOF-model 32 | 33 | #Integrator definitions and options 34 | simTime = N * sample_time 35 | integrator_options = {'tf': sample_time, 36 | 'simplify': True, 37 | 'number_of_finite_elements': 4 38 | } 39 | solver = 'rk' # ipopt' #runge kutta 4 40 | 41 | # DAE problem structure 42 | f = ca.Function('f_1', [x, u], [ode], ['x', 'u'], ['xdot']) 43 | dae = {'x': x, 'p': u, 'ode': f(x, u)} 44 | 45 | 46 | # oae = ca.Function('ode', [x, u], ode, ['x','u'], ['eta_dot', 'nu_r_dot'] ) 47 | # defining the next state via runge kutta method, provided by Casadi. 48 | intg = ca.integrator('intg', 'rk', dae, integrator_options) 49 | 50 | x_next = intg(x0=x, p=u)['xf'] 51 | # print(x_next) 52 | # print(f"x_next= {ca.evalf(intg(x0=x_0, p=u_0)['xf'])}") 53 | 54 | 55 | F = ca.Function('F', [x, u], [x_next]) 56 | # print(ca.evalf(F(x_0,u_0))) 57 | 58 | sim = F.mapaccum(N) 59 | 60 | res = sim(x_initial, u_controls) 61 | 62 | res_arr = np.array(ca.evalf(res)) #resulting position and velocities over simulation horizon 63 | return res_arr 64 | 65 | def write_sim_results(self, res_arr): 66 | 67 | #print(res_arr.shape) 68 | #print(f"sim time: {simTime} sec") 69 | n = int(len(res_arr[0]) * 1) 70 | x = list(range(0, n)) 71 | 72 | fig_1 = plt.plot(res_arr[0, :n], res_arr[1, :n], label='x,y (NED)') 73 | fig_1 = plt.plot(res_arr[0][0], res_arr[1][0], label='Start', marker='o') 74 | plt.legend() 75 | plt.show() 76 | 77 | # fig_2 = plt.plot(x, res_arr[2, :n], label='yaw (r)') 78 | fig_2 = plt.plot(x, res_arr[3, :n], label='surge (m/s)') 79 | fig_2 = plt.plot(x, res_arr[4, :n], label='sway (m/s)') 80 | fig_2 = plt.plot(x, res_arr[5, :n], label='yaw rate (r/s)') 81 | plt.legend() 82 | plt.show() 83 | 84 | if __name__ == '__main__': 85 | N = 1000 86 | sample_time = 0.02 87 | x_initial = [0, 0, 0, 0, 0, 0] # [x,y,psi,surge,sway,yaw] 88 | u_controls = [200, 0, 10] # [force surge, force sway(always zero), torque yaw] 89 | import otter.otter as otter 90 | fossen_6_dof_model = otter.otter(0, 0, 0, 0) # Now the 6DOF model is used to extract the M-matrix, the D matrix and other constants. 91 | #TODO implement a clean variant of Otter_model_3DOF, where otter 6DOF is not used. 92 | model = casadi_otter_model_3DOF.Casadi3dofOtterModel(fossen_6_dof_otter_model=fossen_6_dof_model) 93 | 94 | sim = OtterSimulator(model = model, N=N, sample_time=sample_time) 95 | res_arr = sim.simulate(x_initial=x_initial, u_controls=u_controls, N=N, sample_time= sample_time) 96 | 97 | sim.write_sim_results(res_arr=res_arr) -------------------------------------------------------------------------------- /otter/otter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | This file is based on the file from Fossen Vehicle Simulator but is modified for the purpose of using it in this project. 5 | 6 | Original text: 7 | otter.py: 8 | Class for the Maritime Robotics Otter USV, www.maritimerobotics.com. 9 | The length of the USV is L = 2.0 m. The constructors are: 10 | 11 | otter() 12 | Step inputs for propeller revolutions n1 and n2 13 | 14 | otter('headingAutopilot',psi_d,V_current,beta_current,tau_X) 15 | Heading autopilot with options: 16 | psi_d: desired yaw angle (deg) 17 | V_current: current speed (m/s) 18 | beta_c: current direction (deg) 19 | tau_X: surge force, pilot input (N) 20 | 21 | Methods: 22 | 23 | [nu,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) returns 24 | nu[k+1] and u_actual[k+1] using Euler's method. The control inputs are: 25 | 26 | u_control = [ n1 n2 ]' where 27 | n1: propeller shaft speed, left (rad/s) 28 | n2: propeller shaft speed, right (rad/s) 29 | 30 | u = headingAutopilot(eta,nu,sampleTime) 31 | PID controller for automatic heading control based on pole placement. 32 | 33 | u = stepInput(t) generates propeller step inputs. 34 | 35 | [n1, n2] = controlAllocation(tau_X, tau_N) 36 | Control allocation algorithm. 37 | 38 | References: 39 | T. I. Fossen (2021). Handbook of Marine Craft Hydrodynamics and Motion 40 | Control. 2nd. Edition, Wiley. 41 | URL: www.fossen.biz/wiley 42 | 43 | Author: Thor I. Fossen 44 | """ 45 | import numpy as np 46 | import math 47 | from python_vehicle_simulator.lib.control import PIDpolePlacement 48 | from python_vehicle_simulator.lib.gnc import Smtrx, Hmtrx, m2c, crossFlowDrag, sat 49 | import mpc.TargetTrackingMPC as mpc 50 | from config import config 51 | import utils 52 | 53 | 54 | # Class Vehicle 55 | class otter: 56 | """ 57 | otter() Propeller step inputs 58 | otter('headingAutopilot',psi_d,V_c,beta_c,tau_X) Heading autopilot 59 | 60 | Inputs: 61 | psi_d: desired heading angle (deg)desired heading angle (deg) 62 | V_c: current speed (m/s) 63 | beta_c: current direction (deg) 64 | tau_X: surge force, pilot input (N) 65 | """ 66 | 67 | def __init__( 68 | self, 69 | controlSystem="stepInput", 70 | r=0, 71 | V_current=0, 72 | beta_current=0, 73 | tau_X=120 74 | 75 | 76 | ): 77 | self.trajectory = None 78 | self.eta = None 79 | self.target = config['env']['fixed_target'] 80 | self.sample_time = config['sample_time'] 81 | self.sim_length = config['env']['sim_length'] 82 | # Constants 83 | D2R = math.pi / 180 # deg2rad 84 | g = 9.81 # acceleration of gravity (m/s^2) 85 | rho = 1026 # density of water (kg/m^3) 86 | 87 | if controlSystem == "headingAutopilot": 88 | self.controlDescription = ( 89 | "Heading autopilot, psi_d = " 90 | + str(r) 91 | + " deg" 92 | ) 93 | elif controlSystem == "TargetTrackingMPC": # addition in this project 94 | self.controlDescription = ( 95 | "MPC target tracking" 96 | ) 97 | elif controlSystem == "TargetTrackingAI": # addition in this project 98 | self.controlDescription = ( 99 | "AI controlled target tracking" 100 | ) 101 | elif controlSystem == "manual": 102 | self.controlDescription = ( 103 | "Manual inserted n1, n2 in dynamics u_controls" 104 | ) 105 | else: 106 | self.controlDescription = "Step inputs for n1 and n2" 107 | controlSystem = "stepInput" 108 | 109 | self.__target_tracking_controller = None 110 | self.__ai_target_tracking_controller = None 111 | self.ref = r 112 | self.ref = r 113 | self.V_c = V_current 114 | self.beta_c = beta_current * D2R # angrepsvinkelen mot otteren 115 | self.controlMode = controlSystem 116 | self.tauX = tau_X # surge force (N) 117 | 118 | # Initialize the Otter USV model 119 | self.T_n = 1 # propeller time constants (s) 120 | self.L = 2.0 # Length (m) 121 | self.L = 2.0 # Length (m) 122 | self.B = 1.08 # beam (m) 123 | self.nu = np.array([0, 0, 0, 0, 0, 0], 124 | float) # velocity vector [surge(u), sway(v), have(w),roll(p),pitch(q),yaw(r)] 125 | # For the 3DOF otter, whe will have an underactuated equation, where the the components in the vector will be surge, sway and yaw, the rest is 0. 126 | 127 | self.u_actual = np.array([0, 0], float) # propeller revolution states 128 | self.name = "Otter USV (see 'otter.py' for more details)" 129 | 130 | self.controls = [ 131 | "Left propeller shaft speed (rad/s)", 132 | "Right propeller shaft speed (rad/s)" 133 | ] 134 | self.dimU = len(self.controls) 135 | 136 | # Vehicle parameters 137 | self.m = 55.0 # mass (kg)F 138 | m = self.m 139 | self.mp = 25.0 # Payload (kg) 140 | mp = self.mp 141 | self.m_total = m + mp 142 | self.rp = np.array([0, 0, -0.35], float) # location of payload (m) 143 | rp = self.rp 144 | self.rg = np.array([0.2, 0, -0.2], float) # CG for hull only (m) 145 | rg = self.rg # jalla løsning ! 146 | rg = (m * rg + mp * rp) / (m + mp) # CG corrected for payload 147 | self.rg = rg 148 | self.S_rg = Smtrx(rg) 149 | self.H_rg = Hmtrx(rg) 150 | self.S_rp = Smtrx(rp) 151 | 152 | R44 = 0.4 * self.B # radii of gyration (m) 153 | R55 = 0.25 * self.L 154 | R66 = 0.25 * self.L 155 | T_yaw = 1.0 # time constant in yaw (s) 156 | Umax = 6 * 0.5144 # max forward speed (m/s) 157 | 158 | # Data for one pontoon 159 | self.B_pont = 0.25 # beam of one pontoon (m) 160 | y_pont = 0.395 # distance from centerline to waterline centroid (m) 161 | Cw_pont = 0.75 # waterline area coefficient (-) 162 | Cb_pont = 0.4 # block coefficient, computed from m = 55 kg 163 | 164 | # Inertia dyadic, volume displacement and draft 165 | nabla = (m + mp) / rho # volume 166 | self.T = nabla / (2 * Cb_pont * self.B_pont * self.L) # draft 167 | Ig_CG = m * np.diag(np.array([R44 ** 2, R55 ** 2, R66 ** 2])) 168 | self.Ig = Ig_CG - m * self.S_rg @ self.S_rg - mp * self.S_rp @ self.S_rp 169 | 170 | # Experimental propeller data including lever arms 171 | self.l1 = -y_pont # lever arm, left propeller (m) 172 | self.l2 = y_pont # lever arm, right propeller (m) 173 | self.k_pos = 0.02216 / 2 # Positive Bollard, one propeller 174 | self.k_neg = 0.01289 / 2 # Negative Bollard, one propeller 175 | self.n_max = math.sqrt((0.5 * 24.4 * g) / self.k_pos) # max. prop. rev. 176 | self.n_min = -math.sqrt((0.5 * 13.6 * g) / self.k_neg) # min. prop. rev. 177 | 178 | # MRB_CG = [ (m+mp) * I3 O3 (Fossen 2021, Chapter 3) 179 | # O3 Ig ] 180 | MRB_CG = np.zeros((6, 6)) 181 | MRB_CG[0:3, 0:3] = (m + mp) * np.identity(3) 182 | MRB_CG[3:6, 3:6] = self.Ig 183 | MRB = self.H_rg.T @ MRB_CG @ self.H_rg 184 | self.MRB = MRB 185 | # print(f"otter MRB = {self.MRB}") 186 | 187 | # Hydrodynamic added mass (best practice) 188 | Xudot = -0.1 * m 189 | Yvdot = -1.5 * m 190 | Zwdot = -1.0 * m 191 | Kpdot = -0.2 * self.Ig[0, 0] 192 | Mqdot = -0.8 * self.Ig[1, 1] 193 | Nrdot = -1.7 * self.Ig[2, 2] 194 | 195 | self.MA = -np.diag([Xudot, Yvdot, Zwdot, Kpdot, Mqdot, Nrdot]) 196 | # print(self.MA) 197 | # System mass matrix 198 | self.M = MRB + self.MA 199 | self.Minv = np.linalg.inv(self.M) 200 | 201 | # Hydrostatic quantities (Fossen 2021, Chapter 4) 202 | Aw_pont = Cw_pont * self.L * self.B_pont # waterline area, one pontoon 203 | I_T = ( 204 | 2 205 | * (1 / 12) 206 | * self.L 207 | * self.B_pont ** 3 208 | * (6 * Cw_pont ** 3 / ((1 + Cw_pont) * (1 + 2 * Cw_pont))) 209 | + 2 * Aw_pont * y_pont ** 2 210 | ) 211 | I_L = 0.8 * 2 * (1 / 12) * self.B_pont * self.L ** 3 212 | KB = (1 / 3) * (5 * self.T / 2 - 0.5 * nabla / (self.L * self.B_pont)) 213 | BM_T = I_T / nabla # BM values 214 | BM_L = I_L / nabla 215 | KM_T = KB + BM_T # KM values 216 | KM_L = KB + BM_L 217 | KG = self.T - rg[2] 218 | GM_T = KM_T - KG # GM values 219 | GM_L = KM_L - KG 220 | 221 | G33 = rho * g * (2 * Aw_pont) # spring stiffness 222 | G44 = rho * g * nabla * GM_T 223 | G55 = rho * g * nabla * GM_L 224 | G_CF = np.diag([0, 0, G33, G44, G55, 0]) # spring stiff. matrix in CF 225 | LCF = -0.2 226 | H = Hmtrx(np.array([LCF, 0.0, 0.0])) # transform G_CF from CF to CO 227 | self.G = H.T @ G_CF @ H 228 | 229 | # Natural frequencies 230 | w3 = math.sqrt(G33 / self.M[2, 2]) 231 | w4 = math.sqrt(G44 / self.M[3, 3]) 232 | w5 = math.sqrt(G55 / self.M[4, 4]) 233 | 234 | # Linear damping terms (hydrodynamic derivatives)m 235 | Xu = -24.4 * g / Umax # specified using the maximum speed 236 | Yv = 0 237 | Zw = -2 * 0.3 * w3 * self.M[2, 2] # specified using relative damping 238 | Kp = -2 * 0.2 * w4 * self.M[3, 3] 239 | Mq = -2 * 0.4 * w5 * self.M[4, 4] 240 | Nr = -self.M[5, 5] / T_yaw # specified by the time constant T_yaw 241 | 242 | self.D = -np.diag([Xu, Yv, Zw, Kp, Mq, Nr]) # D-matrisen til Otteren 243 | # print(self.D) 244 | # Trim: theta = -7.5 deg corresponds to 13.5 cm less height aft 245 | self.trim_moment = 0 246 | self.trim_setpoint = 280 247 | 248 | # Propeller configuration/input matrix 249 | B = self.k_pos * np.array([[1, 1], [-self.l1, -self.l2]]) 250 | self.Binv = np.linalg.inv(B) 251 | 252 | # Heading autopilot 253 | self.e_int = 0 # integral state 254 | self.wn = 1.2 # PID pole placement 255 | self.zeta = 0.8 256 | 257 | # Reference model 258 | self.r_max = 10 * math.pi / 180 # maximum yaw rate 259 | self.psi_d = 0 # angle, angular rate and angular acc. states 260 | self.r_d = 0 261 | self.a_d = 0 262 | self.wn_d = self.wn / 5 # desired natural frequency in yaw 263 | self.zeta_d = 1 # desired relative damping ratio 264 | 265 | def dynamics(self, eta, nu, u_actual, u_control, sampleTime): 266 | """ 267 | [nu,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) integrates 268 | the Otter USV equations of motion using Euler's method. 269 | eta = np.array([0, 0, 0, 0, 0, 0], float) # position/attitude, user editable in the main loop 270 | nu = vehicle.nu # velocity, defined by vehicle class 271 | u_actual = vehicle.u_actual # actual inputs, defined by vehicle class 272 | beta_c (in rad) # current direction (deg) 273 | """ 274 | 275 | # Input vector 276 | n = np.array([u_actual[0], u_actual[1]]) 277 | 278 | # Current velocities 279 | u_c = self.V_c * math.cos(self.beta_c - eta[5]) # current surge vel. 280 | v_c = self.V_c * math.sin(self.beta_c - eta[5]) # current sway vel. 281 | 282 | nu_c = np.array([u_c, v_c, 0, 0, 0, 0], float) # current velocity vector 283 | nu_r = nu - nu_c # relative velocity vector 284 | 285 | # Rigid body and added mass Coriolis and centripetal matrices in center of gravity (CG) 286 | # CRB_CG = [ (m+mp) * Smtrx(nu2) O3 287 | # O3 -Smtrx(Ig*nu2) ](Fossen 2021, Chapter 6) 288 | CRB_CG = np.zeros((6, 6)) 289 | CRB_CG[0:3, 0:3] = self.m_total * Smtrx(nu[3:6]) 290 | CRB_CG[3:6, 3:6] = -Smtrx(np.matmul(self.Ig, nu[3:6])) 291 | CRB = self.H_rg.T @ CRB_CG @ self.H_rg # transform CRB from CG to CO(Body) 292 | 293 | # print(f"CRB: {utils.convert_6DOFto3DOF(CRB)}") 294 | CA = m2c(self.MA, nu_r) 295 | CA[5, 0] = 0 # assume that the Munk moment in yaw can be neglected 296 | CA[5, 1] = 0 # if nonzero, must be balanced by adding nonlinear damping 297 | # print(f"CA: {utils.convert_6DOFto3DOF(CA)}") 298 | C = CRB + CA 299 | # print(f"C: {utils.convert_6DOFto3DOF(C)}") 300 | 301 | # Ballast 302 | g_0 = np.array([0.0, 0.0, 0.0, 0.0, self.trim_moment, 0.0]) 303 | 304 | # Control forces and moments - with propeller revolution saturation 305 | thrust = np.zeros(2) 306 | for i in range(0, 2): 307 | 308 | n[i] = sat(n[i], self.n_min, self.n_max) # saturation, physical limits 309 | 310 | if n[i] > 0: # positive thrust 311 | thrust[i] = self.k_pos * n[i] * abs(n[i]) 312 | else: # negative thrust 313 | thrust[i] = self.k_neg * n[i] * abs(n[i]) 314 | 315 | # Control forces and moments 316 | tau = np.array( 317 | [ 318 | thrust[0] + thrust[1], 319 | 0, 320 | 0, 321 | 0, 322 | 0, 323 | -self.l1 * thrust[0] - self.l2 * thrust[1], 324 | ] 325 | ) 326 | #print(f"tau_actual from otter: {tau}") 327 | # Hydrodynamic linear(surge) damping(surge resistance) + nonlinear yaw damping 328 | tau_damp = -np.matmul(self.D, nu_r) 329 | tau_damp[5] = tau_damp[5] - 10 * self.D[5, 5] * abs(nu_r[5]) * nu_r[5] 330 | # print(f"tau_damp_shape: {tau_damp}") 331 | 332 | tau_crossflow = crossFlowDrag(self.L, self.B_pont, self.T, nu_r) # (Cross-flow drags .158 i fossen) 333 | 334 | # State derivatives (with dimension) 335 | sum_tau = ( 336 | tau 337 | + tau_damp 338 | + tau_crossflow 339 | - np.matmul(C, nu_r) 340 | - np.matmul(self.G, eta) # side 181 ligning 7.250, kun gjeldende i 6DOF 341 | - g_0 342 | ) 343 | # print(f"sum_tau = tau + tau_damp + tau_crossflow - C(nu) - G - g_0: \n" 344 | # f"sum_tau = {tau} + {tau_damp} + {tau_crossflow} - {np.matmul(C, nu_r)} - {np.matmul(self.G, eta) - g_0}") 345 | 346 | nu_dot = np.matmul(self.Minv, sum_tau) # USV kinetics (the kinematics is considered in the simulation loop) 347 | n_dot = (u_control - n) / self.T_n # propeller dynamics 348 | trim_dot = (self.trim_setpoint - self.trim_moment) / 5 # trim dynamics 349 | 350 | # Forward Euler integration [k+1] 351 | nu = nu + sampleTime * nu_dot 352 | n = n + sampleTime * n_dot 353 | self.trim_moment = self.trim_moment + sampleTime * trim_dot 354 | self.trim_moment = self.trim_moment + sampleTime * trim_dot 355 | 356 | u_actual = np.array(n, float) 357 | 358 | return nu, u_actual 359 | 360 | def controlAllocation(self, tau_X, tau_N): 361 | """ 362 | [n1, n2] = controlAllocation(tau_X, tau_N) 363 | """ 364 | tau = np.array([tau_X, tau_N]) # tau = B * u_alloc 365 | u_alloc = np.matmul(self.Binv, tau) # u_alloc = inv(B) * tau 366 | 367 | # u_alloc = abs(n) * n --> n = sign(u_alloc) * sqrt(u_alloc) 368 | n1 = np.sign(u_alloc[0]) * math.sqrt(abs(u_alloc[0])) 369 | n2 = np.sign(u_alloc[1]) * math.sqrt(abs(u_alloc[1])) 370 | 371 | return n1, n2 372 | 373 | def headingAutopilot(self, eta, nu, sampleTime): 374 | """ 375 | u = headingAutopilot(eta,nu,sampleTime) is a PID controller 376 | for automatic heading control based on pole placement. 377 | 378 | tau_N = (T/K) * a_d + (1/K) * rd 379 | - Kp * ( ssa( psi-psi_d ) + Td * (r - r_d) + (1/Ti) * z ) 380 | 381 | """ 382 | psi = eta[5] # yaw angle 383 | r = nu[5] # yaw rate 384 | e_psi = psi - self.psi_d # yaw angle tracking error 385 | e_r = r - self.r_d # yaw rate tracking error 386 | psi_ref = self.ref * math.pi / 180 # yaw angle setpoint 387 | 388 | wn = self.wn # PID natural frequency 389 | zeta = self.zeta # PID natural relative damping factor 390 | wn_d = self.wn_d # reference model natural frequency 391 | zeta_d = self.zeta_d # reference model relative damping factor 392 | 393 | m = 41.4 # moment of inertia in yaw including added mass 394 | T = 1 395 | K = T / m 396 | d = 1 / K 397 | k = 0 398 | 399 | # PID feedback controller with 3rd-order reference model 400 | tau_X = self.tauX 401 | 402 | [tau_N, self.e_int, self.psi_d, self.r_d, self.a_d] = PIDpolePlacement( 403 | self.e_int, 404 | e_psi, 405 | e_r, 406 | self.psi_d, 407 | self.r_d, 408 | self.a_d, 409 | m, 410 | d, 411 | k, 412 | wn_d, 413 | zeta_d, 414 | wn, 415 | zeta, 416 | psi_ref, 417 | self.r_max, 418 | sampleTime, 419 | ) 420 | 421 | [n1, n2] = self.controlAllocation(tau_X, tau_N) 422 | u_control = np.array([n1, n2], float) 423 | 424 | return u_control 425 | 426 | def target_tracking_mpc(self, initial_state, target): 427 | """ 428 | This method solves the optima controls 429 | :param initial_state: 430 | :param target: 431 | :return: 432 | """ 433 | #self.__target_tracking_controller.set_target(target) 434 | if not target: 435 | target = self.target 436 | tau = self.__target_tracking_controller.solve_optimal_control(initial_state=initial_state, target=target) 437 | #print(f"optimal control forces from MPC:{tau}") 438 | tau_X = tau[0] # desired thurs force in surge 439 | tau_N = tau[2] # desired moment among yaw 440 | 441 | u_control = np.array(self.controlAllocation(tau_X=tau_X, tau_N=tau_N), float) 442 | #print(f"u_controls: {u_control}") 443 | return u_control, tau 444 | 445 | def set_target_tracking_mpc(self, MPC): 446 | self.__target_tracking_controller = MPC 447 | 448 | def set_target_tracking_ai_controller(self, ai_controller): 449 | self.__ai_target_tracking_controller = ai_controller 450 | 451 | def target_tracking_ai_controller(self, initial_states, target, sample_time, t): 452 | #print(f"initial_states_otter:{initial_states}") 453 | tau = self.__ai_target_tracking_controller.get_controller_action(initial_state= initial_states, target = target, sample_time = sample_time, t= t ) 454 | #print(f"optimal control forces from AI:{tau}") 455 | tau_X = tau[0] # desired thurs force in surge 456 | tau_N = tau[1] # desired moment among yaw 457 | 458 | u_control = np.array(self.controlAllocation(tau_X=tau_X, tau_N=tau_N), float) 459 | #print(u_control) 460 | return u_control -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-21/AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_NED.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-21/AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_NED.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-21/AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_compute_time.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-21/AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_compute_time.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-21/AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_controls.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-21/AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_controls.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-21/AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_error.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-21/AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_error.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-21/AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_velocities.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-21/AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_velocities.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-21/copy_config.yml: -------------------------------------------------------------------------------- 1 | 2 | #simulation config 3 | sample_time : 0.1 4 | 5 | #Vehicle parameters 6 | constraints: 7 | forces: #(according to SNAME) 8 | X: #surge 9 | upper: 150 10 | lower: -116 11 | N: #torque among yaw 12 | upper: 73 13 | lower: -73 14 | 15 | 16 | #MPC controller parameters 17 | MPC: 18 | debug: True 19 | prediction_horizon: 10 20 | sample_time: 0.2 21 | solver : 'ipopt' 22 | R: [0.5,0 ,0.5] #weight matrix for the controller part of the matrix (sway is set to be zero) 23 | Q: [10000, 10000] #weight matrix for the state-element in the cost function 24 | nlp_options: #tips: https://github-wiki-see.page/m/casadi/casadi/wiki/FAQ%3A-how-to-pass-options-to-solvers#:~:text=Options%20for%20nonlinear%20programming%20solvers%20are%20passed%20in,%3D%20nlpsol%20%28%27solver_name%27%2C%27ipopt%27%2Cnlp%2Coptions%29%20Using%20Opti%20%3A%20opti.solver%20%28%27ipopt%27%2Coptions%29 25 | print_time: False 26 | 27 | options: 28 | ipopt: #ipopt options: https://coin-or.github.io/Ipopt/OPTIONS.html 29 | print_level : 0 # can be between 0-12 and will print different at each solution 30 | max_iter: 1000 31 | linear_solver: 32 | 33 | 34 | #AI-controller parameters 35 | env: #invironment parameters 36 | debug: False 37 | render_mode: #'human' #'human' #will render a plot every 1000 epoch (not working as it should) 38 | sim_length: 0.1 # how long to run the simulation between each action during training (seconds) 39 | sample_time: 0.1 # how long integration steps are used in the simulation 40 | observation_space: # used to normalize the observations to the neural network 41 | lower: 0 42 | higher: 600 # represents the 2 x radius range of the positioning system 43 | world_size: [-200,200] #represent min and max in a quadratic box shape (Not used) 44 | time_out: 1000 # steps for each episode 45 | random_target: True #defines of the target should randomly spawn each episode 46 | fixed_target: [30,0] 47 | target_spawn_region: 50 #defines the radius of the region where the target will randomly appear, used in training if the AI 48 | target_confident_region: 1 # radius, defines a confident region around the target, 49 | target_cycles: 100 #how many times the target is appearing in the same spot 50 | moving_target: 51 | pattern: c #None, Linear(l) or Circular(c) 52 | velocity: [0.0,0.8] 53 | radius: 200 #the path radius if circular movement 54 | 55 | 56 | 57 | #Tuning coefficients for the reward function in the environment 58 | c1: 0 # tuning coefficient for distance to target reward 59 | c2: 1 # tuning coefficient for target reached reward 60 | c3: 0.01 # tuning coefficient for controller penalty 61 | c4: 0.1 # tuning coefficient for intermediate time penalty 62 | c5: 0 # tuning parameter for time out penalty 63 | c6: 1 # tuning parameter for distance towards target change rate (speed towards target) 64 | c7: 1 # tuning coefficient for desired heading error 65 | 66 | architecture: [128,128] 67 | n_env: 10 #how many env to run in parallel 68 | n_stacked_frames: 8 # how many stacked frames to use, makes a simple "Memory" for the agent 69 | total_timesteps: 80_000_000 70 | starting_learning_rate: 0.00003 #all up until 27.03.2023 has been 0.0003, but I want to se the affect of using 0.003 in a 128x128 network 71 | model: 'PPO_moving_target_normalized' #prefix of the model 72 | #continue training paramaters 73 | continue_training: False 74 | log_dir: 'AI_controller/logs/PPO_moving_target_normalized/1000_30_5_False_1_0_0_0.1_0_0_1/2023-04-10 22-32-24' 75 | #'AI_controller/logs/PPO_moving_target_transferred_lr/1000_30_1_True_1_2_1_1_0_1/2023-04-07 23-16-45' 76 | #'AI_controller/logs/PPO_moving_target_transferred_lr/1000_30_1_True_1_2_1_1_0_1/2023-04-07 18-10-38' 77 | #'AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-28 09-17-53' 78 | #'AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-26 12-15-55' # relative path for the continued training 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-59/AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_NED.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-59/AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_NED.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-59/AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_compute_time.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-59/AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_compute_time.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-59/AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_controls.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-59/AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_controls.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-59/AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_error.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-59/AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_error.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-59/AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_velocities.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-59/AI_1000_50_1_True_0_1_0.01_0.1_0_1_1_velocities.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 1_ 1000_50_1_True_0_1_0.01_0.1_0_1_1 2023-05-01 19-10-59/copy_config.yml: -------------------------------------------------------------------------------- 1 | 2 | #simulation config 3 | sample_time : 0.1 4 | 5 | #Vehicle parameters 6 | constraints: 7 | forces: #(according to SNAME) 8 | X: #surge 9 | upper: 150 10 | lower: -116 11 | N: #torque among yaw 12 | upper: 73 13 | lower: -73 14 | 15 | 16 | #MPC controller parameters 17 | MPC: 18 | debug: True 19 | prediction_horizon: 10 20 | sample_time: 0.2 21 | solver : 'ipopt' 22 | R: [0.5,0 ,0.5] #weight matrix for the controller part of the matrix (sway is set to be zero) 23 | Q: [10000, 10000] #weight matrix for the state-element in the cost function 24 | nlp_options: #tips: https://github-wiki-see.page/m/casadi/casadi/wiki/FAQ%3A-how-to-pass-options-to-solvers#:~:text=Options%20for%20nonlinear%20programming%20solvers%20are%20passed%20in,%3D%20nlpsol%20%28%27solver_name%27%2C%27ipopt%27%2Cnlp%2Coptions%29%20Using%20Opti%20%3A%20opti.solver%20%28%27ipopt%27%2Coptions%29 25 | print_time: False 26 | 27 | options: 28 | ipopt: #ipopt options: https://coin-or.github.io/Ipopt/OPTIONS.html 29 | print_level : 0 # can be between 0-12 and will print different at each solution 30 | max_iter: 1000 31 | linear_solver: 32 | 33 | 34 | #AI-controller parameters 35 | env: #invironment parameters 36 | debug: False 37 | render_mode: #'human' #'human' #will render a plot every 1000 epoch (not working as it should) 38 | sim_length: 0.1 # how long to run the simulation between each action during training (seconds) 39 | sample_time: 0.1 # how long integration steps are used in the simulation 40 | observation_space: # used to normalize the observations to the neural network 41 | lower: 0 42 | higher: 600 # represents the 2 x radius range of the positioning system 43 | world_size: [-200,200] #represent min and max in a quadratic box shape (for now this does not 44 | time_out: 1000 # how many for each episode 45 | random_target: True 46 | fixed_target: [30,0] 47 | target_spawn_region: 50 #defines the radius of the region where the target will randomly appear, used in training if the AI 48 | target_confident_region: 1 # radius, defines a confident region around the target, 49 | target_cycles: 100 #how many times the target is appearing in the same spot 50 | moving_target: 51 | pattern: c #None, Linear(l) or Circular(c) 52 | velocity: [0.0,0.8] 53 | radius: 200 #the path radius if circular movement 54 | 55 | 56 | 57 | #Tuning coefficients for the reward function in the environment 58 | c1: 0 # tuning coefficient for distance to target reward 59 | c2: 1 # tuning coefficient for target reached reward 60 | c3: 0.01 # tuning coefficient for controller penalty 61 | c4: 0.1 # tuning coefficient for intermediate time penalty 62 | c5: 0 # tuning parameter for time out penalty 63 | c6: 1 # tuning parameter for distance towards target change rate (speed towards target) 64 | c7: 1 # tuning coefficient for desired heading error 65 | 66 | architecture: [128,128] 67 | n_env: 10 #how many env to run in parallel 68 | n_stacked_frames: 8 # how many stacked frames to use, makes a simple "Memory" for the agent 69 | total_timesteps: 80_000_000 70 | starting_learning_rate: 0.00003 #all up until 27.03.2023 has been 0.0003, but I want to se the affect of using 0.003 in a 128x128 network 71 | model: 'PPO_moving_target_normalized' #prefix of the model 72 | #continue training paramaters 73 | continue_training: False 74 | log_dir: 'AI_controller/logs/PPO_moving_target_normalized/1000_30_5_False_1_0_0_0.1_0_0_1/2023-04-10 22-32-24' 75 | #'AI_controller/logs/PPO_moving_target_transferred_lr/1000_30_1_True_1_2_1_1_0_1/2023-04-07 23-16-45' 76 | #'AI_controller/logs/PPO_moving_target_transferred_lr/1000_30_1_True_1_2_1_1_0_1/2023-04-07 18-10-38' 77 | #'AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-28 09-17-53' 78 | #'AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-26 12-15-55' # relative path for the continued training 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-24/AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_NED.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-24/AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_NED.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-24/AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_compute_time.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-24/AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_compute_time.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-24/AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_controls.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-24/AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_controls.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-24/AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_error.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-24/AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_error.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-24/AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_velocities.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-24/AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_velocities.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-24/copy_config.yml: -------------------------------------------------------------------------------- 1 | 2 | #simulation config 3 | sample_time : 0.1 4 | 5 | #Vehicle parameters 6 | constraints: 7 | forces: #(according to SNAME) 8 | X: #surge 9 | upper: 150 10 | lower: -116 11 | N: #torque among yaw 12 | upper: 73 13 | lower: -73 14 | 15 | 16 | #MPC controller parameters 17 | MPC: 18 | debug: True 19 | prediction_horizon: 10 20 | sample_time: 0.2 21 | solver : 'ipopt' 22 | R: [0.5,0 ,0.5] #weight matrix for the controller part of the matrix (sway is set to be zero) 23 | Q: [10000, 10000] #weight matrix for the state-element in the cost function 24 | nlp_options: #tips: https://github-wiki-see.page/m/casadi/casadi/wiki/FAQ%3A-how-to-pass-options-to-solvers#:~:text=Options%20for%20nonlinear%20programming%20solvers%20are%20passed%20in,%3D%20nlpsol%20%28%27solver_name%27%2C%27ipopt%27%2Cnlp%2Coptions%29%20Using%20Opti%20%3A%20opti.solver%20%28%27ipopt%27%2Coptions%29 25 | print_time: False 26 | 27 | options: 28 | ipopt: #ipopt options: https://coin-or.github.io/Ipopt/OPTIONS.html 29 | print_level : 0 # can be between 0-12 and will print different at each solution 30 | max_iter: 1000 31 | linear_solver: 32 | 33 | 34 | #AI-controller parameters 35 | env: #invironment parameters 36 | debug: False 37 | render_mode: #'human' #'human' #will render a plot every 1000 epoch (not working as it should) 38 | sim_length: 0.1 # how long to run the simulation between each action during training (seconds) 39 | sample_time: 0.1 # how long integration steps are used in the simulation 40 | observation_space: # used to normalize the observations to the neural network 41 | lower: 0 42 | higher: 600 # represents the 2 x radius range of the positioning system 43 | world_size: [-200,200] #represent min and max in a quadratic box shape (for now this does not 44 | time_out: 1000 # how many for each episode 45 | random_target: True 46 | fixed_target: [30,0] 47 | target_spawn_region: 50 #defines the radius of the region where the target will randomly appear, used in training if the AI 48 | target_confident_region: 1 # radius, defines a confident region around the target, 49 | target_cycles: 100 #how many times the target is appearing in the same spot 50 | moving_target: 51 | pattern: c #None, Linear(l) or Circular(c) 52 | velocity: [0.0,0.8] 53 | radius: 200 #the path radius if circular movement 54 | 55 | 56 | 57 | #Tuning coefficients for the reward function in the environment 58 | c1: 0 # tuning coefficient for distance to target reward 59 | c2: 1 # tuning coefficient for target reached reward 60 | c3: 0.1 # tuning coefficient for controller penalty 61 | c4: 0.1 # tuning coefficient for intermediate time penalty 62 | c5: 0 # tuning parameter for time out penalty 63 | c6: 1 # tuning parameter for distance towards target change rate (speed towards target) 64 | c7: 1 # tuning coefficient for desired heading error 65 | 66 | architecture: [128,128] 67 | n_env: 10 #how many env to run in parallel 68 | n_stacked_frames: 8 # how many stacked frames to use, makes a simple "Memory" for the agent 69 | total_timesteps: 30_000_000 70 | starting_learning_rate: 0.000003 #all up until 27.03.2023 has been 0.0003, but I want to se the affect of using 0.003 in a 128x128 network 71 | model: 'PPO_moving_target_normalized' #prefix of the model 72 | #continue training paramaters 73 | continue_training: False 74 | log_dir: 'AI_controller/logs/PPO_moving_target_normalized/1000_30_5_False_1_0_0_0.1_0_0_1/2023-04-10 22-32-24' 75 | #'AI_controller/logs/PPO_moving_target_transferred_lr/1000_30_1_True_1_2_1_1_0_1/2023-04-07 23-16-45' 76 | #'AI_controller/logs/PPO_moving_target_transferred_lr/1000_30_1_True_1_2_1_1_0_1/2023-04-07 18-10-38' 77 | #'AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-28 09-17-53' 78 | #'AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-26 12-15-55' # relative path for the continued training 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-38/AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_NED.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-38/AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_NED.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-38/AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_compute_time.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-38/AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_compute_time.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-38/AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_controls.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-38/AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_controls.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-38/AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_error.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-38/AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_error.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-38/AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_velocities.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-38/AI_1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06_velocities.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 2_ 1000_50_1_True_0_1_0.1_0.1_0_1_1_3e-06 2023-05-01 19-12-38/copy_config.yml: -------------------------------------------------------------------------------- 1 | 2 | #simulation config 3 | sample_time : 0.1 4 | 5 | #Vehicle parameters 6 | constraints: 7 | forces: #(according to SNAME) 8 | X: #surge 9 | upper: 150 10 | lower: -116 11 | N: #torque among yaw 12 | upper: 73 13 | lower: -73 14 | 15 | 16 | #MPC controller parameters 17 | MPC: 18 | debug: True 19 | prediction_horizon: 10 20 | sample_time: 0.2 21 | solver : 'ipopt' 22 | R: [0.5,0 ,0.5] #weight matrix for the controller part of the matrix (sway is set to be zero) 23 | Q: [10000, 10000] #weight matrix for the state-element in the cost function 24 | nlp_options: #tips: https://github-wiki-see.page/m/casadi/casadi/wiki/FAQ%3A-how-to-pass-options-to-solvers#:~:text=Options%20for%20nonlinear%20programming%20solvers%20are%20passed%20in,%3D%20nlpsol%20%28%27solver_name%27%2C%27ipopt%27%2Cnlp%2Coptions%29%20Using%20Opti%20%3A%20opti.solver%20%28%27ipopt%27%2Coptions%29 25 | print_time: False 26 | 27 | options: 28 | ipopt: #ipopt options: https://coin-or.github.io/Ipopt/OPTIONS.html 29 | print_level : 0 # can be between 0-12 and will print different at each solution 30 | max_iter: 1000 31 | linear_solver: 32 | 33 | 34 | #AI-controller parameters 35 | env: #invironment parameters 36 | debug: False 37 | render_mode: #'human' #'human' #will render a plot every 1000 epoch (not working as it should) 38 | sim_length: 0.1 # how long to run the simulation between each action during training (seconds) 39 | sample_time: 0.1 # how long integration steps are used in the simulation 40 | observation_space: # used to normalize the observations to the neural network 41 | lower: 0 42 | higher: 600 # represents the 2 x radius range of the positioning system 43 | world_size: [-200,200] #represent min and max in a quadratic box shape (for now this does not 44 | time_out: 1000 # how many for each episode 45 | random_target: True 46 | fixed_target: [30,0] 47 | target_spawn_region: 50 #defines the radius of the region where the target will randomly appear, used in training if the AI 48 | target_confident_region: 1 # radius, defines a confident region around the target, 49 | target_cycles: 100 #how many times the target is appearing in the same spot 50 | moving_target: 51 | pattern: c #None, Linear(l) or Circular(c) 52 | velocity: [0.0,0.8] 53 | radius: 200 #the path radius if circular movement 54 | 55 | 56 | 57 | #Tuning coefficients for the reward function in the environment 58 | c1: 0 # tuning coefficient for distance to target reward 59 | c2: 1 # tuning coefficient for target reached reward 60 | c3: 0.1 # tuning coefficient for controller penalty 61 | c4: 0.1 # tuning coefficient for intermediate time penalty 62 | c5: 0 # tuning parameter for time out penalty 63 | c6: 1 # tuning parameter for distance towards target change rate (speed towards target) 64 | c7: 1 # tuning coefficient for desired heading error 65 | 66 | architecture: [128,128] 67 | n_env: 10 #how many env to run in parallel 68 | n_stacked_frames: 8 # how many stacked frames to use, makes a simple "Memory" for the agent 69 | total_timesteps: 30_000_000 70 | starting_learning_rate: 0.000003 #all up until 27.03.2023 has been 0.0003, but I want to se the affect of using 0.003 in a 128x128 network 71 | model: 'PPO_moving_target_normalized' #prefix of the model 72 | #continue training paramaters 73 | continue_training: False 74 | log_dir: 'AI_controller/logs/PPO_moving_target_normalized/1000_30_5_False_1_0_0_0.1_0_0_1/2023-04-10 22-32-24' 75 | #'AI_controller/logs/PPO_moving_target_transferred_lr/1000_30_1_True_1_2_1_1_0_1/2023-04-07 23-16-45' 76 | #'AI_controller/logs/PPO_moving_target_transferred_lr/1000_30_1_True_1_2_1_1_0_1/2023-04-07 18-10-38' 77 | #'AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-28 09-17-53' 78 | #'AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-26 12-15-55' # relative path for the continued training 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-19/AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_NED.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-19/AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_NED.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-19/AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_compute_time.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-19/AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_compute_time.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-19/AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_controls.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-19/AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_controls.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-19/AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_error.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-19/AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_error.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-19/AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_velocities.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-19/AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_velocities.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-19/copy_config.yml: -------------------------------------------------------------------------------- 1 | 2 | #simulation config 3 | sample_time : 0.1 4 | 5 | #Vehicle parameters 6 | constraints: 7 | forces: #(according to SNAME) 8 | X: #surge 9 | upper: 150 10 | lower: -116 11 | N: #torque among yaw 12 | upper: 73 13 | lower: -73 14 | 15 | 16 | #MPC controller parameters 17 | MPC: 18 | debug: True 19 | prediction_horizon: 10 20 | sample_time: 0.2 21 | solver : 'ipopt' 22 | R: [0.5,0 ,0.5] #weight matrix for the controller part of the matrix (sway is set to be zero) 23 | Q: [10000, 10000] #weight matrix for the state-element in the cost function 24 | nlp_options: #tips: https://github-wiki-see.page/m/casadi/casadi/wiki/FAQ%3A-how-to-pass-options-to-solvers#:~:text=Options%20for%20nonlinear%20programming%20solvers%20are%20passed%20in,%3D%20nlpsol%20%28%27solver_name%27%2C%27ipopt%27%2Cnlp%2Coptions%29%20Using%20Opti%20%3A%20opti.solver%20%28%27ipopt%27%2Coptions%29 25 | print_time: False 26 | 27 | options: 28 | ipopt: #ipopt options: https://coin-or.github.io/Ipopt/OPTIONS.html 29 | print_level : 0 # can be between 0-12 and will print different at each solution 30 | max_iter: 1000 31 | linear_solver: 32 | 33 | 34 | #AI-controller parameters 35 | env: #invironment parameters 36 | debug: False 37 | render_mode: #'human' #'human' #will render a plot every 1000 epoch (not working as it should) 38 | sim_length: 0.1 # how long to run the simulation between each action during training (seconds) 39 | sample_time: 0.1 # how long integration steps are used in the simulation 40 | observation_space: # used to normalize the observations to the neural network 41 | lower: 0 42 | higher: 600 # represents the 2 x radius range of the positioning system 43 | world_size: [-200,200] #represent min and max in a quadratic box shape (for now this does not 44 | time_out: 1000 # how many for each episode 45 | random_target: True 46 | fixed_target: [30,0] 47 | target_spawn_region: 50 #defines the radius of the region where the target will randomly appear, used in training if the AI 48 | target_confident_region: 1 # radius, defines a confident region around the target, 49 | target_cycles: 100 #how many times the target is appearing in the same spot 50 | moving_target: 51 | pattern: c #None, Linear(l) or Circular(c) 52 | velocity: [0.0,0.8] 53 | radius: 200 #the path radius if circular movement 54 | 55 | 56 | 57 | #Tuning coefficients for the reward function in the environment 58 | c1: 1 # tuning coefficient for distance to target reward 59 | c2: 0 # tuning coefficient for target reached reward 60 | c3: 0.01 # tuning coefficient for controller penalty 61 | c4: 0.1 # tuning coefficient for intermediate time penalty 62 | c5: 0 # tuning parameter for time out penalty 63 | c6: 0 # tuning parameter for distance towards target change rate (speed towards target) 64 | c7: 1 # tuning coefficient for desired heading error 65 | 66 | architecture: [128,128] 67 | n_env: 10 #how many env to run in parallel 68 | n_stacked_frames: 8 # how many stacked frames to use, makes a simple "Memory" for the agent 69 | total_timesteps: 80_000_000 70 | starting_learning_rate: 0.0003 #all up until 27.03.2023 has been 0.0003, but I want to se the affect of using 0.003 in a 128x128 network 71 | model: 'PPO_moving_target_normalized' #prefix of the model 72 | #continue training paramaters 73 | continue_training: False 74 | log_dir: 'AI_controller/logs/PPO_moving_target_normalized/1000_30_5_False_1_0_0_0.1_0_0_1/2023-04-10 22-32-24' 75 | #'AI_controller/logs/PPO_moving_target_transferred_lr/1000_30_1_True_1_2_1_1_0_1/2023-04-07 23-16-45' 76 | #'AI_controller/logs/PPO_moving_target_transferred_lr/1000_30_1_True_1_2_1_1_0_1/2023-04-07 18-10-38' 77 | #'AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-28 09-17-53' 78 | #'AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-26 12-15-55' # relative path for the continued training 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-59/AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_NED.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-59/AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_NED.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-59/AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_compute_time.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-59/AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_compute_time.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-59/AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_controls.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-59/AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_controls.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-59/AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_error.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-59/AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_error.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-59/AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_velocities.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-59/AI_1000_50_1_True_1_0_0.01_0.1_0_0_1_velocities.pdf -------------------------------------------------------------------------------- /results/TargetTrackingAI/Model 3_ 1000_50_1_True_1_0_0.01_0.1_0_0_1 2023-05-01 19-13-59/copy_config.yml: -------------------------------------------------------------------------------- 1 | 2 | #simulation config 3 | sample_time : 0.1 4 | 5 | #Vehicle parameters 6 | constraints: 7 | forces: #(according to SNAME) 8 | X: #surge 9 | upper: 150 10 | lower: -116 11 | N: #torque among yaw 12 | upper: 73 13 | lower: -73 14 | 15 | 16 | #MPC controller parameters 17 | MPC: 18 | debug: True 19 | prediction_horizon: 10 20 | sample_time: 0.2 21 | solver : 'ipopt' 22 | R: [0.5,0 ,0.5] #weight matrix for the controller part of the matrix (sway is set to be zero) 23 | Q: [10000, 10000] #weight matrix for the state-element in the cost function 24 | nlp_options: #tips: https://github-wiki-see.page/m/casadi/casadi/wiki/FAQ%3A-how-to-pass-options-to-solvers#:~:text=Options%20for%20nonlinear%20programming%20solvers%20are%20passed%20in,%3D%20nlpsol%20%28%27solver_name%27%2C%27ipopt%27%2Cnlp%2Coptions%29%20Using%20Opti%20%3A%20opti.solver%20%28%27ipopt%27%2Coptions%29 25 | print_time: False 26 | 27 | options: 28 | ipopt: #ipopt options: https://coin-or.github.io/Ipopt/OPTIONS.html 29 | print_level : 0 # can be between 0-12 and will print different at each solution 30 | max_iter: 1000 31 | linear_solver: 32 | 33 | 34 | #AI-controller parameters 35 | env: #invironment parameters 36 | debug: False 37 | render_mode: #'human' #'human' #will render a plot every 1000 epoch (not working as it should) 38 | sim_length: 0.1 # how long to run the simulation between each action during training (seconds) 39 | sample_time: 0.1 # how long integration steps are used in the simulation 40 | observation_space: # used to normalize the observations to the neural network 41 | lower: 0 42 | higher: 600 # represents the 2 x radius range of the positioning system 43 | world_size: [-200,200] #represent min and max in a quadratic box shape (for now this does not 44 | time_out: 1000 # how many for each episode 45 | random_target: True 46 | fixed_target: [30,0] 47 | target_spawn_region: 50 #defines the radius of the region where the target will randomly appear, used in training if the AI 48 | target_confident_region: 1 # radius, defines a confident region around the target, 49 | target_cycles: 100 #how many times the target is appearing in the same spot 50 | moving_target: 51 | pattern: c #None, Linear(l) or Circular(c) 52 | velocity: [0.0,0.8] 53 | radius: 200 #the path radius if circular movement 54 | 55 | 56 | 57 | #Tuning coefficients for the reward function in the environment 58 | c1: 1 # tuning coefficient for distance to target reward 59 | c2: 0 # tuning coefficient for target reached reward 60 | c3: 0.01 # tuning coefficient for controller penalty 61 | c4: 0.1 # tuning coefficient for intermediate time penalty 62 | c5: 0 # tuning parameter for time out penalty 63 | c6: 0 # tuning parameter for distance towards target change rate (speed towards target) 64 | c7: 1 # tuning coefficient for desired heading error 65 | 66 | architecture: [128,128] 67 | n_env: 10 #how many env to run in parallel 68 | n_stacked_frames: 8 # how many stacked frames to use, makes a simple "Memory" for the agent 69 | total_timesteps: 80_000_000 70 | starting_learning_rate: 0.0003 #all up until 27.03.2023 has been 0.0003, but I want to se the affect of using 0.003 in a 128x128 network 71 | model: 'PPO_moving_target_normalized' #prefix of the model 72 | #continue training paramaters 73 | continue_training: False 74 | log_dir: 'AI_controller/logs/PPO_moving_target_normalized/1000_30_5_False_1_0_0_0.1_0_0_1/2023-04-10 22-32-24' 75 | #'AI_controller/logs/PPO_moving_target_transferred_lr/1000_30_1_True_1_2_1_1_0_1/2023-04-07 23-16-45' 76 | #'AI_controller/logs/PPO_moving_target_transferred_lr/1000_30_1_True_1_2_1_1_0_1/2023-04-07 18-10-38' 77 | #'AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-28 09-17-53' 78 | #'AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-26 12-15-55' # relative path for the continued training 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /results/TargetTrackingMPC/2023-04-29 13-31-25_case 1/.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingMPC/2023-04-29 13-31-25_case 1/.png -------------------------------------------------------------------------------- /results/TargetTrackingMPC/2023-04-29 13-31-25_case 1/NED.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingMPC/2023-04-29 13-31-25_case 1/NED.pdf -------------------------------------------------------------------------------- /results/TargetTrackingMPC/2023-04-29 13-31-25_case 1/config.yml: -------------------------------------------------------------------------------- 1 | 2 | #simulation config 3 | sample_time : 0.1 4 | 5 | #Vehicle parameters 6 | constraints: 7 | forces: #(according to SNAME) 8 | X: #surge 9 | upper: 150 10 | lower: -116 11 | N: #torque among yaw 12 | upper: 73 13 | lower: -73 14 | 15 | 16 | #MPC controller parameters 17 | MPC: 18 | debug: True 19 | prediction_horizon: 10 #Prediction for the MPCThmodel 20 | sample_time: 0.2 #teh sampling time for the MPC optimization Casadi 3 DOF model 21 | solver : 'ipopt' 22 | R: [0.5,0 ,0.5] #weight matrix for the controller part of the matrix (sway is set to be zero) 23 | Q: [10000, 10000] #weight matrix for the state-element in the cost function 24 | nlp_options: #tip: https://github-wiki-see.page/m/casadi/casadi/wiki/FAQ%3A-how-to-pass-options-to-solvers#:~:text=Options%20for%20nonlinear%20programming%20solvers%20are%20passed%20in,%3D%20nlpsol%20%28%27solver_name%27%2C%27ipopt%27%2Cnlp%2Coptions%29%20Using%20Opti%20%3A%20opti.solver%20%28%27ipopt%27%2Coptions%29 25 | print_time: False 26 | 27 | options: 28 | ipopt: #ipopt options: https://coin-or.github.io/Ipopt/OPTIONS.html 29 | print_level : 2 # can be between 0-12 and will print different at each solution 30 | max_iter: 10000 31 | linear_solver: 32 | output_file: # 'log.csv' 33 | file_print_level: # 1 34 | print_timing_statistics: # 'yes' 35 | 36 | #AI-controller parameters 37 | env: #invironment parameters 38 | debug: False 39 | render_mode: #'human' #'human' #will render a plot every 1000 epoch (not working as it should) 40 | sim_length: 0.1 # how long to run the simulation between each action during training (seconds) 41 | sample_time: 0.1 # how long integration steps are used in the simulation 42 | observation_space: # used to normalize the observations to the neural network 43 | lower: 0 44 | higher: 600 # represents the 2 x radius range of the positioning system 45 | world_size: [-200,200] #represent min and max in a quadratic box shape (for now this does not 46 | time_out: 8000 # how many for each episode 47 | random_target: False 48 | fixed_target: [20,20] 49 | target_spawn_region: 30 #defines the radius of the region where the target will randomly appear, used in training if the AI 50 | target_confident_region: 1 # radius, defines a confident region around the target, 51 | target_cycles: 100 #how many times the target is appearing in the same spot 52 | moving_target: 53 | pattern: c #None, Linear(l) or Circular(c) 54 | velocity: [0.25,0.25] #randomly 55 | radius: 200 #the path radius if circular movement 56 | 57 | 58 | 59 | #Tuning coefficients for the reward function in the environment 60 | c1: 1 # tuning coefficient for distance to target reward 61 | c2: 1 # tuning coefficient for target reached reward 62 | c3: 1 # tuning coefficient for controller penalty 63 | c4: 1 # tuning coefficient for intermediate time penalty 64 | c5: 1 # tuning parameter for time out penalty 65 | c6: 1 # tuning parameter for distance towards target change rate (speed towards target) 66 | c7: 1 # tuning coefficient for desired heading error 67 | 68 | architecture: [128,128] 69 | n_env: 10 #how many env to run in parallel 70 | n_stacked_frames: 8 # how many stacked frames to use, makes a simple "Memory" for the agent 71 | total_timesteps: 8_000_000 72 | starting_learning_rate: 0.000003 #all up until 27.03.2023 has been 0.0003, but I want to se the affect of using 0.003 in a 128x128 network 73 | model: 'PPO_moving_target_normalized' #prefix of the model 74 | #continue training paramaters 75 | continue_training: False 76 | log_dir: 'AI_controller/logs/PPO_moving_target_normalized/1000_30_5_False_1_0_0_0.1_0_0_1/2023-04-10 22-32-24' 77 | #'AI_controller/logs/PPO_moving_target_transferred_lr/1000_30_1_True_1_2_1_1_0_1/2023-04-07 23-16-45' 78 | #'AI_controller/logs/PPO_moving_target_transferred_lr/1000_30_1_True_1_2_1_1_0_1/2023-04-07 18-10-38' 79 | #'AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-28 09-17-53' 80 | #'AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-26 12-15-55' # relative path for the continued training 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /results/TargetTrackingMPC/2023-04-29 13-31-25_case 1/control_compute_time.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingMPC/2023-04-29 13-31-25_case 1/control_compute_time.pdf -------------------------------------------------------------------------------- /results/TargetTrackingMPC/2023-04-29 13-31-25_case 1/control_forces.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingMPC/2023-04-29 13-31-25_case 1/control_forces.pdf -------------------------------------------------------------------------------- /results/TargetTrackingMPC/2023-04-29 13-31-25_case 1/control_rpms.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingMPC/2023-04-29 13-31-25_case 1/control_rpms.pdf -------------------------------------------------------------------------------- /results/TargetTrackingMPC/2023-04-29 13-31-25_case 1/error.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingMPC/2023-04-29 13-31-25_case 1/error.pdf -------------------------------------------------------------------------------- /results/TargetTrackingMPC/2023-04-29 13-31-25_case 1/vel.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingMPC/2023-04-29 13-31-25_case 1/vel.pdf -------------------------------------------------------------------------------- /results/TargetTrackingMPC/2023-04-29 13-31-25_case 1/velocities.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingMPC/2023-04-29 13-31-25_case 1/velocities.pdf -------------------------------------------------------------------------------- /results/TargetTrackingMPC/2023-04-29 15-17-51_ case 2/config.yml: -------------------------------------------------------------------------------- 1 | 2 | #simulation config 3 | sample_time : 0.1 4 | 5 | #Vehicle parameters 6 | constraints: 7 | forces: #(according to SNAME) 8 | X: #surge 9 | upper: 150 10 | lower: -116 11 | N: #torque among yaw 12 | upper: 73 13 | lower: -73 14 | 15 | 16 | #MPC controller parameters 17 | MPC: 18 | debug: True 19 | prediction_horizon: 10 #Prediction for the MPCThmodel 20 | sample_time: 0.2 #teh sampling time for the MPC optimization Casadi 3 DOF model 21 | solver : 'ipopt' 22 | R: [0.5,0 ,0.5] #weight matrix for the controller part of the matrix (sway is set to be zero) 23 | Q: [10000, 10000] #weight matrix for the state-element in the cost function 24 | nlp_options: #tip: https://github-wiki-see.page/m/casadi/casadi/wiki/FAQ%3A-how-to-pass-options-to-solvers#:~:text=Options%20for%20nonlinear%20programming%20solvers%20are%20passed%20in,%3D%20nlpsol%20%28%27solver_name%27%2C%27ipopt%27%2Cnlp%2Coptions%29%20Using%20Opti%20%3A%20opti.solver%20%28%27ipopt%27%2Coptions%29 25 | print_time: False 26 | 27 | options: 28 | ipopt: #ipopt options: https://coin-or.github.io/Ipopt/OPTIONS.html 29 | print_level : 2 # can be between 0-12 and will print different at each solution 30 | max_iter: 10000 31 | linear_solver: 32 | output_file: # 'log.csv' 33 | file_print_level: # 1 34 | print_timing_statistics: # 'yes' 35 | 36 | #AI-controller parameters 37 | env: #invironment parameters 38 | debug: False 39 | render_mode: #'human' #'human' #will render a plot every 1000 epoch (not working as it should) 40 | sim_length: 0.1 # how long to run the simulation between each action during training (seconds) 41 | sample_time: 0.1 # how long integration steps are used in the simulation 42 | observation_space: # used to normalize the observations to the neural network 43 | lower: 0 44 | higher: 600 # represents the 2 x radius range of the positioning system 45 | world_size: [-200,200] #represent min and max in a quadratic box shape (for now this does not 46 | time_out: 8000 # how many for each episode 47 | random_target: False 48 | fixed_target: [20,20] 49 | target_spawn_region: 30 #defines the radius of the region where the target will randomly appear, used in training if the AI 50 | target_confident_region: 1 # radius, defines a confident region around the target, 51 | target_cycles: 100 #how many times the target is appearing in the same spot 52 | moving_target: 53 | pattern: c #None, Linear(l) or Circular(c) 54 | velocity: [0.25,0.25] #randomly 55 | radius: 200 #the path radius if circular movement 56 | 57 | 58 | 59 | #Tuning coefficients for the reward function in the environment 60 | c1: 1 # tuning coefficient for distance to target reward 61 | c2: 1 # tuning coefficient for target reached reward 62 | c3: 1 # tuning coefficient for controller penalty 63 | c4: 1 # tuning coefficient for intermediate time penalty 64 | c5: 1 # tuning parameter for time out penalty 65 | c6: 1 # tuning parameter for distance towards target change rate (speed towards target) 66 | c7: 1 # tuning coefficient for desired heading error 67 | 68 | architecture: [128,128] 69 | n_env: 10 #how many env to run in parallel 70 | n_stacked_frames: 8 # how many stacked frames to use, makes a simple "Memory" for the agent 71 | total_timesteps: 8_000_000 72 | starting_learning_rate: 0.000003 #all up until 27.03.2023 has been 0.0003, but I want to se the affect of using 0.003 in a 128x128 network 73 | model: 'PPO_moving_target_normalized' #prefix of the model 74 | #continue training paramaters 75 | continue_training: False 76 | log_dir: 'AI_controller/logs/PPO_moving_target_normalized/1000_30_5_False_1_0_0_0.1_0_0_1/2023-04-10 22-32-24' 77 | #'AI_controller/logs/PPO_moving_target_transferred_lr/1000_30_1_True_1_2_1_1_0_1/2023-04-07 23-16-45' 78 | #'AI_controller/logs/PPO_moving_target_transferred_lr/1000_30_1_True_1_2_1_1_0_1/2023-04-07 18-10-38' 79 | #'AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-28 09-17-53' 80 | #'AI_controller/logs/PPO_end_when_reaching_target_true/500_150_5_True_0_1_1_1_0_1/2023-03-26 12-15-55' # relative path for the continued training 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /results/TargetTrackingMPC/2023-04-29 15-17-51_ case 2/res_mpc_case2_NED.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingMPC/2023-04-29 15-17-51_ case 2/res_mpc_case2_NED.pdf -------------------------------------------------------------------------------- /results/TargetTrackingMPC/2023-04-29 15-17-51_ case 2/res_mpc_case2_control_compute_time.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingMPC/2023-04-29 15-17-51_ case 2/res_mpc_case2_control_compute_time.pdf -------------------------------------------------------------------------------- /results/TargetTrackingMPC/2023-04-29 15-17-51_ case 2/res_mpc_case2_control_forces.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingMPC/2023-04-29 15-17-51_ case 2/res_mpc_case2_control_forces.pdf -------------------------------------------------------------------------------- /results/TargetTrackingMPC/2023-04-29 15-17-51_ case 2/res_mpc_case2_control_rpms.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingMPC/2023-04-29 15-17-51_ case 2/res_mpc_case2_control_rpms.pdf -------------------------------------------------------------------------------- /results/TargetTrackingMPC/2023-04-29 15-17-51_ case 2/res_mpc_case2_error.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingMPC/2023-04-29 15-17-51_ case 2/res_mpc_case2_error.pdf -------------------------------------------------------------------------------- /results/TargetTrackingMPC/2023-04-29 15-17-51_ case 2/res_mpc_case2_velocities.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/results/TargetTrackingMPC/2023-04-29 15-17-51_ case 2/res_mpc_case2_velocities.pdf -------------------------------------------------------------------------------- /simulator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | This is based on the code of: 5 | Main simulation loop called by main.py. 6 | 7 | Author: Thor I. Fossen 8 | """ 9 | import numpy as np 10 | from python_vehicle_simulator.lib import * 11 | from tqdm import tqdm 12 | 13 | import utils 14 | 15 | 16 | # from .gnc import attitudeEuler 17 | 18 | 19 | ############################################################################### 20 | # Function printSimInfo(vehicle) 21 | ############################################################################### 22 | def printSimInfo(): 23 | """ 24 | Constructors used to define the vehicle objects as (see main.py for details): 25 | otter('headingAutopilot',psi_d,V_c,beta_c,tau_X) 26 | """ 27 | print('---------------------------------------------------------------------------------------') 28 | print('The Python Vehicle Simulator') 29 | print('---------------------------------------------------------------------------------------') 30 | print('3 - Otter unmanned surface vehicle (USV): controlled by two propellers, L = 2.0 m') 31 | 32 | print('---------------------------------------------------------------------------------------') 33 | 34 | 35 | ############################################################################### 36 | # Function printVehicleinfo(vehicle) 37 | ############################################################################### 38 | def printVehicleinfo(vehicle, sampleTime, N): 39 | print('---------------------------------------------------------------------------------------') 40 | print('%s' % (vehicle.name)) 41 | print('Length: %s m' % (vehicle.L)) 42 | print('%s' % (vehicle.controlDescription)) 43 | print('Sampling frequency: %s Hz' % round(1 / sampleTime)) 44 | print('Simulation time: %s seconds' % round(N * sampleTime)) 45 | print('---------------------------------------------------------------------------------------') 46 | 47 | 48 | ############################################################################### 49 | # Function simulate(N, sampleTime, vehicle) 50 | ############################################################################### 51 | def simulate(N, sampleTime, vehicle): 52 | DOF = 6 # degrees of freedom 53 | t = 0 # initial simulation time 54 | 55 | # Initial state vectors 56 | eta = np.array([0, 0, 0, 0, 0, 0], float) # position/attitude, user editable 57 | nu = vehicle.nu # velocity, defined by vehicle class 58 | u_actual = vehicle.u_actual # actual inputs, defined by vehicle class 59 | 60 | # Initialization of table used to store the simulation data 61 | simData = np.empty([0, 2 * DOF + 2 * vehicle.dimU], float) 62 | 63 | initial_vehicle_target = vehicle.target 64 | print(f"initial_target_pos: {initial_vehicle_target}") 65 | target_trajectory = [] 66 | tau_trajectory = [] 67 | time_compute_controller_signals = [] 68 | # Simulator for-loop 69 | for i in tqdm(range(0, N + 1)): 70 | 71 | t = i * sampleTime # simulation time 72 | # manipulate target: 73 | #print(f"t:{t}") 74 | target = utils.simulate_circular_target_motion(initial_position=initial_vehicle_target, radius=200,velocity=0.25, sim_time=t) 75 | #vehicle.target = utils.simulate_linear_target_motion() 76 | 77 | 78 | start_time = time.time() #start timing how long it takes to compute controller signals 79 | if (vehicle.controlMode == 'headingAutopilot'): 80 | u_control = vehicle.headingAutopilot(eta, nu, sampleTime) 81 | 82 | elif (vehicle.controlMode == 'TargetTrackingMPC'): 83 | #print(f"i: {i}") 84 | #print(f"i % t*0.5: {t % 0.5}") 85 | if (t % 0.5) == 0 or t ==0: 86 | initial_states = [eta[0], eta[1], eta[5], nu[0], nu[1], nu[5]] 87 | # print(initial_states) 88 | 89 | u_control, tau = vehicle.target_tracking_mpc(initial_state=initial_states, 90 | # 3DOF equation of motion based MPC, therefore the limited parts 91 | target=target) # must be a [x,y], if None target from vehicle is used 92 | 93 | # this is not working as it should, please test the RL-model from the file: AiControllerTesting.py 94 | elif (vehicle.controlMode == 'TargetTrackingAI'): 95 | initial_states = [eta[0], eta[1], eta[5], nu[0], nu[1], nu[5]] 96 | u_control = vehicle.target_tracking_ai_controller(initial_states = initial_states, target = np.array(target) ,sample_time=sampleTime, t= t ) 97 | 98 | 99 | elif (vehicle.controlMode == 'manual'): 100 | u_control = [-90, 90] 101 | #print(f"manual controls:{u_control}") 102 | # Store simulation data in simData 103 | end_time = time.time() #end timing how long it takes to compute controller signals 104 | time_compute_controller_signals.append(end_time - start_time) #log 105 | 106 | signals = np.append(np.append(np.append(eta, nu), u_control), u_actual) # original script 107 | # signals = np.append(np.append(np.append(eta, nu), [0, 0]), [5, 5]) 108 | 109 | simData = np.vstack([simData, signals]) 110 | 111 | # Propagate vehicle and attitude dynamics 112 | [nu, u_actual] = vehicle.dynamics(eta, nu, u_actual, u_control, sampleTime) # velocity is returned 113 | # print(f"u_actual: {u_actual}") 114 | eta = attitudeEuler(eta, nu, sampleTime) # possition! 115 | # print(t) 116 | 117 | #store intermediat sim_data 118 | target_trajectory.append(target) 119 | tau_trajectory.append(tau) 120 | # Store simulation time vector 121 | time_compute_controller_signals = np.array(time_compute_controller_signals) 122 | 123 | simTime = np.arange(start=0, stop=t + sampleTime, step=sampleTime)[:, None] 124 | target_trajectory = np.array(target_trajectory) 125 | #plt.plot(target_trajectory[:,0],target_trajectory[:,1], label = 'Target trajectory') 126 | #plt.legend() 127 | #plt.show() 128 | return (simTime, simData, target_trajectory,tau_trajectory, time_compute_controller_signals) 129 | -------------------------------------------------------------------------------- /thesis.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akseljohan/Otter-USV-Target-Tracking-Controllers/12a51887ca6b06addcc9e3f01ab2f066fbccdc33/thesis.pdf -------------------------------------------------------------------------------- /utils.py: -------------------------------------------------------------------------------- 1 | import shutil 2 | import time 3 | 4 | import matplotlib 5 | # matplotlib.use('tkagg') 6 | from matplotlib import pyplot as plt 7 | from matplotlib.markers import MarkerStyle 8 | 9 | from pynput import keyboard 10 | import numpy as np 11 | import casadi as ca 12 | from python_vehicle_simulator.lib import gnc # some functions from Fossen 13 | import math 14 | 15 | from config import config 16 | 17 | plt.rcParams.update({ 18 | "font.family": "serif", 19 | 'lines.linewidth': 1}) 20 | 21 | 22 | def on_press(key): 23 | global break_program 24 | print(key) 25 | if key == keyboard.Key.end: 26 | break_program = True 27 | return False 28 | 29 | 30 | def convert_6DOFto3DOF(sixDofMatrix): 31 | return np.delete(np.delete(sixDofMatrix, (2, 3, 4), 1), (2, 3, 4), 0) 32 | 33 | 34 | def crossFlowDrags3DOF(L, B, T, nu_r): 35 | """ 36 | Based on Fossen 2021 37 | tau_crossflow = crossFlowDrag(L,B,T,nu_r) computes the cross-flow drag 38 | integrals for a marine craft using strip theory. 39 | 40 | M d/dt nu_r + C(nu_r)*nu_r + D*nu_r + g(eta) = tau + tau_crossflow 41 | """ 42 | 43 | rho = 1026 # density of water 44 | n = 20 # number of strips 45 | 46 | dx = L / 20 47 | Cd_2D = gnc.Hoerner(B, T) # 2D drag coefficient based on Hoerner's curve 48 | 49 | Yh = 0 50 | Nh = 0 51 | xL = -L / 2 52 | 53 | # print(f"T:{T}, dx: {dx}, Cd_2D: {Cd_2D}, xL:{xL} ") 54 | # print(f"initial states: Yh:{Yh}, Nh: {Nh}, xL: {xL}") 55 | 56 | v_r = nu_r[4] # relative sway velocity 57 | r = nu_r[5] # yaw rate 58 | 59 | for i in range(0, n + 1): 60 | Ucf = ca.fabs(v_r + xL * r) * (v_r + xL * r) 61 | Yh = Yh - 0.5 * rho * T * Cd_2D * Ucf * dx # sway force 62 | Nh = Nh - 0.5 * rho * T * Cd_2D * xL * Ucf * dx # yaw moment 63 | xL += dx 64 | 65 | tau_crossflow = -ca.vertcat(0, Yh, Nh) 66 | 67 | # print(f"out: Yh:{Yh}, Nh: {Nh}, xL: {xL}") 68 | return tau_crossflow 69 | 70 | 71 | # ------------------------------------------------------------------------------ 72 | def m2c(M, nu): 73 | """ 74 | C = m2c(M,nu) computes the Coriolis and centripetal matrix C from the 75 | mass matrix M and generalized velocity vector nu (Fossen 2021, Ch. 3) 76 | """ 77 | 78 | M = 0.5 * (M + M.T) # systematization of the inertia matrix 79 | 80 | # 3-DOF model (surge, sway and yaw) 81 | # C = [ 0 0 -M(2,2)*nu(2)-M(2,3)*nu(3) 82 | # 0 0 M(1,1)*nu(1) 83 | # M(2,2)*nu(2)+M(2,3)*nu(3) -M(1,1)*nu(1) 0 ] 84 | C = ca.MX.zeros(3, 3) 85 | C[0, 2] = -M[1, 1] * nu[1] - M[1, 2] * nu[2] 86 | C[1, 2] = M[0, 0] * nu[0] 87 | C[2, 0] = -C[0, 2] 88 | C[2, 1] = -C[1, 2] 89 | 90 | return C 91 | 92 | 93 | def normalize(low, high, value): 94 | """ 95 | :param low: 96 | :param high: 97 | :param value: 98 | :return: 99 | """ 100 | if high != low: 101 | return 2 * (value - low) / (high - low) - 1 102 | else: 103 | return 0 104 | 105 | 106 | def denormalize(low, high, norm_value): 107 | """ 108 | :param low: lower force value 109 | :param high: higher force value 110 | :param norm_value: 111 | :return: denormalized value 112 | """ 113 | if norm_value < 0: 114 | return -low * norm_value 115 | elif norm_value > 0: 116 | return high * norm_value 117 | # (norm_value / 2 + 0.5) * (high - low) + low 118 | return 0 119 | 120 | 121 | def get_random_target_point(max_radius): 122 | """ 123 | :param max_radius: 124 | :return: x,y 125 | """ 126 | theta = np.random.ranf() * 2 * np.pi # radom angle in radians 127 | r = max_radius * np.sqrt(np.random.ranf()) # find random length of radius within max limit 128 | 129 | x = int(r * np.cos(theta)) 130 | y = int(r * np.sin(theta)) 131 | return [x, y] 132 | 133 | 134 | def plot_veloceties_ai(trajectory, target=None, file_path=None): 135 | """ 136 | A method to be used to print the observations from AI in MarineVehivleTargetTrackingEnv.py 137 | :param trajectory: np matrix, shape = (12,n) witht he elements [[self.eta[0], # x (N) 138 | self.eta[1], # y (E) 139 | self.eta[5], # psi (angle from north) 140 | self.nu[0], # surge vel. 141 | self.nu[1], # sway vel. 142 | self.nu[5], # yaw vel. 143 | self.vehicle.target[0], # x, target 144 | self.vehicle.target[1], # y, target 145 | self.get_delta_pos_target()[0], # distance between vehicle and target in x 146 | self.get_delta_pos_target()[1], # distance between vehicle and target in y 147 | self.get_euclidean_distance(), # euclidean distance vehicle and target 148 | self.action_dot, 149 | self.get_speed_towards_target(), 150 | self.get_smalles_sign_angle(angle=(self.get_psi_d() - self.eta[5])), 151 | # the angle between the target and the heading of the vessle 152 | self.action[0], 153 | self.action[1], 154 | self.t / self.time_out, # time awareness, 155 | self.radius # aware of the region around the target 156 | ]] 157 | :param target: a trajectory with the target coordinates 158 | :return: 159 | """ 160 | 161 | fig, axis = plt.subplots(4, figsize=(10, 8)) 162 | 163 | axis[0].plot(list(range(0, trajectory.shape[1])), trajectory[12, :], label=r'$v_{x_{ref}}$') 164 | axis[0].set_ylabel('Vel. towards target') 165 | axis[0].legend() 166 | axis[1].plot(list(range(0, trajectory.shape[1])), trajectory[10, :], label=r'$d$') 167 | axis[1].set_ylabel('Euclidean dist.') 168 | axis[1].legend() 169 | axis[2].plot(list(range(0, trajectory.shape[1])), trajectory[3, :], label=r'$u$') 170 | axis[2].plot(list(range(0, trajectory.shape[1])), trajectory[4, :], label=r'$v$') 171 | axis[2].plot(list(range(0, trajectory.shape[1])), trajectory[5, :], label=r'$r$') 172 | axis[2].set_ylabel('Velocities') 173 | axis[2].legend() 174 | axis[3].plot(list(range(0, trajectory.shape[1])), trajectory[-4, :], label=r'$u_X$') 175 | axis[3].plot(list(range(0, trajectory.shape[1])), trajectory[-3, :], label=r'$u_N$') 176 | axis[3].set_ylabel('Actions') 177 | axis[3].legend() 178 | 179 | if file_path: 180 | plt.savefig(f'{file_path}/velocity.pdf', pad_inches=0.1, bbox_inches='tight') 181 | else: 182 | plt.savefig('velocity.pdf', pad_inches=0.1, bbox_inches='tight') 183 | plt.show() 184 | plt.close() 185 | 186 | 187 | def plot_veloceties(vel_matrix_3DOF, sample_time, file_path=None): 188 | """ 189 | A method to be used to print the observations from AI in MarineVehivleTargetTrackingEnv.py 190 | :param vel_matrix_3DOF: np matrix, shape = (12,n) witht he elements [[self.eta[0], # x (N) 191 | self.eta[1], # y (E) 192 | self.eta[5], # psi (angle from north) 193 | self.nu[0], # surge vel. 194 | self.nu[1], # sway vel. 195 | self.nu[5], # yaw vel. 196 | self.vehicle.target[0], # x, target 197 | self.vehicle.target[1], # y, target 198 | ]] 199 | :param action_matrix: np matrix, shape(2,n) [[left], 200 | [right]] 201 | :return: 202 | """ 203 | t = np.array(list(range(0,len(vel_matrix_3DOF[0])))) * sample_time 204 | fig, axis = plt.subplots(figsize=(10, 8)) 205 | 206 | axis.plot(t, vel_matrix_3DOF[0, :], label=r'$u$ (m/s)') 207 | #axis[0].legend() 208 | axis.plot(t, vel_matrix_3DOF[1, :], label=r'$v$ (m/s)') 209 | #axis[0].legend() 210 | axis.plot(t, vel_matrix_3DOF[2, :], label=r'$r$ (rad/s)') 211 | #axis.axhline(0, linestyle="--") 212 | axis.legend() 213 | #axis[0].set_ylabel('Velocities') 214 | #axis.set_yabel('Velocities') 215 | #plt.ylabel('Velocities') 216 | #plt.legend() 217 | 218 | if file_path: 219 | plt.savefig(f'{file_path}', pad_inches=0.1, bbox_inches='tight') 220 | else: 221 | plt.savefig('velocity.pdf', pad_inches=0.1, bbox_inches='tight') 222 | plt.show() 223 | plt.close() 224 | return fig 225 | 226 | 227 | def plot_trajectory(trajectory, target=None, file_path=None): 228 | """ 229 | Plots the trajectory of the Otter and the target 230 | :param trajectory: shape() [x,y,psi,u,v,r] 231 | :param target: 232 | :return: 233 | """ 234 | # printing x,y 235 | fi = plt.figure(figsize=(10, 8)) 236 | ax = plt.subplot() 237 | ax.scatter(y=trajectory[0, 0], x=trajectory[1, 0], marker='s', s=100) 238 | ax.plot(trajectory[1, :], trajectory[0, :], label=r'$\eta_{x,y}$') 239 | # plt.plot(trajectory[6, :], trajectory[7, :], label='target') 240 | ax.set_aspect('equal', adjustable='box') 241 | ax.set_ylabel('North') 242 | ax.set_xlabel('East') 243 | 244 | if target is not None: 245 | ax.plot(target[1, 0], target[0, 0], marker='x', ) 246 | ax.plot(target[1, :], target[0, :], label=r'$x_{ref}$', linestyle='--', color='orange') 247 | # circle = plt.Circle((target[0], target[1]), config['env']['target_confident_region'], color='r', fill=False) 248 | # fig = plt.gcf() 249 | # ax = fig.gca() 250 | # ax.add_patch(circle) 251 | 252 | for i in range(trajectory.shape[1]): 253 | if i % 1000 == 0: 254 | plt.arrow(trajectory[1, i], trajectory[0, i], dy=np.cos(trajectory[2, i]) * 2, 255 | dx=np.sin(trajectory[2, i]) * 2, length_includes_head=False, 256 | head_width=3, head_length=3) 257 | # m = MarkerStyle("triangle") 258 | # m._transform.rotate_deg(math.degrees(trajectory[2,i])) 259 | # plt.scatter(x = trajectory[0,i], y = trajectory[1,i], 260 | # marker = (4, 1, math.degrees(trajectory[2, i])+180), 261 | # color = 'blue') 262 | plt.legend() 263 | if file_path: 264 | plt.savefig(f'{file_path}', pad_inches=0.1, bbox_inches='tight') 265 | else: 266 | plt.savefig('NED.pdf', pad_inches=0.1, bbox_inches='tight') 267 | plt.show(block=False) 268 | # plt.pause(1) 269 | plt.close() 270 | 271 | 272 | def plot_error(pos_otter, pos_target, path=None, sample_time=None): 273 | """ 274 | :param pos_otter: n,2- matrix x,y values for the otter pos 275 | :param pos_target: n,2 - np. matrix containing x,y of the target 276 | :param path: the save path and filename with extension, if non provided no file is saved 277 | :param sample_time: sampleing time in the data 278 | :return: fig 279 | """ 280 | error = np.linalg.norm((pos_otter - pos_target), axis=-1, ord=2) 281 | 282 | fig = plt.figure(figsize=[10, 5]) 283 | ax = plt.subplot() 284 | 285 | ax.plot(np.array(list(range(0, len(error)))) * sample_time, error, label=r'$\||\eta_{x,y} - x_{ref}\||$') 286 | ax.set_ylabel('Error (m)') 287 | ax.set_xlabel('Time (s)') 288 | ax.axhline(0, linestyle="--") 289 | plt.legend() 290 | 291 | if path: 292 | plt.savefig(f'{path}', pad_inches=0.1, bbox_inches='tight') 293 | plt.show() 294 | plt.close() 295 | return fig 296 | 297 | 298 | def plot_controls(u_control, u_actual, sample_time, file_path=None): 299 | """ 300 | 301 | :param u_control: vector for the controls_signals of the Otter 302 | :param u_actual: vector with the actual_actuator response of the Otter 303 | :param sample_time: The sampeling time to derive the time in seconds 304 | :param file_path: filpeath and filnename for the plots to be saved 305 | :return: fig 306 | """ 307 | #fig = plt.figure() 308 | fig, ax = plt.subplots(1, 2 ,figsize=(10, 8)) 309 | t = np.array(list(range(0,len(u_control)))) * sample_time 310 | print(t.shape) 311 | ax[0].set_title('Left') 312 | ax[0].plot(t, u_control[:,0], label=f'Signal (rad/s)') 313 | ax[0].plot(t, u_actual[:,0], label=f'Actual (rad/s)') 314 | ax[0].legend(loc='lower right', bbox_to_anchor=(1.32, -0.12), 315 | fancybox=True, shadow=False, ncol=1, borderaxespad = 0.1) 316 | ax[0].set_xlabel("Time (s)") 317 | 318 | 319 | ax[1].set_title('Right') 320 | ax[1].plot(t, u_control[:,1])#, label=f'Signal (rad/s)') 321 | ax[1].plot(t, u_actual[:,1])#, label=f'Actual (rad/s)') 322 | #ax[1].legend(loc='upper right', #bbox_to_anchor=(0.5, -0.005), 323 | # fancybox=True, shadow=False, ncol=1, borderaxespad = 0.1) 324 | ax[1].set_xlabel("Time (s)") 325 | if file_path is not None: 326 | plt.savefig(f'{file_path}', pad_inches=0.1, bbox_inches='tight') 327 | 328 | return fig 329 | 330 | def plot_solv_time(solv_time_data, sample_time, file_path=None): 331 | """ 332 | :param tau: control forces 333 | :param sample_time: sample time of the collected data 334 | :param file_path: filepath and 335 | :return: 336 | """ 337 | fig, ax = plt.subplots(1, 1, figsize=(10, 8)) 338 | t = np.array(list(range(0, len(solv_time_data)))) * sample_time 339 | ax.set_ylabel("Seconds") 340 | ax.plot(t, solv_time_data) 341 | ax.set_xlabel("Time (s)") 342 | if file_path is not None: 343 | plt.savefig(f'{file_path}', pad_inches=0.1, bbox_inches='tight') 344 | 345 | return fig 346 | def plot_control_forces(tau, sample_time, file_path=None): 347 | """ 348 | :param tau: control forces 349 | :param sample_time: sample time of the collected data 350 | :param file_path: filepath and 351 | :return: 352 | """ 353 | tau = np.array(tau) 354 | print(f"plot control forces: input shape {tau.shape}") 355 | # fig = plt.figure() 356 | fig, ax = plt.subplots(2, 1, figsize=(10, 8)) 357 | t = np.array(list(range(0, len(tau)))) * sample_time 358 | print(t.shape) 359 | ax[0].set_title('Surge (X)') 360 | ax[0].plot(t, tau[:, 0]) 361 | #ax[0].legend(loc='lower right', bbox_to_anchor=(1.32, -0.12), 362 | # fancybox=True, shadow=False, ncol=1, borderaxespad=0.1) 363 | ax[0].set_xlabel("Time (s)") 364 | 365 | ax[1].set_title('Yaw (Nm)') 366 | ax[1].plot(t, tau[:, 1]) # , label=f'Signal (rad/s)') 367 | # ax[1].legend(loc='upper right', #bbox_to_anchor=(0.5, -0.005), 368 | # fancybox=True, shadow=False, ncol=1, borderaxespad = 0.1) 369 | ax[1].set_xlabel("Time (s)") 370 | if file_path is not None: 371 | plt.savefig(f'{file_path}', pad_inches=0.1, bbox_inches='tight') 372 | 373 | return fig 374 | def copy_config(destination_dir): 375 | shutil.copy(src='config/config.yml', dst=destination_dir) 376 | 377 | 378 | def simulate_circular_target_motion(initial_position, radius, velocity, sim_time): 379 | """ 380 | Simulates the position of a particle moving in a circle. 381 | 382 | Args: 383 | initial_position (Tuple[float, float]): The initial position of the particle as a tuple of x and y coordinates. 384 | radius (float): The radius of the circle. 385 | angular_velocity (float): The angular velocity of the particle. 386 | time (float): The time for which to simulate the particle position. 387 | 388 | Returns: 389 | Tuple[float, float]: new position for target. 390 | """ 391 | angular_velocity = velocity / radius 392 | angle = angular_velocity * sim_time 393 | 394 | # Calculate the new x and y coordinates 395 | x = round(initial_position[0] + math.cos(angle) * radius - radius, 4) 396 | y = round(initial_position[1] + math.sin(angle) * radius, 4) 397 | 398 | return x, y 399 | 400 | 401 | def simulate_linear_target_motion(initial_position, v_y, v_x, sim_time): 402 | """ 403 | :param initial_position: 404 | :param v_y: 405 | :param v_x: 406 | :param sim_time: 407 | :return: new position of target 408 | """ 409 | x1 = initial_position[0] + (v_x * sim_time) 410 | y1 = initial_position[1] + (v_y * sim_time) 411 | return x1, y1 412 | 413 | 414 | if __name__ == '__main__': 415 | 416 | # example using the normalize function 417 | print(f"normalize function -10, 10, value 0: {normalize(-10, 10, 0)}") 418 | low = -60 419 | high = 180 420 | value = -60 421 | print(f"normalize function low:{low}, high: {high}, value {value}: {normalize(low=low, high=high, value=value)}") 422 | print( 423 | f"norm and denorm function low: {low}, high: {high}, value {value}: {denormalize(low=low, high=high, norm_value=normalize(low=low, high=high, value=value))}") 424 | print(f"denorm function low: {low}, high: {high}, value {-0.1}: {denormalize(low=low, high=high, norm_value=0.01)}") 425 | 426 | # example using the point cloud 427 | point_cloud = [] 428 | for i in range(1000): 429 | max_radius = 300 430 | # print(get_random_target_point(max_radius)) 431 | point_cloud.append(get_random_target_point(max_radius)) 432 | point_cloud = np.array(point_cloud) 433 | print(point_cloud.shape) 434 | plt.scatter(point_cloud[:, 0], point_cloud[:, 1]) 435 | plt.show() 436 | 437 | x = 0 438 | y = 0 439 | v_y = 400 440 | v_x = 10 441 | sim_time = 1 # second 442 | x_traj = [] 443 | y_traj = [] 444 | for i in range(10): 445 | x, y = simulate_linear_target_motion([0, 0], v_y=v_y, v_x=v_x, sim_time=sim_time) 446 | print(f"x:{x}, y: {y}") 447 | x_traj.append(x) 448 | y_traj.append(y) 449 | plt.plot(x_traj, y_traj, label="Linear trajectory") 450 | plt.legend() 451 | plt.show() 452 | x_traj = [] 453 | y_traj = [] 454 | N = 100000 455 | sample_time = 0.5 456 | for i in (range(0, N + 1)): 457 | t = i * sample_time 458 | print(t) 459 | [x, y] = simulate_circular_target_motion(initial_position=[10, 10], radius=10, velocity=0.25, sim_time=t) 460 | print(f"x:{x}, y: {y}") 461 | x_traj.append(x) 462 | y_traj.append(y) 463 | 464 | print(x_traj) 465 | plt.plot(x_traj[0], y_traj[0], label="start", marker="x") 466 | plt.plot(x_traj, y_traj, label="Glider pos in the plane") 467 | plt.legend() 468 | plt.show() 469 | --------------------------------------------------------------------------------