From b8927cf1c547af4629e4831dd7874a66d67c57ff Mon Sep 17 00:00:00 2001 From: spirkelmann Date: Thu, 13 Jun 2019 13:18:46 +0200 Subject: [PATCH] working version of MPC controller --- micropython_firmware/main.py | 44 ++-- remote_control/casadi_opt.py | 280 +++++++++++++++----------- remote_control/position_controller.py | 69 ++++++- 3 files changed, 267 insertions(+), 126 deletions(-) diff --git a/micropython_firmware/main.py b/micropython_firmware/main.py index 6a72408..315eaf1 100644 --- a/micropython_firmware/main.py +++ b/micropython_firmware/main.py @@ -54,46 +54,64 @@ class Robot: print("connected!") listening = True while listening: - # expected data: '(u1, u2)'\n" + # expected data: '(t, u1, u2)'\n" # where ui = control for motor i # ui \in [-1.0, 1.0] try: data = comm_socket.readline() data_str = data.decode() - #print("Data received: {}".format(data_str)) - #print("processing data = {}".format(data_str)) + print("Data received: {}".format(data_str)) + print("processing data = {}".format(data_str)) l = data_str.strip('()\n').split(',') - #print("l = {}".format(l)) - u1 = int(float(l[0])*100) - #print("u1 = {}".format(u1)) - u2 = int(float(l[1])*100) - #print("u2 = {}".format(u2)) + print("l = {}".format(l)) + t = float(l[0]) + print("t = {}".format(t)) + u1 = int(float(l[1])*100) + print("u1 = {}".format(u1)) + u2 = int(float(l[2])*100) + print("u2 = {}".format(u2)) except ValueError: print("ValueError: Data has wrong format.") print("Data received: {}".format(data_str)) print("Shutting down ...") u1 = u2 = 0 + t = 0.0 listening = False + comm_socket.close() + socket.close() + del comm_socket + del socket + print("disconnected!") except IndexError: print("IndexError: Data has wrong format.") print("Data received: {}".format(data_str)) print("Shutting down ...") u1 = u2 = 0 + t = 0.0 listening = False + comm_socket.close() + socket.close() + del comm_socket + del socket + print("disconnected!") except Exception as e: print("Some other error occured") print("Exception: {}".format(e)) print("Shutting down ...") u1 = u2 = 0 + t = 0.0 listening = False + comm_socket.close() + socket.close() + del comm_socket + del socket + print("disconnected!") finally: self.m1.speed(u1) self.m2.speed(u2) - comm_socket.close() - socket.close() - del comm_socket - del socket - print("disconnected!") + time.sleep(t) + self.m1.speed(0) + self.m2.speed(0) wall_e = Robot() wall_e.remote_control() diff --git a/remote_control/casadi_opt.py b/remote_control/casadi_opt.py index 3d3893c..9703532 100644 --- a/remote_control/casadi_opt.py +++ b/remote_control/casadi_opt.py @@ -1,13 +1,14 @@ from casadi import * +import time # look at: https://github.com/casadi/casadi/blob/master/docs/examples/python/vdp_indirect_multiple_shooting.py class OpenLoopSolver: - def __init__(self, N=60, T=6.0): + def __init__(self, N=20, T=2.0): self.T = T self.N = N - def solve(self, x0): + def setup(self): x = SX.sym('x') y = SX.sym('y') theta = SX.sym('theta') @@ -15,29 +16,31 @@ class OpenLoopSolver: 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 + (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 + 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) f = Function('f', [x, y, theta, omegar, omegal], [f1, f2, f3]) print("f = {}".format(f)) # 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 M = 4 # RK4 steps per interval DT = self.T / self.N / M print("DT = {}".format(DT)) - f = Function('f', [state, control], [xdot, L]) + self.f = Function('f', [state, control], [xdot, L]) X0 = MX.sym('X0', 3) U = MX.sym('U', 2) X = X0 @@ -45,10 +48,10 @@ class OpenLoopSolver: runge_kutta = True if runge_kutta: for j in range(M): - k1, k1_q = f(X, U) - k2, k2_q = f(X + DT / 2 * k1, U) - k3, k3_q = f(X + DT / 2 * k2, U) - k4, k4_q = f(X + DT * k3, U) + k1, k1_q = self.f(X, U) + k2, k2_q = self.f(X + DT / 2 * k1, U) + k3, k3_q = self.f(X + DT / 2 * k2, U) + k4, k4_q = self.f(X + DT * k3, U) X = X + DT / 6 * (k1 + 2 * k2 + 2 * k3 + k4) Q = Q + DT / 6 * (k1_q + 2 * k2_q + 2 * k3_q + k4_q) else: @@ -75,127 +78,180 @@ class OpenLoopSolver: ubg = [] # Formulate the NLP - Xk = MX(x0) - for k in range(self.N): - # New NLP variable for the control - U1k = MX.sym('U1_' + str(k), 2) - # U2k = MX.sym('U2_' + str(k)) - w += [U1k] - lbw += [-10, -10] - ubw += [10, 10] - w0 += [0, 0] - - # Integrate till the end of the interval - Fk = F(x0=Xk, p=U1k) - Xk = Fk['xf'] - J = J + Fk['qf'] - - # Add inequality constraint - # g += [Xk[1]] - # lbg += [-.0] - # ubg += [inf] - - # Create an NLP solver - prob = {'f': J, 'x': vertcat(*w), 'g': vertcat(*g)} - self.solver = nlpsol('solver', 'ipopt', prob) + # Xk = MX(x0) + # for k in range(self.N): + # # New NLP variable for the control + # U1k = MX.sym('U1_' + str(k), 2) + # # U2k = MX.sym('U2_' + str(k)) + # w += [U1k] + # lbw += [-0.5, -0.5] + # ubw += [0.5, 0.5] + # w0 += [0, 0] + # + # # Integrate till the end of the interval + # Fk = F(x0=Xk, p=U1k) + # Xk = Fk['xf'] + # J = J + Fk['qf'] + # + # # Add inequality constraint + # # g += [Xk[1]] + # # lbg += [-.0] + # # ubg += [inf] + # + # # Create an NLP solver + # prob = {'f': J, 'x': vertcat(*w), 'g': vertcat(*g)} + # self.solver = nlpsol('solver', 'ipopt', prob) # Solve the NLP - sol = self.solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg) - w_opt = sol['x'] + if False: + sol = self.solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg) + w_opt = sol['x'] - # Plot the solution - u_opt = w_opt - x_opt = [x0] - for k in range(self.N): - Fk = F(x0=x_opt[-1], p=u_opt[2*k:2*k+2]) - x_opt += [Fk['xf'].full()] - x1_opt = [r[0] for r in x_opt] - x2_opt = [r[1] for r in x_opt] - x3_opt = [r[2] for r in x_opt] + # Plot the solution + u_opt = w_opt + x_opt = [self.x0] + for k in range(self.N): + Fk = F(x0=x_opt[-1], p=u_opt[2*k:2*k+2]) + x_opt += [Fk['xf'].full()] + x1_opt = [r[0] for r in x_opt] + x2_opt = [r[1] 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)] - import matplotlib.pyplot as plt - plt.figure(2) - plt.clf() - plt.plot(tgrid, x1_opt, '--') - plt.plot(tgrid, x2_opt, '-') - plt.plot(tgrid, x3_opt, '*') + #import matplotlib.pyplot as plt + #plt.figure(2) + #plt.clf() + #plt.plot(tgrid, x1_opt, '--') + #plt.plot(tgrid, x2_opt, '-') + #plt.plot(tgrid, x3_opt, '*') #plt.step(tgrid, vertcat(DM.nan(1), u_opt), '-.') - plt.xlabel('t') - plt.legend(['x1','x2','x3','u']) - plt.grid() + #plt.xlabel('t') + #plt.legend(['x1','x2','x3','u']) + #plt.grid() #plt.show() #return # alternative solution using multiple shooting (way faster!) - opti = Opti() # Optimization problem + self.opti = Opti() # Optimization problem # ---- decision variables --------- - X = opti.variable(3,self.N+1) # state trajectory - Q = opti.variable(1,self.N+1) # state trajectory - posx = X[0,:] - posy = X[1,:] - angle = X[2,:] - U = opti.variable(2,self.N) # control trajectory (throttle) - #T = opti.variable() # final time + self.X = self.opti.variable(3,self.N+1) # state trajectory + self.Q = self.opti.variable(1,self.N+1) # state trajectory + + self.U = self.opti.variable(2,self.N) # control trajectory (throttle) + #T = self.opti.variable() # final time # ---- objective --------- - #opti.minimize(T) # race in minimal time + #self.opti.minimize(T) # race in minimal time # ---- dynamic constraints -------- #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 --- - #opti.set_initial(speed, 1) - #opti.set_initial(T, 1) + #self.opti.set_initial(speed, 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 ------ - 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() + # set numerical backend + + + # delete constraints + 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 @@ -206,8 +262,8 @@ class OpenLoopSolver: plt.figure(3) plot(sol.value(posx), sol.value(posy)) ax = plt.gca() - circle = plt.Circle(p, r) - ax.add_artist(circle) + #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") diff --git a/remote_control/position_controller.py b/remote_control/position_controller.py index 1ff8298..6ca867a 100644 --- a/remote_control/position_controller.py +++ b/remote_control/position_controller.py @@ -11,6 +11,7 @@ import pygame import numpy as np import socket import scipy.integrate +import copy import threading from copy import deepcopy @@ -118,8 +119,12 @@ class RemoteController: self.dirs, = self.ax.plot([], []) plt.xlabel('x-position') plt.ylabel('y-position') + plt.grid() self.ols = OpenLoopSolver() + self.ols.setup() + + self.target = (0.0, 0.0) def ani(self): 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() def controller(self): + tgrid = None + us1 = None + us2 = None print("starting control") while True: @@ -342,7 +350,66 @@ class RemoteController: for event in events: if event.type == pygame.KEYDOWN: 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): rospy.init_node('controller_node', anonymous=True)