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

working version of MPC controller

Browse Source
This commit is contained in:
Simon Pirkelmann 2019-06-13 13:18:46 +02:00
parent f100f21162
commit b8927cf1c5
3 changed files with 267 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
micropython_firmware/main.py
View File

@ -54,46 +54,64 @@ class Robot:
print("connected!") print("connected!")
listening = True listening = True
while listening: while listening:
# expected data: '(u1, u2)'\n" # expected data: '(t, u1, u2)'\n"
# where ui = control for motor i # where ui = control for motor i
# ui \in [-1.0, 1.0] # ui \in [-1.0, 1.0]
try: try:
data = comm_socket.readline() data = comm_socket.readline()
data_str = data.decode() data_str = data.decode()
#print("Data received: {}".format(data_str)) print("Data received: {}".format(data_str))
#print("processing data = {}".format(data_str)) print("processing data = {}".format(data_str))
l = data_str.strip('()\n').split(',') l = data_str.strip('()\n').split(',')
#print("l = {}".format(l)) print("l = {}".format(l))
u1 = int(float(l[0])*100) t = float(l[0])
#print("u1 = {}".format(u1)) print("t = {}".format(t))
u2 = int(float(l[1])*100) u1 = int(float(l[1])*100)
#print("u2 = {}".format(u2)) print("u1 = {}".format(u1))
u2 = int(float(l[2])*100)
print("u2 = {}".format(u2))
except ValueError: except ValueError:
print("ValueError: Data has wrong format.") print("ValueError: Data has wrong format.")
print("Data received: {}".format(data_str)) print("Data received: {}".format(data_str))
print("Shutting down ...") print("Shutting down ...")
u1 = u2 = 0 u1 = u2 = 0
t = 0.0
listening = False listening = False
comm_socket.close()
socket.close()
del comm_socket
del socket
print("disconnected!")
except IndexError: except IndexError:
print("IndexError: Data has wrong format.") print("IndexError: Data has wrong format.")
print("Data received: {}".format(data_str)) print("Data received: {}".format(data_str))
print("Shutting down ...") print("Shutting down ...")
u1 = u2 = 0 u1 = u2 = 0
t = 0.0
listening = False listening = False
comm_socket.close()
socket.close()
del comm_socket
del socket
print("disconnected!")
except Exception as e: except Exception as e:
print("Some other error occured") print("Some other error occured")
print("Exception: {}".format(e)) print("Exception: {}".format(e))
print("Shutting down ...") print("Shutting down ...")
u1 = u2 = 0 u1 = u2 = 0
t = 0.0
listening = False listening = False
comm_socket.close()
socket.close()
del comm_socket
del socket
print("disconnected!")
finally: finally:
self.m1.speed(u1) self.m1.speed(u1)
self.m2.speed(u2) self.m2.speed(u2)
comm_socket.close() time.sleep(t)
socket.close() self.m1.speed(0)
del comm_socket self.m2.speed(0)
del socket
print("disconnected!")
wall_e = Robot() wall_e = Robot()
wall_e.remote_control() wall_e.remote_control()

280
remote_control/casadi_opt.py
View File

