started working on optimization based controller

remote_control/casadi_opt.py

@@ -2,9 +2,12 @@ from casadi import *
 
 # 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
 
+    def solve(self, x0):
         x = SX.sym('x')
         y = SX.sym('y')
         theta = SX.sym('theta')
@@ -12,24 +15,27 @@ state = vertcat(x, y, theta)
         r = 0.03
         R = 0.05
         d = 0.02
-#r = SX.sym('r')
+
-#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
+        # 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
         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)
 
         # Fixed step Runge-Kutta 4 integrator
         M = 4  # RK4 steps per interval
-DT = T/N/M
+        DT = self.T / self.N / M
         print("DT = {}".format(DT))
         f = Function('f', [state, control], [xdot, L])
         X0 = MX.sym('X0', 3)
@@ -46,7 +52,7 @@ if runge_kutta:
                 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:
-    DT = T/N
+            DT = self.T / self.N
             k1, k1_q = f(X, U)
             X = X + DT * k1
             Q = Q + DT * k1_q
@@ -69,8 +75,8 @@ lbg = []
         ubg = []
 
         # 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
             U1k = MX.sym('U1_' + str(k), 2)
             # U2k = MX.sym('U2_' + str(k))
@@ -91,25 +97,25 @@ for k in range(N):
 
         # Create an NLP solver
         prob = {'f': J, 'x': vertcat(*w), 'g': vertcat(*g)}
-solver = nlpsol('solver', 'ipopt', prob);
+        self.solver = nlpsol('solver', 'ipopt', prob)
 
         # 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']
 
         # Plot the solution
         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])
             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 = [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
-plt.figure(1)
+        plt.figure(2)
         plt.clf()
         plt.plot(tgrid, x1_opt, '--')
         plt.plot(tgrid, x2_opt, '-')
@@ -119,17 +125,18 @@ 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
 
         # ---- 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,:]
         posy   = X[1,:]
         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
 
         # ---- objective          ---------
@@ -138,8 +145,8 @@ U = opti.variable(2,N)   # control trajectory (throttle)
         # ---- dynamic constraints --------
         #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
            k1, k1_q = f(X[:,k],         U[:,k])
            k2, k2_q = f(X[:,k]+dt/2*k1, U[:,k])
@@ -149,7 +156,7 @@ 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)
            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[:,N])
+        opti.minimize(Q[:,self.N])
 
         # ---- path constraints -----------
         #limit = lambda pos: 1-sin(2*pi*pos)/2
@@ -157,9 +164,9 @@ opti.minimize(Q[:,N])
         opti.subject_to(opti.bounded(-10,U,10)) # control is limited
 
         # ---- 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(Q[:,0]==0.0)
 
@@ -168,11 +175,12 @@ opti.subject_to(Q[:,0]==0.0)
         #opti.subject_to(X[2,:]<=4) # 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):
-    pass
+        #    opti.subject_to((X[0,k]-p[0])**2 + (X[1,k]-p[1])**2  > r**2)
+        #    pass
 
 
         # ---- initial values for solver ---
@@ -195,7 +203,7 @@ plot(sol.value(posx),label="posx")
         plot(sol.value(posy),label="posy")
         plot(sol.value(angle),label="angle")
 
-plt.figure()
+        plt.figure(3)
         plot(sol.value(posx), sol.value(posy))
         ax = plt.gca()
         circle = plt.Circle(p, r)

remote_control/position_controller.py

@@ -20,6 +20,8 @@ import matplotlib.animation as anim
 
 import time
 
+from casadi_opt import OpenLoopSolver
+
 from marker_pos_angle.msg import id_pos_angle
 
 class Robot:
@@ -59,7 +61,7 @@ def f_ode(t, x, u):
 class RemoteController:
     def __init__(self):
 
-        self.robots = [Robot(3)]
+        self.robots = [Robot(5)]
 
         self.robot_ids = {}
         for r in self.robots:
@@ -69,7 +71,7 @@ class RemoteController:
         self.rc_socket = socket.socket()
         try:
             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:
             print("could not connect to socket")
 
@@ -117,6 +119,8 @@ class RemoteController:
         plt.xlabel('x-position')
         plt.ylabel('y-position')
 
+        self.ols = OpenLoopSolver()
+
     def ani(self):
         self.ani = anim.FuncAnimation(self.fig, init_func=self.ani_init, func=self.ani_update, interval=10, blit=True)
         plt.ion()
@@ -208,7 +212,8 @@ class RemoteController:
 
             keyboard_control = False
             keyboard_control_speed_test = False
-            pid = True
+            pid = False
+            open_loop_solve = True
 
             if keyboard_control: # keyboard controller
                 events = pygame.event.get()
@@ -330,6 +335,15 @@ class RemoteController:
 
                             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):
     rospy.init_node('controller_node', anonymous=True)