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

started working on optimization based controller

Browse Source
This commit is contained in:
Simon Pirkelmann 2019-06-11 17:40:39 +02:00
parent 0d14738671
commit f100f21162
2 changed files with 215 additions and 193 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

346
remote_control/casadi_opt.py
View File

@ -2,78 +2,84 @@ from casadi import *
# look at: https://github.com/casadi/casadi/blob/master/docs/examples/python/vdp_indirect_multiple_shooting.py # look at: https://github.com/casadi/casadi/blob/master/docs/examples/python/vdp_indirect_multiple_shooting.py
T = 3.0 class OpenLoopSolver:
N = 30 def __init__(self, N=60, T=6.0):
self.T = T
self.N = N
x = SX.sym('x') def solve(self, x0):
y = SX.sym('y') x = SX.sym('x')
theta = SX.sym('theta') y = SX.sym('y')
state = vertcat(x, y, theta) theta = SX.sym('theta')
r = 0.03 state = vertcat(x, y, theta)
R = 0.05 r = 0.03
d = 0.02 R = 0.05
#r = SX.sym('r') d = 0.02
#R = SX.sym('R')
#d = SX.sym('d')
omegar = SX.sym('omegar')
omegal = SX.sym('omegal')
control = vertcat(omegar, omegal)
f1 = (r/2 * cos(theta) - r*d/(2*R) * sin(theta)) * omegar + (r/2 * cos(theta) + r*d/(2*R) * sin(theta)) * omegal
f2 = (r/2 * sin(theta) + r*d/(2*R) * cos(theta)) * omegar + (r/2 * sin(theta) - r*d/(2*R) * cos(theta)) * omegal
f3 = r/(2*R) * omegar - r/(2*R) * omegal
xdot = vertcat(f1, f2, f3)
f = Function('f', [x,y,theta, omegar, omegal], [f1, f2, f3])
print("f = {}".format(f))
L = x**2 + y**2 + 1e-2 * theta**2 + 1e-4 * (omegar**2 + omegal**2) omegar = SX.sym('omegar')
omegal = SX.sym('omegal')
control = vertcat(omegar, omegal)
# Fixed step Runge-Kutta 4 integrator # model equation
M = 4 # RK4 steps per interval f1 = (r / 2 * cos(theta) - r * d / (2 * R) * sin(theta)) * omegar + (r / 2 * cos(theta) + r * d / (2 * R) * sin(
DT = T/N/M theta)) * omegal
print("DT = {}".format(DT)) f2 = (r / 2 * sin(theta) + r * d / (2 * R) * cos(theta)) * omegar + (r / 2 * sin(theta) - r * d / (2 * R) * cos(
f = Function('f', [state, control], [xdot, L]) theta)) * omegal
X0 = MX.sym('X0', 3) f3 = r / (2 * R) * omegar - r / (2 * R) * omegal
U = MX.sym('U', 2) xdot = vertcat(f1, f2, f3)
X = X0 f = Function('f', [x, y, theta, omegar, omegal], [f1, f2, f3])
Q = 0 print("f = {}".format(f))
runge_kutta = True
if runge_kutta: # cost functional
L = x ** 2 + y ** 2 + 1e-2 * theta ** 2 + 1e-4 * (omegar ** 2 + omegal ** 2)
# Fixed step Runge-Kutta 4 integrator
M = 4 # RK4 steps per interval
DT = self.T / self.N / M
print("DT = {}".format(DT))
f = Function('f', [state, control], [xdot, L])
X0 = MX.sym('X0', 3)
U = MX.sym('U', 2)
X = X0
Q = 0
runge_kutta = True
if runge_kutta:
for j in range(M): for j in range(M):
k1, k1_q = f(X, U) k1, k1_q = f(X, U)
k2, k2_q = f(X + DT/2 * k1, U) k2, k2_q = f(X + DT / 2 * k1, U)
k3, k3_q = f(X + DT/2 * k2, U) k3, k3_q = f(X + DT / 2 * k2, U)
k4, k4_q = f(X + DT * k3, U) k4, k4_q = f(X + DT * k3, U)
X=X+DT/6*(k1 +2*k2 +2*k3 +k4) X = X + DT / 6 * (k1 + 2 * k2 + 2 * k3 + k4)
Q = Q + DT/6*(k1_q + 2*k2_q + 2*k3_q + k4_q) Q = Q + DT / 6 * (k1_q + 2 * k2_q + 2 * k3_q + k4_q)
else: else:
DT = T/N DT = self.T / self.N
k1, k1_q = f(X, U) k1, k1_q = f(X, U)
X = X + DT * k1 X = X + DT * k1
Q = Q + DT * k1_q Q = Q + DT * k1_q
F = Function('F', [X0, U], [X, Q],['x0','p'],['xf','qf']) F = Function('F', [X0, U], [X, Q], ['x0', 'p'], ['xf', 'qf'])
#F_euler = Function('F_euler', [X0, U], [Xeuler, Qeuler], ['x0', 'p'], ['xf', 'qf']) # F_euler = Function('F_euler', [X0, U], [Xeuler, Qeuler], ['x0', 'p'], ['xf', 'qf'])
Fk = F(x0=[0.2,0.3, 0.0],p=[-1.1, 1.1]) Fk = F(x0=[0.2, 0.3, 0.0], p=[-1.1, 1.1])
print(Fk['xf']) print(Fk['xf'])
print(Fk['qf']) print(Fk['qf'])
# Start with an empty NLP # Start with an empty NLP
w=[] w = []
w0 = [] w0 = []
lbw = [] lbw = []
ubw = [] ubw = []
J = 0 J = 0
g=[] g = []
lbg = [] lbg = []
ubg = [] ubg = []
# Formulate the NLP # Formulate the NLP
Xk = MX([1.1, 1.1, 0.0]) Xk = MX(x0)
for k in range(N): for k in range(self.N):
# New NLP variable for the control # New NLP variable for the control
U1k = MX.sym('U1_' + str(k), 2) U1k = MX.sym('U1_' + str(k), 2)
#U2k = MX.sym('U2_' + str(k)) # U2k = MX.sym('U2_' + str(k))
w += [U1k] w += [U1k]
lbw += [-10, -10] lbw += [-10, -10]
ubw += [10, 10] ubw += [10, 10]
@ -82,64 +88,65 @@ for k in range(N):
# Integrate till the end of the interval # Integrate till the end of the interval
Fk = F(x0=Xk, p=U1k) Fk = F(x0=Xk, p=U1k)
Xk = Fk['xf'] Xk = Fk['xf']
J=J+Fk['qf'] J = J + Fk['qf']
# Add inequality constraint # Add inequality constraint
#g += [Xk[1]] # g += [Xk[1]]
#lbg += [-.0] # lbg += [-.0]
#ubg += [inf] # ubg += [inf]
# Create an NLP solver # Create an NLP solver
prob = {'f': J, 'x': vertcat(*w), 'g': vertcat(*g)} prob = {'f': J, 'x': vertcat(*w), 'g': vertcat(*g)}
solver = nlpsol('solver', 'ipopt', prob); self.solver = nlpsol('solver', 'ipopt', prob)
# Solve the NLP # Solve the NLP
sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg) sol = self.solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg)
w_opt = sol['x'] w_opt = sol['x']
# Plot the solution # Plot the solution
u_opt = w_opt u_opt = w_opt
x_opt = [[1.1, 1.1, -0.0]] x_opt = [x0]
for k in range(N): for k in range(self.N):
Fk = F(x0=x_opt[-1], p=u_opt[2*k:2*k+2]) Fk = F(x0=x_opt[-1], p=u_opt[2*k:2*k+2])
x_opt += [Fk['xf'].full()] x_opt += [Fk['xf'].full()]
x1_opt = [r[0] for r in x_opt] x1_opt = [r[0] for r in x_opt]
x2_opt = [r[1] for r in x_opt] x2_opt = [r[1] for r in x_opt]
x3_opt = [r[2] for r in x_opt] x3_opt = [r[2] for r in x_opt]
tgrid = [T/N*k for k in range(N+1)] tgrid = [self.T/self.N*k for k in range(self.N+1)]
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
plt.figure(1) plt.figure(2)
plt.clf() plt.clf()
plt.plot(tgrid, x1_opt, '--') plt.plot(tgrid, x1_opt, '--')
plt.plot(tgrid, x2_opt, '-') plt.plot(tgrid, x2_opt, '-')
plt.plot(tgrid, x3_opt, '*') plt.plot(tgrid, x3_opt, '*')
#plt.step(tgrid, vertcat(DM.nan(1), u_opt), '-.') #plt.step(tgrid, vertcat(DM.nan(1), u_opt), '-.')
plt.xlabel('t') plt.xlabel('t')
plt.legend(['x1','x2','x3','u']) plt.legend(['x1','x2','x3','u'])
plt.grid() plt.grid()
#plt.show() #plt.show()
#return
# alternative solution using multiple shooting (way faster!) # alternative solution using multiple shooting (way faster!)
opti = Opti() # Optimization problem opti = Opti() # Optimization problem
# ---- decision variables --------- # ---- decision variables ---------
X = opti.variable(3,N+1) # state trajectory X = opti.variable(3,self.N+1) # state trajectory
Q = opti.variable(1,N+1) # state trajectory Q = opti.variable(1,self.N+1) # state trajectory
posx = X[0,:] posx = X[0,:]
posy = X[1,:] posy = X[1,:]
angle = X[2,:] angle = X[2,:]
U = opti.variable(2,N) # control trajectory (throttle) U = opti.variable(2,self.N) # control trajectory (throttle)
#T = opti.variable() # final time #T = opti.variable() # final time
# ---- objective --------- # ---- objective ---------
#opti.minimize(T) # race in minimal time #opti.minimize(T) # race in minimal time
# ---- dynamic constraints -------- # ---- dynamic constraints --------
#f = lambda x,u: vertcat(f1, f2, f3) # dx/dt = f(x,u) #f = lambda x,u: vertcat(f1, f2, f3) # dx/dt = f(x,u)
dt = T/N # length of a control interval dt = self.T/self.N # length of a control interval
for k in range(N): # loop over control intervals for k in range(self.N): # loop over control intervals
# Runge-Kutta 4 integration # Runge-Kutta 4 integration
k1, k1_q = f(X[:,k], U[:,k]) k1, k1_q = f(X[:,k], U[:,k])
k2, k2_q = f(X[:,k]+dt/2*k1, U[:,k]) k2, k2_q = f(X[:,k]+dt/2*k1, U[:,k])
@ -149,76 +156,77 @@ for k in range(N): # loop over control intervals
q_next = Q[:,k] + dt/6*(k1_q + 2 * k2_q + 2 * k3_q + k4_q) q_next = Q[:,k] + dt/6*(k1_q + 2 * k2_q + 2 * k3_q + k4_q)
opti.subject_to(X[:,k+1]==x_next) # close the gaps opti.subject_to(X[:,k+1]==x_next) # close the gaps
opti.subject_to(Q[:,k+1]==q_next) # close the gaps opti.subject_to(Q[:,k+1]==q_next) # close the gaps
opti.minimize(Q[:,N]) opti.minimize(Q[:,self.N])
# ---- path constraints ----------- # ---- path constraints -----------
#limit = lambda pos: 1-sin(2*pi*pos)/2 #limit = lambda pos: 1-sin(2*pi*pos)/2
#opti.subject_to(speed<=limit(pos)) # track speed limit #opti.subject_to(speed<=limit(pos)) # track speed limit
opti.subject_to(opti.bounded(-10,U,10)) # control is limited opti.subject_to(opti.bounded(-10,U,10)) # control is limited
# ---- boundary conditions -------- # ---- boundary conditions --------
opti.subject_to(posx[0]==1.10) # start at position 0 ... opti.subject_to(posx[0]==x0[0]) # start at position 0 ...
opti.subject_to(posy[0]==1.10) # ... from stand-still opti.subject_to(posy[0]==x0[1]) # ... from stand-still
opti.subject_to(angle[0]==0.0) # finish line at position 1 opti.subject_to(angle[0]==x0[2]) # finish line at position 1
#opti.subject_to(speed[-1]==0) # .. with speed 0 #opti.subject_to(speed[-1]==0) # .. with speed 0
opti.subject_to(Q[:,0]==0.0) opti.subject_to(Q[:,0]==0.0)
# ---- misc. constraints ---------- # ---- misc. constraints ----------
#opti.subject_to(X[1,:]>=0) # Time must be positive #opti.subject_to(X[1,:]>=0) # Time must be positive
#opti.subject_to(X[2,:]<=4) # Time must be positive #opti.subject_to(X[2,:]<=4) # Time must be positive
#opti.subject_to(X[2,:]>=-2) # Time must be positive #opti.subject_to(X[2,:]>=-2) # Time must be positive
r = 0.25 # avoid obstacle
p = (0.5, 0.5) #r = 0.25
for k in range(N): #p = (0.5, 0.5)
opti.subject_to((X[0,k]-p[0])**2 + (X[1,k]-p[1])**2 > r**2) #for k in range(self.N):
# opti.subject_to((X[0,k]-p[0])**2 + (X[1,k]-p[1])**2 > r**2)
# pass
# ---- initial values for solver ---
#opti.set_initial(speed, 1)
#opti.set_initial(T, 1)
# ---- solve NLP ------
opti.solver("ipopt") # set numerical backend
sol = opti.solve() # actual solve
#x0 = sol.value(opti.x)
#lam_g0 = sol.value(opti.lam_g)
#opti.set_initial(opti.lam_g, lam_g0)
#opti.set_initial(opti.x, x0)
#opti.solve()
from pylab import plot, step, figure, legend, show, spy
plot(sol.value(posx),label="posx")
plot(sol.value(posy),label="posy")
plot(sol.value(angle),label="angle")
plt.figure(3)
plot(sol.value(posx), sol.value(posy))
ax = plt.gca()
circle = plt.Circle(p, r)
ax.add_artist(circle)
#plot(limit(sol.value(pos)),'r--',label="speed limit")
#step(range(N),sol.value(U),'k',label="throttle")
legend(loc="upper left")
plt.show()
pass pass
# linearization
# A = zeros(states.shape[0], states.shape[0])
# for i in range(f.shape[0]):
# for j in range(states.shape[0]):
# A[i,j] = diff(f[i,0], states[j])
# Alin = A.subs([(theta,0), (omegar,0), (omegal,0)])
# print("A = {}".format(Alin))
# B = zeros(f.shape[0], controls.shape[0])
# for i in range(f.shape[0]):
# for j in range(controls.shape[0]):
# B[i,j] = diff(f[i,0], controls[j])
# print("B = {}".format(B))
# dfdtheta = diff(f, theta)
#print(dfdtheta.doit())
# takeaway: linearization is not helpful, because the linearized system is not stabilizable
# ---- initial values for solver --- # -> alternative: use nonlinear control method
#opti.set_initial(speed, 1)
#opti.set_initial(T, 1)
# ---- solve NLP ------
opti.solver("ipopt") # set numerical backend
sol = opti.solve() # actual solve
#x0 = sol.value(opti.x)
#lam_g0 = sol.value(opti.lam_g)
#opti.set_initial(opti.lam_g, lam_g0)
#opti.set_initial(opti.x, x0)
#opti.solve()
from pylab import plot, step, figure, legend, show, spy
plot(sol.value(posx),label="posx")
plot(sol.value(posy),label="posy")
plot(sol.value(angle),label="angle")
plt.figure()
plot(sol.value(posx), sol.value(posy))
ax = plt.gca()
circle = plt.Circle(p, r)
ax.add_artist(circle)
#plot(limit(sol.value(pos)),'r--',label="speed limit")
#step(range(N),sol.value(U),'k',label="throttle")
legend(loc="upper left")
plt.show()
pass
# linearization
# A = zeros(states.shape[0], states.shape[0])
# for i in range(f.shape[0]):
# for j in range(states.shape[0]):
# A[i,j] = diff(f[i,0], states[j])
# Alin = A.subs([(theta,0), (omegar,0), (omegal,0)])
# print("A = {}".format(Alin))
# B = zeros(f.shape[0], controls.shape[0])
# for i in range(f.shape[0]):
# for j in range(controls.shape[0]):
# B[i,j] = diff(f[i,0], controls[j])
# print("B = {}".format(B))
# dfdtheta = diff(f, theta)
#print(dfdtheta.doit())
# takeaway: linearization is not helpful, because the linearized system is not stabilizable
# -> alternative: use nonlinear control method