@ -1,13 +1,14 @@
from casadi import * from casadi import *
import time
# 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
class OpenLoopSolver: class OpenLoopSolver:
def __init__(self, N=60, T=6.0): def __init__(self, N=20, T=2.0):
self.T = T self.T = T
self.N = N self.N = N
def solve(self, x0): def setup(self):
x = SX.sym('x') x = SX.sym('x')
y = SX.sym('y') y = SX.sym('y')
theta = SX.sym('theta') theta = SX.sym('theta')
@ -15,29 +16,31 @@ class OpenLoopSolver:
r = 0.03 r = 0.03
R = 0.05 R = 0.05
d = 0.02 d = 0.02
omega_max = 13.32
omegar = SX.sym('omegar') omegar = SX.sym('omegar')
omegal = SX.sym('omegal') omegal = SX.sym('omegal')
control = vertcat(omegar, omegal) control = vertcat(omegar, omegal)
# model equation # model equation
f1 = (r / 2 * cos(theta) - r * d / (2 * R) * sin(theta)) * omegar + (r / 2 * cos(theta) + r * d / (2 * R) * sin( f1 = (r / 2 * cos(theta) - r * d / (2 * R) * sin(theta)) * omegar * omega_max + (r / 2 * cos(theta) + r * d / (2 * R) * sin(
theta)) * omegal theta)) * omegal * omega_max
f2 = (r / 2 * sin(theta) + r * d / (2 * R) * cos(theta)) * omegar + (r / 2 * sin(theta) - r * d / (2 * R) * cos( f2 = (r / 2 * sin(theta) + r * d / (2 * R) * cos(theta)) * omegar * omega_max + (r / 2 * sin(theta) - r * d / (2 * R) * cos(
theta)) * omegal theta)) * omegal * omega_max
f3 = r / (2 * R) * omegar - r / (2 * R) * omegal f3 = -(r / (2 * R) * omegar - r / (2 * R) * omegal) * omega_max
xdot = vertcat(f1, f2, f3) xdot = vertcat(f1, f2, f3)
f = Function('f', [x, y, theta, omegar, omegal], [f1, f2, f3]) f = Function('f', [x, y, theta, omegar, omegal], [f1, f2, f3])
print("f = {}".format(f)) print("f = {}".format(f))
# cost functional # cost functional
L = x ** 2 + y ** 2 + 1e-2 * theta ** 2 + 1e-4 * (omegar ** 2 + omegal ** 2) target = (-0.0, 0.0)
L = (x-target[0]) ** 2 + (y-target[1]) ** 2 + 1e-2 * theta ** 2 + 1e-2 * (omegar ** 2 + omegal ** 2)
# Fixed step Runge-Kutta 4 integrator # Fixed step Runge-Kutta 4 integrator
M = 4 # RK4 steps per interval M = 4 # RK4 steps per interval
DT = self.T / self.N / M DT = self.T / self.N / M
print("DT = {}".format(DT)) print("DT = {}".format(DT))
f = Function('f', [state, control], [xdot, L]) self.f = Function('f', [state, control], [xdot, L])
X0 = MX.sym('X0', 3) X0 = MX.sym('X0', 3)
U = MX.sym('U', 2) U = MX.sym('U', 2)
X = X0 X = X0
@ -45,10 +48,10 @@ class OpenLoopSolver:
runge_kutta = True runge_kutta = True
if runge_kutta: if runge_kutta:
for j in range(M): for j in range(M):
k1, k1_q = f(X, U) k1, k1_q = self.f(X, U)
k2, k2_q = f(X + DT / 2 * k1, U) k2, k2_q = self.f(X + DT / 2 * k1, U)
k3, k3_q = f(X + DT / 2 * k2, U) k3, k3_q = self.f(X + DT / 2 * k2, U)
k4, k4_q = f(X + DT * k3, U) k4, k4_q = self.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:
@ -75,127 +78,180 @@ class OpenLoopSolver:
ubg = [] ubg = []
# Formulate the NLP # Formulate the NLP
Xk = MX(x0) # Xk = MX(x0)
for k in range(self.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 += [-0.5, -0.5]
ubw += [10, 10] # ubw += [0.5, 0.5]
w0 += [0, 0] # w0 += [0, 0]
#
# 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)}
self.solver = nlpsol('solver', 'ipopt', prob) # self.solver = nlpsol('solver', 'ipopt', prob)
# Solve the NLP # Solve the NLP
sol = self.solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg) if False:
w_opt = sol['x'] sol = self.solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg)
w_opt = sol['x']
# Plot the solution # Plot the solution
u_opt = w_opt u_opt = w_opt
x_opt = [x0] x_opt = [self.x0]
for k in range(self.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 = [self.T/self.N*k for k in range(self.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(2) #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 #return
# alternative solution using multiple shooting (way faster!) # alternative solution using multiple shooting (way faster!)
opti = Opti() # Optimization problem self.opti = Opti() # Optimization problem
# ---- decision variables --------- # ---- decision variables ---------
X = opti.variable(3,self.N+1) # state trajectory self.X = self.opti.variable(3,self.N+1) # state trajectory
Q = opti.variable(1,self.N+1) # state trajectory self.Q = self.opti.variable(1,self.N+1) # state trajectory
posx = X[0,:]
posy = X[1,:] self.U = self.opti.variable(2,self.N) # control trajectory (throttle)
angle = X[2,:] #T = self.opti.variable() # final time
U = opti.variable(2,self.N) # control trajectory (throttle)
#T = opti.variable() # final time
# ---- objective --------- # ---- objective ---------
#opti.minimize(T) # race in minimal time #self.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 = self.T/self.N # length of a control interval
for k in range(self.N): # loop over control intervals
# Runge-Kutta 4 integration
k1, k1_q = f(X[:,k], U[:,k])
k2, k2_q = f(X[:,k]+dt/2*k1, U[:,k])
k3, k3_q = f(X[:,k]+dt/2*k2, U[:,k])
k4, k4_q = f(X[:,k]+dt*k3, U[:,k])
x_next = X[:,k] + dt/6*(k1+2*k2+2*k3+k4)
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(Q[:,k+1]==q_next) # close the gaps
opti.minimize(Q[:,self.N])
# ---- path constraints -----------
#limit = lambda pos: 1-sin(2*pi*pos)/2
#opti.subject_to(speed<=limit(pos)) # track speed limit
opti.subject_to(opti.bounded(-10,U,10)) # control is limited
# ---- boundary conditions --------
opti.subject_to(posx[0]==x0[0]) # start at position 0 ...
opti.subject_to(posy[0]==x0[1]) # ... from stand-still
opti.subject_to(angle[0]==x0[2]) # finish line at position 1
#opti.subject_to(speed[-1]==0) # .. with speed 0
opti.subject_to(Q[:,0]==0.0)
# ---- misc. constraints ----------
#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,:]>=-2) # Time must be positive
# avoid obstacle
#r = 0.25
#p = (0.5, 0.5)
#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 --- # ---- initial values for solver ---
#opti.set_initial(speed, 1) #self.opti.set_initial(speed, 1)
#opti.set_initial(T, 1) #self.opti.set_initial(T, 1)
def solve(self, x0, target):
tstart = time.time()
x = SX.sym('x')
y = SX.sym('y')
theta = SX.sym('theta')
state = vertcat(x, y, theta)
r = 0.03
R = 0.05
d = 0.02
omega_max = 13.32
omegar = SX.sym('omegar')
omegal = SX.sym('omegal')
control = vertcat(omegar, omegal)
# model equation
f1 = (r / 2 * cos(theta) - r * d / (2 * R) * sin(theta)) * omegar * omega_max + (r / 2 * cos(theta) + r * d / (
2 * R) * sin(
theta)) * omegal * omega_max
f2 = (r / 2 * sin(theta) + r * d / (2 * R) * cos(theta)) * omegar * omega_max + (r / 2 * sin(theta) - r * d / (
2 * R) * cos(
theta)) * omegal * omega_max
f3 = -(r / (2 * R) * omegar - r / (2 * R) * omegal) * omega_max
xdot = vertcat(f1, f2, f3)
L = (x - target[0]) ** 2 + (y - target[1]) ** 2 + 1e-2 * theta ** 2 + 1e-2 * (omegar ** 2 + omegal ** 2)
self.f = Function('f', [state, control], [xdot, L])
# ---- solve NLP ------ # ---- solve NLP ------
opti.solver("ipopt") # set numerical backend
sol = opti.solve() # actual solve
#x0 = sol.value(opti.x) # set numerical backend
#lam_g0 = sol.value(opti.lam_g)
#opti.set_initial(opti.lam_g, lam_g0)
#opti.set_initial(opti.x, x0) # delete constraints
#opti.solve() self.opti.subject_to()
# add new constraints
dt = self.T / self.N # length of a control interval
for k in range(self.N): # loop over control intervals
# Runge-Kutta 4 integration
k1, k1_q = self.f(self.X[:, k], self.U[:, k])
k2, k2_q = self.f(self.X[:, k] + dt / 2 * k1, self.U[:, k])
k3, k3_q = self.f(self.X[:, k] + dt / 2 * k2, self.U[:, k])
k4, k4_q = self.f(self.X[:, k] + dt * k3, self.U[:, k])
x_next = self.X[:, k] + dt / 6 * (k1 + 2 * k2 + 2 * k3 + k4)
q_next = self.Q[:, k] + dt / 6 * (k1_q + 2 * k2_q + 2 * k3_q + k4_q)
self.opti.subject_to(self.X[:, k + 1] == x_next) # close the gaps
self.opti.subject_to(self.Q[:, k + 1] == q_next) # close the gaps
self.opti.minimize(self.Q[:, self.N])
# ---- path constraints -----------
# limit = lambda pos: 1-sin(2*pi*pos)/2
# self.opti.subject_to(speed<=limit(pos)) # track speed limit
self.opti.subject_to(self.opti.bounded(-0.5, self.U, 0.5)) # control is limited
# ---- boundary conditions --------
# self.opti.subject_to(speed[-1]==0) # .. with speed 0
self.opti.subject_to(self.Q[:, 0] == 0.0)
self.opti.solver("ipopt")
# ---- misc. constraints ----------
# self.opti.subject_to(X[1,:]>=0) # Time must be positive
# self.opti.subject_to(X[2,:]<=4) # Time must be positive
# self.opti.subject_to(X[2,:]>=-2) # Time must be positive
# avoid obstacle
# r = 0.25
# p = (0.5, 0.5)
# for k in range(self.N):
# self.opti.subject_to((X[0,k]-p[0])**2 + (X[1,k]-p[1])**2 > r**2)
# pass
posx = self.X[0, :]
posy = self.X[1, :]
angle = self.X[2, :]
self.opti.subject_to(posx[0] == x0[0]) # start at position 0 ...
self.opti.subject_to(posy[0] == x0[1]) # ... from stand-still
self.opti.subject_to(angle[0] == x0[2]) # finish line at position 1
tend = time.time()
print("setting up problem took {} seconds".format(tend - tstart))
sol = self.opti.solve() # actual solve
#x0 = sol.value(self.opti.x)
#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_1 = sol.value(self.U[0,:])
u_opt_2 = sol.value(self.U[1,:])
return (u_opt_1, u_opt_2)
#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()
from pylab import plot, step, figure, legend, show, spy from pylab import plot, step, figure, legend, show, spy
@ -206,8 +262,8 @@ class OpenLoopSolver:
plt.figure(3) plt.figure(3)
plot(sol.value(posx), sol.value(posy)) plot(sol.value(posx), sol.value(posy))
ax = plt.gca() ax = plt.gca()
circle = plt.Circle(p, r) #circle = plt.Circle(p, r)
ax.add_artist(circle) #ax.add_artist(circle)
#plot(limit(sol.value(pos)),'r--',label="speed limit") #plot(limit(sol.value(pos)),'r--',label="speed limit")
#step(range(N),sol.value(U),'k',label="throttle") #step(range(N),sol.value(U),'k',label="throttle")
legend(loc="upper left") legend(loc="upper left")

69
remote_control/position_controller.py
View File

@ -11,6 +11,7 @@ import pygame
import numpy as np import numpy as np
import socket import socket
import scipy.integrate import scipy.integrate
import copy
import threading import threading
from copy import deepcopy from copy import deepcopy
@ -118,8 +119,12 @@ class RemoteController:
self.dirs, = self.ax.plot([], []) self.dirs, = self.ax.plot([], [])
plt.xlabel('x-position') plt.xlabel('x-position')
plt.ylabel('y-position') plt.ylabel('y-position')
plt.grid()
self.ols = OpenLoopSolver() self.ols = OpenLoopSolver()
self.ols.setup()
self.target = (0.0, 0.0)
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)
@ -207,6 +212,9 @@ class RemoteController:
self.mutex.release() self.mutex.release()
def controller(self): def controller(self):
tgrid = None
us1 = None
us2 = None
print("starting control") print("starting control")
while True: while True:
@ -342,7 +350,66 @@ class RemoteController:
for event in events: for event in events:
if event.type == pygame.KEYDOWN: if event.type == pygame.KEYDOWN:
if event.key == pygame.K_UP: if event.key == pygame.K_UP:
self.ols.solve(self.xms[-1]) self.controlling = True
elif event.key == pygame.K_DOWN:
self.controlling = False
self.rc_socket.send('(0.0,0.0)\n')
elif event.key == pygame.K_0:
self.target = (0.0, 0.0)
elif event.key == pygame.K_1:
self.target = (0.5,0.5)
elif event.key == pygame.K_2:
self.target = (0.5, -0.5)
elif event.key == pygame.K_3:
self.target = (-0.5,-0.5)
elif event.key == pygame.K_4:
self.target = (-0.5,0.5)
if self.controlling:
tmpc_start = time.time()
# get measurement
last_measurement = copy.deepcopy(self.xms[-1])
res = self.ols.solve(last_measurement, self.target)
#tgrid = res[0]
us1 = res[0]
us2 = res[1]
# tt = 0
# x = last_measurement
# t_ol = np.array([tt])
# x_ol = np.array([x])
# # compute open loop prediction
# for i in range(len(us1)):
# r = scipy.integrate.ode(f_ode)
# r.set_f_params(np.array([us1[i] * 13.32, us2[i] * 13.32]))
# r.set_initial_value(x, tt)
#
# tt = tt + 0.1
# x = r.integrate(tt)
#
# t_ol = np.vstack((t_ol, tt))
# x_ol = np.vstack((x_ol, x))
#plt.figure(4)
#plt.plot(x_ol[:,0], x_ol[:,1])
#if event.key == pygame.K_DOWN:
# if tgrid is not None:
tmpc_end = time.time()
print("---------------- mpc solution took {} seconds".format(tmpc_end-tmpc_start))
for i in range(0, 1):
u1 = us1[i]
u2 = us2[i]
dt_mpc = time.time() - self.t
#if dt_mpc < 0.1:
# time.sleep(0.1 - dt_mpc)
self.rc_socket.send('({},{},{})\n'.format(0.1,u1, u2))
self.t = time.time()
time.sleep(0.01)
#
pass
def main(args): def main(args):
rospy.init_node('controller_node', anonymous=True) rospy.init_node('controller_node', anonymous=True)