-
Notifications
You must be signed in to change notification settings - Fork 1
/
fwvspy.py
97 lines (78 loc) · 3.39 KB
/
fwvspy.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
import sys
from importlib import reload
import controller
import yaml
import numpy as np
import random
class arguments():
def __init__(self, filename, animate, plot, initUavs):
self.filename = filename
self.animate = animate
self.plot = plot
self.initUavs = initUavs
if __name__ == '__main__':
sys.path.append("controllers/")
with open('config/initialize.yaml') as f:
params = yaml.load(f, Loader=yaml.FullLoader)
params["RobotswithPayload"]["payload"]["payloadCtrl"]["name"] = "lee"
params["simtime"] = -1.022e3
robotLoadSys = params["RobotswithPayload"]
robots = robotLoadSys["Robots"]
# for id, robot in robots.items():
# robot['q_dg'] = [random.randrange(-30, 30), random.randrange(-30, 30), random.randrange(0, 360)]
# robots[id]['q_dg'] = robot['q_dg']
args = arguments("cftrial", False, False, False)
animateOrPlotdict = {'animate':args.animate, 'plot':args.plot}
print('\nsimulation using the python script\n')
uavs_py, payload_py, ctrlInps_py, u_parpy, u_perpy = controller.main(args, animateOrPlotdict, params)
sys.path.remove("/home/khaledwahba94/imrc/pyCrazyflie/controllers")
sys.path.remove("controllers/")
del sys.modules["controller"]
del sys.modules["cffirmware"]
sys.path.append("crazyflie-firmware/")
import controller
params["RobotswithPayload"]["payload"]["payloadCtrl"]["name"] = "lee_firmware"
print('\nsimulation using the firmware\n')
uavs_fw, payload_fw, ctrlInps_fw, u_parfw, u_perfw = controller.main(args, animateOrPlotdict, params)
mu_des_py = payload_py.mu_des_stack
mu_des_fw = payload_fw.mu_des_stack
# #extract data
qi_py = payload_py.plFullState[:,6:15]
qi_fw = payload_fw.plFullState[:,6:15]
wi_py = payload_py.plFullState[:,15::]
wi_fw = payload_fw.plFullState[:,15::]
v_py = payload_py.plFullState[:,3:6]
v_fw = payload_fw.plFullState[:,3:6]
print()
# for uavpy, uavfw in zip(uavs_py.values(), uavs_fw.values()):
# for i, j in zip(range(len(uavpy.fullState)), range(len(uavfw.fullState))):
# print("statepy: ", uavpy.fullState[i,0:3])
# print("statefw: ", uavfw.fullState[j,0:3])
# for i, j in zip(range(len(v_py)), range(len(v_fw))):
# print("1 v_py: ", v_py[i])
# print("1 v_fw: ", v_fw[j])
# for i, j in zip(range(len(qi_py)), range(len(qi_fw))):
# print("1 qi_py: ", qi_py[i,0:3])
# print("1 qi_fw: ", qi_fw[j,0:3])
# print("2 qi_py: ", qi_py[i,3:6])
# print("2 qi_fw: ", qi_fw[j,3:6])
# print("3 qi_py: ", qi_py[i,6:9])
# print("3 qi_fw: ", qi_fw[j,6:9])
# print()
# for i, j in zip(range(len(wi_py)), range(len(wi_fw))):
# print("1 wi_py: ", wi_py[i,0:3])
# print("1 wi_fw: ", wi_fw[j,0:3])
# print("2 wi_py: ", wi_py[i,3:6])
# print("2 wi_fw: ", wi_fw[j,3:6])
# print("3 wi_py: ", wi_py[i,6:9])
# print("3 wi_fw: ", wi_fw[j,6:9])
# for i, j in zip(range(len(mu_des_py)), range(len(mu_des_fw))):
# print("mu_py: ", mu_des_py[i,:])
# print("mu_fw: ", mu_des_fw[j,:])
# for upar_py, upar_fw in zip(u_parpy, u_parfw):
# print('uparpy: ', upar_py)
# print('uparfw: ',upar_fw)
# print()
# for uper_py, uper_fw in zip(u_perpy, u_perfw):
# print('uperpy: ', uper_py)
# print('uperfw: ',uper_fw)