20
remote_control/position_controller.py
View File

@ -20,6 +20,8 @@ import matplotlib.animation as anim
import time import time
from casadi_opt import OpenLoopSolver
from marker_pos_angle.msg import id_pos_angle from marker_pos_angle.msg import id_pos_angle
class Robot: class Robot:
@ -59,7 +61,7 @@ def f_ode(t, x, u):
class RemoteController: class RemoteController:
def __init__(self): def __init__(self):
self.robots = [Robot(3)] self.robots = [Robot(5)]
self.robot_ids = {} self.robot_ids = {}
for r in self.robots: for r in self.robots:
@ -69,7 +71,7 @@ class RemoteController:
self.rc_socket = socket.socket() self.rc_socket = socket.socket()
try: try:
pass pass
self.rc_socket.connect(('192.168.1.101', 1234)) # connect to robot self.rc_socket.connect(('192.168.1.103', 1234)) # connect to robot
except socket.error: except socket.error:
print("could not connect to socket") print("could not connect to socket")
@ -117,6 +119,8 @@ class RemoteController:
plt.xlabel('x-position') plt.xlabel('x-position')
plt.ylabel('y-position') plt.ylabel('y-position')
self.ols = OpenLoopSolver()
def ani(self): def ani(self):
self.ani = anim.FuncAnimation(self.fig, init_func=self.ani_init, func=self.ani_update, interval=10, blit=True) self.ani = anim.FuncAnimation(self.fig, init_func=self.ani_init, func=self.ani_update, interval=10, blit=True)
plt.ion() plt.ion()
@ -208,7 +212,8 @@ class RemoteController:
keyboard_control = False keyboard_control = False
keyboard_control_speed_test = False keyboard_control_speed_test = False
pid = True pid = False
open_loop_solve = True
if keyboard_control: # keyboard controller if keyboard_control: # keyboard controller
events = pygame.event.get() events = pygame.event.get()
@ -330,6 +335,15 @@ class RemoteController:
time.sleep(dt) time.sleep(dt)
elif open_loop_solve:
# open loop controller
events = pygame.event.get()
for event in events:
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_UP:
self.ols.solve(self.xms[-1])
def main(args): def main(args):
rospy.init_node('controller_node', anonymous=True) rospy.init_node('controller_node', anonymous=True)