-
Notifications
You must be signed in to change notification settings - Fork 1
/
main.py
executable file
·197 lines (146 loc) · 5.55 KB
/
main.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
This project implements an autonomous, decentralized swarming strategies including:
- Flocking (Reynolds, Olfati-Saber, Starling, and Cao)
- Dynamic Encirclement
- Heterogeneous Pinning Control with Reinforcement Learning
- Arbitrary Closed Curves (primarily lemniscates)
- Shepherding
The strategies requires no human invervention once the target is selected and all agents rely on local knowledge only.
Each vehicle makes its own decisions about where to go based on its relative position to other vehicles.
The following agent dynamics are available:
1. double integrator
2. quadrotor helicopter (quadcopter)
Created on Tue Dec 22 11:48:18 2020
Lasted updated on Fri Feb 02 16:23 2024
@author: tjards
dev notes:
to do:
- standardize the planners (standard inputs, outputs, ...etc)
- too many conditional statements throughout
- 2D option
"""
#%% Import stuff
# --------------
# official packages
# ------------------
import numpy as np
import matplotlib.pyplot as plt
#plt.style.use('dark_background')
#plt.style.use('classic')
plt.style.use('default')
#plt.style.available
#plt.style.use('Solarize_Light2')
import json
import h5py
import os
# define data path
# ----------------
data_directory = 'data'
#file_path = os.path.join(data_directory, f"data_{formatted_date}.json")
data_file_path = os.path.join(data_directory, "data.h5")
#%% Setup Simulation
# ------------------
np.random.seed(42)
Ti = 0 # initial time
Tf = 30 # final time (later, add a condition to break out when desirable conditions are met)
Ts = 0.02 # sample time
f = 0 # parameter for future use
verbose = 1 # 1 = print progress reports, 0 = silent
system = 'swarm'
strategy = 'cao'
# reynolds = Reynolds flocking + Olfati-Saber obstacle
# saber = Olfati-Saber flocking
# starling = swarm like starlings
# circle = encirclement
# lemni = dynamic lemniscates and other closed curves
# pinning = pinning control (with RL)
# shep = shepherding
# cao = cao flocking
# save to config file
# -------------------
config_sim = {'Ti': Ti, 'Tf': Tf, 'Ts': Ts, 'verbose': 1, 'system': system, 'strategy': strategy}
with open(os.path.join("config", "config_sim.json"), 'w') as configs_sim:
json.dump(config_sim, configs_sim)
#%% build the system
# ------------------
import orchestrator
Agents, Targets, Trajectory, Obstacles, Learners = orchestrator.build_system(system, strategy)
Controller = orchestrator.Controller(Agents.tactic_type, Agents.nAgents, Agents.state)
Controller.learning_agents(Agents.tactic_type, Learners)
# pull out constants
rVeh = Agents.rVeh
tactic_type = Agents.tactic_type
dynamics = Agents.config_agents['dynamics']
#%% initialize the data store
# ---------------------------
from data import data_manager
Database = data_manager.History(Agents, Targets, Obstacles, Controller, Trajectory, Ts, Tf, Ti, f)
#%% Run Simulation
# ----------------------
t = Ti
i = 1
if verbose == 1:
print('starting simulation with ',Agents.nAgents,' agents.')
while round(t,3) < Tf:
# initialize keyword arguments
# ----------------------------
my_kwargs = {}
# Evolve the target
# -----------------
Targets.evolve(t)
# Update the obstacles (if required)
# ----------------------------------
Obstacles.evolve(Targets.targets, Agents.state, rVeh)
# Evolve the states
# -----------------
Agents.evolve(Controller.cmd, Controller.pin_matrix, t, Ts)
# Store results
# -------------
Database.update(Agents, Targets, Obstacles, Controller, Trajectory, t, f, i)
# Increment
# ---------
t += Ts
i += 1
# print progress
if verbose == 1 and (round(t,2)).is_integer():
print(round(t,1),' of ',Tf,' sec completed.')
# Compute Trajectory
# --------------------
# we'll need the record of lemni parameters
if tactic_type == 'lemni':
# only need to pass last timestep, so reduce this later
my_kwargs['lemni_all'] = Database.lemni_all
Trajectory.update(tactic_type, Agents.state, Targets.targets, t, i, **my_kwargs)
# Compute the commads (next step)
# --------------------------------
if tactic_type == 'pinning' and dynamics == 'quadcopter':
my_kwargs['quads_headings'] = Agents.quads_headings
Controller.commands(Agents.state, tactic_type, Agents.centroid, Targets.targets, Obstacles.obstacles_plus, Obstacles.walls, Trajectory.trajectory, dynamics, **my_kwargs)
#%% Save data
# -----------
if verbose == 1:
print('saving data.')
data_manager.save_data_HDF5(Database, data_file_path)
if verbose == 1:
print('done.')
#%% Produce plots
# --------------
import visualization.plot_sim as plot_sim
if verbose == 1:
print('building plots.')
plot_sim.plotMe(data_file_path)
#%% Produce animation of simulation
# ---------------------------------
import visualization.animation_sim as animation_sim
if verbose == 1:
print('building animation.')
# pull out the relevant configs
with open(os.path.join("config", "config_sim.json"), 'r') as configs_sim:
config_sim = json.load(configs_sim)
config_Ts = config_sim['Ts']
with open(os.path.join("config", "config_agents.json"), 'r') as configs_agents:
config_agents = json.load(configs_agents)
config_tactic_type = config_agents['tactic_type']
ani = animation_sim.animateMe(data_file_path, config_Ts, config_tactic_type)