Telos4/RoboRally
1
0
Fork 1
Code Issues Pull Requests Releases Wiki Activity

added warmstart

Browse Source
This commit is contained in:
Simon Pirkelmann 2019-06-13 13:45:45 +02:00
parent b8927cf1c5
commit 8548348edd
1 changed files with 11 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
remote_control/casadi_opt.py
View File

@ -8,6 +8,9 @@ class OpenLoopSolver:
self.T = T self.T = T
self.N = N self.N = N
self.opti_x0 = None
self.opti_lam_g0 = None
def setup(self): def setup(self):
x = SX.sym('x') x = SX.sym('x')
y = SX.sym('y') y = SX.sym('y')
@ -213,7 +216,8 @@ class OpenLoopSolver:
# self.opti.subject_to(speed[-1]==0) # .. with speed 0 # self.opti.subject_to(speed[-1]==0) # .. with speed 0
self.opti.subject_to(self.Q[:, 0] == 0.0) self.opti.subject_to(self.Q[:, 0] == 0.0)
self.opti.solver("ipopt")
solver = self.opti.solver("ipopt", {}, {"print_level": 0})
# ---- misc. constraints ---------- # ---- misc. constraints ----------
# self.opti.subject_to(X[1,:]>=0) # Time must be positive # self.opti.subject_to(X[1,:]>=0) # Time must be positive
@ -237,9 +241,13 @@ class OpenLoopSolver:
print("setting up problem took {} seconds".format(tend - tstart)) print("setting up problem took {} seconds".format(tend - tstart))
if self.opti_x0 is not None:
self.opti.set_initial(self.opti.lam_g, self.opti_lam_g0)
self.opti.set_initial(self.opti.x, self.opti_x0)
sol = self.opti.solve() # actual solve sol = self.opti.solve() # actual solve
#x0 = sol.value(self.opti.x) self.opti_x0 = sol.value(self.opti.x)
self.opti_lam_g0 = sol.value(self.opti.lam_g)
#u_opt_1 = map(lambda x: float(x), [u_opt[i * 2] for i in range(0, 60)]) #u_opt_1 = map(lambda x: float(x), [u_opt[i * 2] for i in range(0, 60)])
#u_opt_2 = map(lambda x: float(x), [u_opt[i * 2 + 1] for i in range(0, 60)]) #u_opt_2 = map(lambda x: float(x), [u_opt[i * 2 + 1] for i in range(0, 60)])
@ -249,8 +257,7 @@ class OpenLoopSolver:
return (u_opt_1, u_opt_2) return (u_opt_1, u_opt_2)
#lam_g0 = sol.value(self.opti.lam_g) #lam_g0 = sol.value(self.opti.lam_g)
#self.opti.set_initial(self.opti.lam_g, lam_g0)
#self.opti.set_initial(self.opti.x, x0)
#self.opti.solve() #self.opti.solve()
from pylab import plot, step, figure, legend, show, spy from pylab import plot, step, figure, legend, show, spy