-
Notifications
You must be signed in to change notification settings - Fork 0
/
solve_load.py
131 lines (114 loc) · 3.36 KB
/
solve_load.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
#general imports
import pdb
import copy
import scipy
import sys
import math
import numpy as np
import json
import csv
INIT = False
#function specific imports
from BCHOL import BCHOL
from buildKKT import buildKKT
#import solve
def runSolve():
"""
runSolve is the main interface that lets you execute all the
calculations to solve the LQR problem.
"""
# Prompt the user to select the file type
file_type = "json"
# Read data based on user's choice
if file_type == 'json':
file_name = "lqr_prob.json"
# file_name = "lqr_prob_256.json"
with open(file_name,'r') as file:
data = json.load(file)
# Access the top-level elements
nhorizon = data['nhorizon']
x0 = data['x0']
lqrdata = data['lqrdata']
soln = np.array(data['soln']).flatten()
# Initialize arrays for each variable
Q_list = []
R_list = []
q_list = []
r_list = []
c_list = []
A_list = []
B_list = []
d_list = []
d_list.append(x0)
# Access elements inside lqrdata (assuming it's a list of dictionaries)
for lqr in lqrdata:
index = lqr['index']
nstates =lqr['nstates']
ninputs = lqr['ninputs']
Q_list.append(lqr['Q'])
R_list.append(lqr['R'])
q_list.append(lqr['q'])
r_list.append(lqr['r'])
c_list.append(lqr['c'])
A_list.append(lqr['A'])
B_list.append(lqr['B'])
if(index!=nhorizon):
d_list.append(lqr['d'])
# Accessing solution data
"""
print("\nSolution data:")
for sol in soln:
print(sol)
"""
elif file_type == 'csv':
file_name = input("Enter the CSV file name: ")
with open(file_name,'r') as file:
reader = csv.DictReader(file)
data = [row for row in reader]
#fix the csv example later
else:
print("Invalid file type.")
#transform the lists to numpy arrays
Q =np.array([np.diag(row) for row in Q_list])
R = np.array([np.diag(row) for row in R_list])
q = np.array(q_list)
r = np.array(r_list)
A = np.array(A_list)
B = np.array(B_list)
d = np.array(d_list)
c = np.array(c_list)
depth = int(math.log2(nhorizon))
nhorizon=int(nhorizon)
nstates= int(nstates)
ninputs=int(ninputs)
#INIT looks identical to the solve_lqr.cuh, 3D array use A[i] to get the matrix
#B is transposed here - fix later
if(INIT):
for i in range(nhorizon):
print("i: ",i)
print(f"A matrix \n {A[i]}")
print(f"B matrix: \n{B[i]}")
print(f"Q matrix \n:{Q[i]}")
print(f"R matrix: \n{R[i]}")
print(f"q {q[i]}")
print(f"r {r[i]}")
print(f"d {d[i]}")
#check d0=x0
print("d0 ", d[0])
#check against KKT
KKT,kkt =buildKKT(nhorizon,ninputs, nstates,Q,R,q,r,A,B,d)
dxul = np.linalg.solve(KKT, -kkt)
print("Traditional KKT,np soln\n")
with np.printoptions(precision=4, suppress=True):
print(dxul)
#imitating calling the kernel
chol_dxul=BCHOL(nhorizon,ninputs,nstates,Q,R,q,r,A,B,d)
print("returned bchol dxul soln in the form of q,r, all lambdas later\n")
with np.printoptions(precision=4, suppress=True):
print(chol_dxul)
# #DELETE LATER
# print("soln as in Brian's code order:\n")
# for i in range(nhorizon):
# print(f"d_{i} {d[i]}") #lambdas
# print(f"q_{i} {q[i]}") #x vector
# print(f"r_{i} {r[i]}") #u vector