WIP: racecar mpc

.old/tracking/mpc_demo/cvxpy_mpc.py

@@ -0,0 +1,178 @@
+import numpy as np
+np.seterr(divide='ignore', invalid='ignore')
+
+from scipy.integrate import odeint
+from scipy.interpolate import interp1d
+import cvxpy as cp
+
+def get_linear_model(x_bar,u_bar):
+    """
+    """
+
+    # Control problem statement.
+
+    N = 5 #number of state variables
+    M = 2 #number of control variables
+    T = 20 #Prediction Horizon
+    dt = 0.25 #discretization step
+
+    x = x_bar[0]
+    y = x_bar[1]
+    theta = x_bar[2]
+    psi = x_bar[3]
+    cte = x_bar[4]
+
+    v = u_bar[0]
+    w = u_bar[1]
+
+    A = np.zeros((N,N))
+    A[0,2]=-v*np.sin(theta)
+    A[1,2]=v*np.cos(theta)
+    A[4,3]=v*np.cos(-psi)
+    A_lin=np.eye(N)+dt*A
+
+    B = np.zeros((N,M))
+    B[0,0]=np.cos(theta)
+    B[1,0]=np.sin(theta)
+    B[2,1]=1
+    B[3,1]=-1
+    B[4,0]=np.sin(-psi)
+    B_lin=dt*B
+
+    f_xu=np.array([v*np.cos(theta),v*np.sin(theta),w,-w,v*np.sin(-psi)]).reshape(N,1)
+    C_lin = dt*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))
+
+    return A_lin,B_lin,C_lin
+
+def calc_err(state,path):
+    """
+    Finds psi and cte w.r.t. the closest waypoint.
+
+    :param state: array_like, state of the vehicle [x_pos, y_pos, theta]
+    :param path: array_like, reference path ((x1, x2, ...), (y1, y2, ...), (th1 ,th2, ...)]
+    :returns: (float,float)
+    """
+
+    dx = state[0]-path[0,:]
+    dy = state[1]-path[1,:]
+    dist = np.sqrt(dx**2 + dy**2)
+    nn_idx = np.argmin(dist)
+
+    try:
+        v = [path[0,nn_idx+1] - path[0,nn_idx],
+             path[1,nn_idx+1] - path[1,nn_idx]]
+        v /= np.linalg.norm(v)
+
+        d = [path[0,nn_idx] - state[0],
+             path[1,nn_idx] - state[1]]
+
+        if np.dot(d,v) > 0:
+            target_idx = nn_idx
+        else:
+            target_idx = nn_idx+1
+
+    except IndexError as e:
+        target_idx = nn_idx
+
+    path_ref_vect = [np.cos(path[2,target_idx] + np.pi / 2),
+                     np.sin(path[2,target_idx] + np.pi / 2)]
+
+    #heading error w.r.t path frame
+    psi = path[2,target_idx] - state[2]
+
+    # the cross-track error is given by the scalar projection of the car->wp vector onto the faxle versor
+    #cte = np.dot([dx[target_idx], dy[target_idx]],front_axle_vect)
+    cte = np.dot([dx[target_idx], dy[target_idx]],path_ref_vect)
+
+    return target_idx,psi,cte
+
+def optimize(starting_state,u_bar,track):
+    '''
+    :param starting_state:
+    :param u_bar:
+    :param track:
+    :returns:
+    '''
+
+    MAX_SPEED = 1.25
+    MIN_SPEED = 0.75
+    MAX_STEER_SPEED = 1.57/2
+
+    N = 5 #number of state variables
+    M = 2 #number of control variables
+    T = 20 #Prediction Horizon
+    dt = 0.25 #discretization step
+
+    #Starting Condition
+    x0 = np.zeros(N)
+    x0[0] = starting_state[0]
+    x0[1] = starting_state[1]
+    x0[2] = starting_state[2]
+    _,psi,cte = calc_err(x0,track)
+    x0[3]=psi
+    x0[4]=cte
+
+    # Prediction
+    x_bar=np.zeros((N,T+1))
+    x_bar[:,0]=x0
+
+    for t in range (1,T+1):
+        xt=x_bar[:,t-1].reshape(5,1)
+        ut=u_bar[:,t-1].reshape(2,1)
+
+        A,B,C=get_linear_model(xt,ut)
+
+        xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)
+
+        _,psi,cte = calc_err(xt_plus_one,track)
+        xt_plus_one[3]=psi
+        xt_plus_one[4]=cte
+
+        x_bar[:,t]= xt_plus_one
+
+    #CVXPY Linear MPC problem statement
+    cost = 0
+    constr = []
+    x = cp.Variable((N, T+1))
+    u = cp.Variable((M, T))
+
+    for t in range(T):
+
+        # Tracking
+        if t > 0:
+            idx,_,_ = calc_err(x_bar[:,t],track)
+            delta_x = track[:,idx]-x[0:3,t]
+            cost+= cp.quad_form(delta_x,10*np.eye(3))
+
+        # Tracking last time step
+        if t == T:
+            idx,_,_ = calc_err(x_bar[:,t],track)
+            delta_x = track[:,idx]-x[0:3,t]
+            cost+= cp.quad_form(delta_x,100*np.eye(3))
+
+        # Actuation rate of change
+        if t < (T - 1):
+            cost += cp.quad_form(u[:, t + 1] - u[:, t], 25*np.eye(M))
+
+        # Actuation effort
+        cost += cp.quad_form( u[:, t],1*np.eye(M))
+
+        # Constrains
+        A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])
+        constr += [x[:,t+1] == A*x[:,t] + B*u[:,t] + C.flatten()]
+
+    # sums problem objectives and concatenates constraints.
+    constr += [x[:,0] == x0] # starting condition
+    constr += [u[0, :] <= MAX_SPEED]
+    constr += [u[0, :] >= MIN_SPEED]
+    constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]
+
+    # Solve
+    prob = cp.Problem(cp.Minimize(cost), constr)
+    solution = prob.solve(solver=cp.ECOS, verbose=False)
+
+    #retrieved optimized U and assign to u_bar to linearize in next step
+    u_bar=np.vstack((np.array(u.value[0, :]).flatten(),
+                    (np.array(u.value[1, :]).flatten())))
+
+    return u_bar

.old/tracking/mpc_demo/mpc_demo_nosim.py

@@ -12,12 +12,9 @@ import time
 
 # Robot Starting position
 SIM_START_X=0
-SIM_START_Y=0.5
+SIM_START_Y=2
 SIM_START_H=0
 
-from mpc_config import Params
-P=Params()
-
 # Classes
 class MPC():
 
@@ -25,15 +22,19 @@ class MPC():
 
         # State for the robot mathematical model [x,y,heading]
         self.state =  [SIM_START_X, SIM_START_Y, SIM_START_H]
+        # Sim step
+        self.dt = 0.25
 
-        self.opt_u =  np.zeros((P.M,P.T))
+        # starting guess output
+        N = 5 #number of state variables
+        M = 2 #number of control variables
+        T = 20 #Prediction Horizon
+        self.opt_u =  np.zeros((M,T))
         self.opt_u[0,:] = 1 #m/s
         self.opt_u[1,:] = np.radians(0) #rad/s
 
         # Interpolated Path to follow given waypoints
-        #self.path = compute_path_from_wp([0,10,12,2,4,14],[0,0,2,10,12,12])
-        self.path = compute_path_from_wp([0,3,4,6,10,13],
-                                         [0,0,2,4,3,3],1)
+        self.path = compute_path_from_wp([0,10,12,2,4,14],[0,0,2,10,12,12])
 
         # Sim help vars
         self.sim_time=0
@@ -58,9 +59,9 @@ class MPC():
         y=self.state[1]
         th=self.state[2]
         for idx,v,w in zip(range(len(self.opt_u[0,:])),self.opt_u[0,:],self.opt_u[1,:]):
-            x = x+v*np.cos(th)*P.dt
-            y = y+v*np.sin(th)*P.dt
-            th= th +w*P.dt
+            x = x+v*np.cos(th)*self.dt
+            y = y+v*np.sin(th)*self.dt
+            th= th +w*self.dt
             predicted[0,idx]=x
             predicted[1,idx]=y
 
@@ -97,13 +98,13 @@ class MPC():
         :param ang_v: float
         '''
 
-        self.state[0] = self.state[0] +lin_v*np.cos(self.state[2])*P.dt
-        self.state[1] = self.state[1] +lin_v*np.sin(self.state[2])*P.dt
-        self.state[2] = self.state[2] +ang_v*P.dt
+        self.state[0] = self.state[0] +lin_v*np.cos(self.state[2])*self.dt
+        self.state[1] = self.state[1] +lin_v*np.sin(self.state[2])*self.dt
+        self.state[2] = self.state[2] +ang_v*self.dt
 
     def plot_sim(self):
 
-        self.sim_time = self.sim_time+P.dt
+        self.sim_time = self.sim_time+self.dt
         self.x_history.append(self.state[0])
         self.y_history.append(self.state[1])
         self.h_history.append(self.state[2])
@@ -151,14 +152,13 @@ class MPC():
         plt.yticks(np.arange(min(self.path[1,:])-1., max(self.path[1,:]+1.)+1, 1.0))
         plt.xlabel('map x')
         plt.xticks(np.arange(min(self.path[0,:])-1., max(self.path[0,:]+1.)+1, 1.0))
-        plt.axis("equal")
         #plt.legend()
 
         plt.subplot(grid[0, 2])
         #plt.title("Linear Velocity {} m/s".format(self.v_history[-1]))
         plt.plot(self.v_history,c='tab:orange')
         locs, _ = plt.xticks()
-        plt.xticks(locs[1:], locs[1:]*P.dt)
+        plt.xticks(locs[1:], locs[1:]*self.dt)
         plt.ylabel('v(t) [m/s]')
         plt.xlabel('t [s]')
 
@@ -167,7 +167,7 @@ class MPC():
         plt.plot(np.degrees(self.w_history),c='tab:orange')
         plt.ylabel('w(t) [deg/s]')
         locs, _ = plt.xticks()
-        plt.xticks(locs[1:], locs[1:]*P.dt)
+        plt.xticks(locs[1:], locs[1:]*self.dt)
         plt.xlabel('t [s]')
 
         plt.tight_layout()

.old/tracking/mpc_demo/utils.py

@@ -0,0 +1,33 @@
+import numpy as np
+from scipy.interpolate import interp1d
+
+def compute_path_from_wp(start_xp, start_yp, step = 0.1):
+    """
+    Interpolation range is computed to assure one point every fixed distance step [m].
+
+    :param start_xp: array_like, list of starting x coordinates
+    :param start_yp: array_like, list of starting y coordinates
+    :param step: float, interpolation distance [m] between consecutive waypoints
+    :returns: array_like, of shape (3,N)
+    """
+
+    final_xp=[]
+    final_yp=[]
+    delta = step #[m]
+
+    for idx in range(len(start_xp)-1):
+        section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))
+
+        interp_range = np.linspace(0,1,int(section_len/delta))
+
+        fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)
+        fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)
+
+        final_xp=np.append(final_xp,fx(interp_range))
+        final_yp=np.append(final_yp,fy(interp_range))
+
+    dx = np.append(0, np.diff(final_xp))
+    dy = np.append(0, np.diff(final_yp))
+    theta = np.arctan2(dy, dx)
+
+    return np.vstack((final_xp,final_yp,theta))

mpc_demo/.ipynb_checkpoints/main-checkpoint.py

@@ -1,184 +0,0 @@
-#! /usr/bin/env python
-
-import numpy as np
-import matplotlib.pyplot as plt
-from matplotlib import animation
-
-from utils import compute_path_from_wp
-from cvxpy_mpc import optimize
-
-import sys
-import time
-
-# Robot Starting position
-SIM_START_X=0
-SIM_START_Y=2
-SIM_START_H=0
-
-# Classes
-class MPC():
-
-    def __init__(self):
-
-        # State for the robot mathematical model [x,y,heading]
-        self.state =  [SIM_START_X, SIM_START_Y, SIM_START_H]
-        # Sim step
-        self.dt = 0.25
-
-        # starting guess output
-        N = 5 #number of state variables
-        M = 2 #number of control variables
-        T = 20 #Prediction Horizon
-        self.opt_u =  np.zeros((M,T))
-        self.opt_u[0,:] = 1 #m/s
-        self.opt_u[1,:] = np.radians(0) #rad/s
-
-        # Interpolated Path to follow given waypoints
-        self.path = compute_path_from_wp([0,10,12,2,4,14],[0,0,2,10,12,12])
-
-        # Sim help vars
-        self.sim_time=0
-        self.x_history=[]
-        self.y_history=[]
-        self.h_history=[]
-        self.v_history=[]
-        self.w_history=[]
-
-        #Initialise plot
-        plt.style.use("ggplot")
-        self.fig = plt.figure()
-        plt.ion()
-        plt.show()
-
-    def run(self):
-        '''
-        '''
-
-        while 1:
-            if self.state is not None:
-
-                if np.sqrt((self.state[0]-self.path[0,-1])**2+(self.state[1]-self.path[1,-1])**2)<1:
-                    print("Success! Goal Reached")
-                    return
-
-                #optimization loop
-                start=time.time()
-                self.opt_u = optimize(self.state,
-                                        self.opt_u,
-                                        self.path)
-
-                # print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start))
-
-                self.update_sim(self.opt_u[0,1],self.opt_u[1,1])
-
-                self.plot_sim()
-
-    def update_sim(self,lin_v,ang_v):
-        '''
-        Updates state.
-
-        :param lin_v: float
-        :param ang_v: float
-        '''
-
-        self.state[0] = self.state[0] +lin_v*np.cos(self.state[2])*self.dt
-        self.state[1] = self.state[1] +lin_v*np.sin(self.state[2])*self.dt
-        self.state[2] = self.state[2] +ang_v*self.dt
-
-    def plot_sim(self):
-
-        self.sim_time = self.sim_time+self.dt
-        self.x_history.append(self.state[0])
-        self.y_history.append(self.state[1])
-        self.h_history.append(self.state[2])
-        self.v_history.append(self.opt_u[0,1])
-        self.w_history.append(self.opt_u[1,1])
-
-        plt.clf()
-
-        grid = plt.GridSpec(2, 3)
-
-        plt.subplot(grid[0:2, 0:2])
-        plt.title("MPC Simulation \n" + "Simulation elapsed time {}s".format(self.sim_time))
-
-        plt.plot(self.path[0,:],self.path[1,:], c='tab:orange',
-                                                marker=".",
-                                                label="reference track")
-
-        plt.plot(self.x_history, self.y_history, c='tab:blue',
-                                                 marker=".",
-                                                 alpha=0.5,
-                                                 label="vehicle trajectory")
-
-        # plt.plot(self.x_history[-1], self.y_history[-1], c='tab:blue',
-        #                                                  marker=".",
-        #                                                  markersize=12,
-        #                                                  label="vehicle position")
-        # plt.arrow(self.x_history[-1],
-        #           self.y_history[-1],
-        #           np.cos(self.h_history[-1]),
-        #           np.sin(self.h_history[-1]),
-        #           color='tab:blue',
-        #           width=0.2,
-        #           head_length=0.5,
-        #           label="heading")
-
-        plot_car(self.x_history[-1], self.y_history[-1], self.h_history[-1])
-
-        plt.ylabel('map y')
-        plt.yticks(np.arange(min(self.path[1,:])-1., max(self.path[1,:]+1.)+1, 1.0))
-        plt.xlabel('map x')
-        plt.xticks(np.arange(min(self.path[0,:])-1., max(self.path[0,:]+1.)+1, 1.0))
-        #plt.legend()
-
-        plt.subplot(grid[0, 2])
-        #plt.title("Linear Velocity {} m/s".format(self.v_history[-1]))
-        plt.plot(self.v_history,c='tab:orange')
-        locs, _ = plt.xticks()
-        plt.xticks(locs[1:], locs[1:]*self.dt)
-        plt.ylabel('v(t) [m/s]')
-        plt.xlabel('t [s]')
-
-        plt.subplot(grid[1, 2])
-        #plt.title("Angular Velocity {} m/s".format(self.w_history[-1]))
-        plt.plot(np.degrees(self.w_history),c='tab:orange')
-        plt.ylabel('w(t) [deg/s]')
-        locs, _ = plt.xticks()
-        plt.xticks(locs[1:], locs[1:]*self.dt)
-        plt.xlabel('t [s]')
-
-        plt.tight_layout()
-
-        plt.draw()
-        plt.pause(0.1)
-
-
-def plot_car(x, y, yaw):
-    LENGTH = 1.0  # [m]
-    WIDTH = 0.5  # [m]
-    OFFSET = LENGTH/2  # [m]
-
-    outline = np.array([[-OFFSET, (LENGTH - OFFSET), (LENGTH - OFFSET), -OFFSET, -OFFSET],
-                        [WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]])
-
-    Rotm = np.array([[np.cos(yaw), np.sin(yaw)],
-                     [-np.sin(yaw), np.cos(yaw)]])
-
-    outline = (outline.T.dot(Rotm)).T
-
-    outline[0, :] += x
-    outline[1, :] += y
-
-    plt.plot(np.array(outline[0, :]).flatten(), np.array(outline[1, :]).flatten(), 'tab:blue')
-
-
-
-def do_sim():
-    sim=MPC()
-    try:
-        sim.run()
-    except Exception as e:
-        sys.exit(e)
-
-if __name__ == '__main__':
-    do_sim()

mpc_demo/cvxpy_mpc.py

@@ -5,90 +5,42 @@ from scipy.integrate import odeint
 from scipy.interpolate import interp1d
 import cvxpy as cp
 
+from utils import road_curve, f, df
+
+from mpc_config import Params
+P=Params()
+
 def get_linear_model(x_bar,u_bar):
     """
     """
 
-    # Control problem statement.
-
-    N = 5 #number of state variables
-    M = 2 #number of control variables
-    T = 20 #Prediction Horizon
-    dt = 0.25 #discretization step
-
     x = x_bar[0]
     y = x_bar[1]
     theta = x_bar[2]
-    psi = x_bar[3]
-    cte = x_bar[4]
 
     v = u_bar[0]
     w = u_bar[1]
 
-    A = np.zeros((N,N))
+    A = np.zeros((P.N,P.N))
     A[0,2]=-v*np.sin(theta)
     A[1,2]=v*np.cos(theta)
-    A[4,3]=v*np.cos(-psi)
-    A_lin=np.eye(N)+dt*A
+    A_lin=np.eye(P.N)+P.dt*A
 
-    B = np.zeros((N,M))
+    B = np.zeros((P.N,P.M))
     B[0,0]=np.cos(theta)
     B[1,0]=np.sin(theta)
     B[2,1]=1
-    B[3,1]=-1
-    B[4,0]=np.sin(-psi)
-    B_lin=dt*B
+    B_lin=P.dt*B
 
-    f_xu=np.array([v*np.cos(theta),v*np.sin(theta),w,-w,v*np.sin(-psi)]).reshape(N,1)
-    C_lin = dt*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))
+    f_xu=np.array([v*np.cos(theta),v*np.sin(theta),w]).reshape(P.N,1)
+    C_lin = P.dt*(f_xu - np.dot(A,x_bar.reshape(P.N,1)) - np.dot(B,u_bar.reshape(P.M,1)))
 
     return A_lin,B_lin,C_lin
 
-def calc_err(state,path):
-    """
-    Finds psi and cte w.r.t. the closest waypoint.
 
-    :param state: array_like, state of the vehicle [x_pos, y_pos, theta]
-    :param path: array_like, reference path ((x1, x2, ...), (y1, y2, ...), (th1 ,th2, ...)]
-    :returns: (float,float)
-    """
-
-    dx = state[0]-path[0,:]
-    dy = state[1]-path[1,:]
-    dist = np.sqrt(dx**2 + dy**2)
-    nn_idx = np.argmin(dist)
-
-    try:
-        v = [path[0,nn_idx+1] - path[0,nn_idx],
-             path[1,nn_idx+1] - path[1,nn_idx]]
-        v /= np.linalg.norm(v)
-
-        d = [path[0,nn_idx] - state[0],
-             path[1,nn_idx] - state[1]]
-
-        if np.dot(d,v) > 0:
-            target_idx = nn_idx
-        else:
-            target_idx = nn_idx+1
-
-    except IndexError as e:
-        target_idx = nn_idx
-
-    path_ref_vect = [np.cos(path[2,target_idx] + np.pi / 2),
-                     np.sin(path[2,target_idx] + np.pi / 2)]
-
-    #heading error w.r.t path frame
-    psi = path[2,target_idx] - state[2]
-
-    # the cross-track error is given by the scalar projection of the car->wp vector onto the faxle versor
-    #cte = np.dot([dx[target_idx], dy[target_idx]],front_axle_vect)
-    cte = np.dot([dx[target_idx], dy[target_idx]],path_ref_vect)
-
-    return target_idx,psi,cte
-
-def optimize(starting_state,u_bar,track):
+def optimize(state,u_bar,track):
     '''
-    :param starting_state:
+    :param state:
     :param u_bar:
     :param track:
     :returns:
@@ -98,78 +50,52 @@ def optimize(starting_state,u_bar,track):
     MIN_SPEED = 0.75
     MAX_STEER_SPEED = 1.57/2
 
-    N = 5 #number of state variables
-    M = 2 #number of control variables
-    T = 20 #Prediction Horizon
-    dt = 0.25 #discretization step
+    # compute polynomial coefficients of the track
+    K=road_curve(state,track)
 
-    #Starting Condition
-    x0 = np.zeros(N)
-    x0[0] = starting_state[0]
-    x0[1] = starting_state[1]
-    x0[2] = starting_state[2]
-    _,psi,cte = calc_err(x0,track)
-    x0[3]=psi
-    x0[4]=cte
-
-    # Prediction
-    x_bar=np.zeros((N,T+1))
-    x_bar[:,0]=x0
-
-    for t in range (1,T+1):
-        xt=x_bar[:,t-1].reshape(5,1)
-        ut=u_bar[:,t-1].reshape(2,1)
+    # dynamics starting state w.r.t vehicle frame
+    x_bar=np.zeros((P.N,P.T+1))
 
+    #prediction for linearization of costrains
+    for t in range (1,P.T+1):
+        xt=x_bar[:,t-1].reshape(P.N,1)
+        ut=u_bar[:,t-1].reshape(P.M,1)
         A,B,C=get_linear_model(xt,ut)
-
         xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)
-
-        _,psi,cte = calc_err(xt_plus_one,track)
-        xt_plus_one[3]=psi
-        xt_plus_one[4]=cte
-
         x_bar[:,t]= xt_plus_one
 
     #CVXPY Linear MPC problem statement
     cost = 0
     constr = []
-    x = cp.Variable((N, T+1))
-    u = cp.Variable((M, T))
+    x = cp.Variable((P.N, P.T+1))
+    u = cp.Variable((P.M, P.T))
 
-    for t in range(T):
+    for t in range(P.T):
 
-        # Tracking
-        if t > 0:
-            idx,_,_ = calc_err(x_bar[:,t],track)
-            delta_x = track[:,idx]-x[0:3,t]
-            cost+= cp.quad_form(delta_x,10*np.eye(3))
-
-        # Tracking last time step
-        if t == T:
-            idx,_,_ = calc_err(x_bar[:,t],track)
-            delta_x = track[:,idx]-x[0:3,t]
-            cost+= cp.quad_form(delta_x,100*np.eye(3))
+        #cost += 30*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi
+        cost += 50*cp.sum_squares(x[2,t]-np.arctan2(df(x_bar[0,t],K),x_bar[0,t])) # psi
+        cost += 20*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte
 
         # Actuation rate of change
-        if t < (T - 1):
-            cost += cp.quad_form(u[:, t + 1] - u[:, t], 25*np.eye(M))
+        if t < (P.T - 1):
+            cost += cp.quad_form(u[:, t + 1] - u[:, t], 100*np.eye(P.M))
 
         # Actuation effort
-        cost += cp.quad_form( u[:, t],1*np.eye(M))
+        cost += cp.quad_form( u[:, t],1*np.eye(P.M))
 
-        # Constrains
+        # Kinrmatics Constrains (Linearized model)
         A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])
-        constr += [x[:,t+1] == A*x[:,t] + B*u[:,t] + C.flatten()]
+        constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]
 
     # sums problem objectives and concatenates constraints.
-    constr += [x[:,0] == x0] # starting condition
+    constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition
     constr += [u[0, :] <= MAX_SPEED]
     constr += [u[0, :] >= MIN_SPEED]
     constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]
 
     # Solve
     prob = cp.Problem(cp.Minimize(cost), constr)
-    solution = prob.solve(solver=cp.ECOS, verbose=False)
+    solution = prob.solve(solver=cp.OSQP, verbose=False)
 
     #retrieved optimized U and assign to u_bar to linearize in next step
     u_bar=np.vstack((np.array(u.value[0, :]).flatten(),

mpc_demo/mpc_demo_nosim.py

@@ -12,9 +12,12 @@ import time
 
 # Robot Starting position
 SIM_START_X=0
-SIM_START_Y=2
+SIM_START_Y=0.5
 SIM_START_H=0
 
+from mpc_config import Params
+P=Params()
+
 # Classes
 class MPC():
 
@@ -22,19 +25,15 @@ class MPC():
 
         # State for the robot mathematical model [x,y,heading]
         self.state =  [SIM_START_X, SIM_START_Y, SIM_START_H]
-        # Sim step
-        self.dt = 0.25
 
-        # starting guess output
-        N = 5 #number of state variables
-        M = 2 #number of control variables
-        T = 20 #Prediction Horizon
-        self.opt_u =  np.zeros((M,T))
+        self.opt_u =  np.zeros((P.M,P.T))
         self.opt_u[0,:] = 1 #m/s
         self.opt_u[1,:] = np.radians(0) #rad/s
 
         # Interpolated Path to follow given waypoints
-        self.path = compute_path_from_wp([0,10,12,2,4,14],[0,0,2,10,12,12])
+        #self.path = compute_path_from_wp([0,10,12,2,4,14],[0,0,2,10,12,12])
+        self.path = compute_path_from_wp([0,3,4,6,10,13],
+                                         [0,0,2,4,3,3],1)
 
         # Sim help vars
         self.sim_time=0
@@ -59,9 +58,9 @@ class MPC():
         y=self.state[1]
         th=self.state[2]
         for idx,v,w in zip(range(len(self.opt_u[0,:])),self.opt_u[0,:],self.opt_u[1,:]):
-            x = x+v*np.cos(th)*self.dt
-            y = y+v*np.sin(th)*self.dt
-            th= th +w*self.dt
+            x = x+v*np.cos(th)*P.dt
+            y = y+v*np.sin(th)*P.dt
+            th= th +w*P.dt
             predicted[0,idx]=x
             predicted[1,idx]=y
 
@@ -98,13 +97,13 @@ class MPC():
         :param ang_v: float
         '''
 
-        self.state[0] = self.state[0] +lin_v*np.cos(self.state[2])*self.dt
-        self.state[1] = self.state[1] +lin_v*np.sin(self.state[2])*self.dt
-        self.state[2] = self.state[2] +ang_v*self.dt
+        self.state[0] = self.state[0] +lin_v*np.cos(self.state[2])*P.dt
+        self.state[1] = self.state[1] +lin_v*np.sin(self.state[2])*P.dt
+        self.state[2] = self.state[2] +ang_v*P.dt
 
     def plot_sim(self):
 
-        self.sim_time = self.sim_time+self.dt
+        self.sim_time = self.sim_time+P.dt
         self.x_history.append(self.state[0])
         self.y_history.append(self.state[1])
         self.h_history.append(self.state[2])
@@ -152,13 +151,14 @@ class MPC():
         plt.yticks(np.arange(min(self.path[1,:])-1., max(self.path[1,:]+1.)+1, 1.0))
         plt.xlabel('map x')
         plt.xticks(np.arange(min(self.path[0,:])-1., max(self.path[0,:]+1.)+1, 1.0))
+        plt.axis("equal")
         #plt.legend()
 
         plt.subplot(grid[0, 2])
         #plt.title("Linear Velocity {} m/s".format(self.v_history[-1]))
         plt.plot(self.v_history,c='tab:orange')
         locs, _ = plt.xticks()
-        plt.xticks(locs[1:], locs[1:]*self.dt)
+        plt.xticks(locs[1:], locs[1:]*P.dt)
         plt.ylabel('v(t) [m/s]')
         plt.xlabel('t [s]')
 
@@ -167,7 +167,7 @@ class MPC():
         plt.plot(np.degrees(self.w_history),c='tab:orange')
         plt.ylabel('w(t) [deg/s]')
         locs, _ = plt.xticks()
-        plt.xticks(locs[1:], locs[1:]*self.dt)
+        plt.xticks(locs[1:], locs[1:]*P.dt)
         plt.xlabel('t [s]')
 
         plt.tight_layout()

mpc_demo/mpc_demo_pybullet.py

@@ -5,6 +5,9 @@ from matplotlib import animation
 from utils import compute_path_from_wp
 from cvxpy_mpc import optimize
 
+from mpc_config import Params
+P=Params()
+
 import sys
 import time
 
@@ -40,7 +43,7 @@ def plot(path,x_history,y_history):
 
     plt.plot(path[0,:],path[1,:], c='tab:orange',marker=".",label="reference track")
     plt.plot(x_history, y_history, c='tab:blue',marker=".",alpha=0.5,label="vehicle trajectory")
-
+    plt.axis("equal")
     plt.legend()
     plt.show()
 
@@ -58,19 +61,19 @@ def run_sim():
     p.setGravity(0,0,-10)
 
     # MPC time step
-    dt = 0.25
+    P.dt = 0.25
 
-    # starting guess output
-    N = 5 #number of state variables
-    M = 2 #number of control variables
-    T = 20 #Prediction Horizon
-
-    opt_u =  np.zeros((M,T))
+    opt_u =  np.zeros((P.M,P.T))
     opt_u[0,:] = 1 #m/s
     opt_u[1,:] = np.radians(0) #rad/s
 
     # Interpolated Path to follow given waypoints
-    path = compute_path_from_wp([0,10,12,2,4,14],[0,0,2,10,12,12])
+    path = compute_path_from_wp([0,3,4,6,10,13],
+                                     [0,0,2,4,3,3],1)
+
+    for x_,y_ in zip(path[0,:],path[1,:]):
+        p.addUserDebugLine([x_,y_,0],[x_,y_,0.33],[0,0,1])
+
     x_history=[]
     y_history=[]
 
@@ -98,8 +101,8 @@ def run_sim():
 
         set_ctrl(turtle,opt_u[0,1],opt_u[1,1])
 
-        if dt-elapsed>0:
-            time.sleep(dt-elapsed)
+        if P.dt-elapsed>0:
+            time.sleep(P.dt-elapsed)
 
 if __name__ == '__main__':
     run_sim()

mpc_demo/utils.py

@@ -3,14 +3,7 @@ from scipy.interpolate import interp1d
 
 def compute_path_from_wp(start_xp, start_yp, step = 0.1):
     """
-    Interpolation range is computed to assure one point every fixed distance step [m].
-
-    :param start_xp: array_like, list of starting x coordinates
-    :param start_yp: array_like, list of starting y coordinates
-    :param step: float, interpolation distance [m] between consecutive waypoints
-    :returns: array_like, of shape (3,N)
     """
-
     final_xp=[]
     final_yp=[]
     delta = step #[m]
@@ -18,7 +11,7 @@ def compute_path_from_wp(start_xp, start_yp, step = 0.1):
     for idx in range(len(start_xp)-1):
         section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))
 
-        interp_range = np.linspace(0,1,int(section_len/delta))
+        interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))
 
         fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)
         fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)
@@ -26,8 +19,66 @@ def compute_path_from_wp(start_xp, start_yp, step = 0.1):
         final_xp=np.append(final_xp,fx(interp_range))
         final_yp=np.append(final_yp,fy(interp_range))
 
-    dx = np.append(0, np.diff(final_xp))
-    dy = np.append(0, np.diff(final_yp))
-    theta = np.arctan2(dy, dx)
+    return np.vstack((final_xp,final_yp))
 
-    return np.vstack((final_xp,final_yp,theta))
+def get_nn_idx(state,path):
+    """
+    """
+    dx = state[0]-path[0,:]
+    dy = state[1]-path[1,:]
+    dist = np.sqrt(dx**2 + dy**2)
+    nn_idx = np.argmin(dist)
+
+    try:
+        v = [path[0,nn_idx+1] - path[0,nn_idx],
+             path[1,nn_idx+1] - path[1,nn_idx]]
+        v /= np.linalg.norm(v)
+
+        d = [path[0,nn_idx] - state[0],
+             path[1,nn_idx] - state[1]]
+
+        if np.dot(d,v) > 0:
+            target_idx = nn_idx
+        else:
+            target_idx = nn_idx+1
+
+    except IndexError as e:
+        target_idx = nn_idx
+
+    return target_idx
+
+def road_curve(state,track):
+    """
+    """
+
+    POLY_RANK = 3
+
+    #given vehicle pos find lookahead waypoints
+    nn_idx=get_nn_idx(state,track)-1
+    LOOKAHED = POLY_RANK + 1
+    lk_wp=track[:,nn_idx:nn_idx+LOOKAHED]
+
+    #trasform lookahead waypoints to vehicle ref frame
+    dx = lk_wp[0,:] - state[0]
+    dy = lk_wp[1,:] - state[1]
+
+    wp_vehicle_frame = np.vstack(( dx * np.cos(-state[2]) - dy * np.sin(-state[2]),
+                                   dy * np.cos(-state[2]) + dx * np.sin(-state[2]) ))
+
+    #fit poly
+    return np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], POLY_RANK, rcond=None, full=False, w=None, cov=False)
+
+def f(x,coeff):
+    """
+    """
+    return round(coeff[0]*x**3 + coeff[1]*x**2 + coeff[2]*x**1 + coeff[3]*x**0,6)
+
+# def f(x,coeff):
+#     return  round(coeff[0]*x**5+coeff[1]*x**4+coeff[2]*x**3+coeff[3]*x**2+coeff[4]*x**1+coeff[5]*x**0,6)
+
+def df(x,coeff):
+    """
+    """
+    return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)
+# def df(x,coeff):
+#     return round(5*coeff[0]*x**4 + 4*coeff[1]*x**3 +3*coeff[2]*x**2 + 2*coeff[3]*x**1 + coeff[4]*x**0,6)

mpc_demo_v2/cvxpy_mpc.py

@@ -1,104 +0,0 @@
-import numpy as np
-np.seterr(divide='ignore', invalid='ignore')
-
-from scipy.integrate import odeint
-from scipy.interpolate import interp1d
-import cvxpy as cp
-
-from utils import road_curve, f, df
-
-from mpc_config import Params
-P=Params()
-
-def get_linear_model(x_bar,u_bar):
-    """
-    """
-
-    x = x_bar[0]
-    y = x_bar[1]
-    theta = x_bar[2]
-
-    v = u_bar[0]
-    w = u_bar[1]
-
-    A = np.zeros((P.N,P.N))
-    A[0,2]=-v*np.sin(theta)
-    A[1,2]=v*np.cos(theta)
-    A_lin=np.eye(P.N)+P.dt*A
-
-    B = np.zeros((P.N,P.M))
-    B[0,0]=np.cos(theta)
-    B[1,0]=np.sin(theta)
-    B[2,1]=1
-    B_lin=P.dt*B
-
-    f_xu=np.array([v*np.cos(theta),v*np.sin(theta),w]).reshape(P.N,1)
-    C_lin = P.dt*(f_xu - np.dot(A,x_bar.reshape(P.N,1)) - np.dot(B,u_bar.reshape(P.M,1)))
-
-    return A_lin,B_lin,C_lin
-
-
-def optimize(state,u_bar,track):
-    '''
-    :param state:
-    :param u_bar:
-    :param track:
-    :returns:
-    '''
-
-    MAX_SPEED = 1.25
-    MIN_SPEED = 0.75
-    MAX_STEER_SPEED = 1.57/2
-
-    # compute polynomial coefficients of the track
-    K=road_curve(state,track)
-
-    # dynamics starting state w.r.t vehicle frame
-    x_bar=np.zeros((P.N,P.T+1))
-
-    #prediction for linearization of costrains
-    for t in range (1,P.T+1):
-        xt=x_bar[:,t-1].reshape(P.N,1)
-        ut=u_bar[:,t-1].reshape(P.M,1)
-        A,B,C=get_linear_model(xt,ut)
-        xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)
-        x_bar[:,t]= xt_plus_one
-
-    #CVXPY Linear MPC problem statement
-    cost = 0
-    constr = []
-    x = cp.Variable((P.N, P.T+1))
-    u = cp.Variable((P.M, P.T))
-
-    for t in range(P.T):
-
-        #cost += 30*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi
-        cost += 50*cp.sum_squares(x[2,t]-np.arctan2(df(x_bar[0,t],K),x_bar[0,t])) # psi
-        cost += 20*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte
-
-        # Actuation rate of change
-        if t < (P.T - 1):
-            cost += cp.quad_form(u[:, t + 1] - u[:, t], 100*np.eye(P.M))
-
-        # Actuation effort
-        cost += cp.quad_form( u[:, t],1*np.eye(P.M))
-
-        # Kinrmatics Constrains (Linearized model)
-        A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])
-        constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]
-
-    # sums problem objectives and concatenates constraints.
-    constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition
-    constr += [u[0, :] <= MAX_SPEED]
-    constr += [u[0, :] >= MIN_SPEED]
-    constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]
-
-    # Solve
-    prob = cp.Problem(cp.Minimize(cost), constr)
-    solution = prob.solve(solver=cp.OSQP, verbose=False)
-
-    #retrieved optimized U and assign to u_bar to linearize in next step
-    u_bar=np.vstack((np.array(u.value[0, :]).flatten(),
-                    (np.array(u.value[1, :]).flatten())))
-
-    return u_bar

mpc_demo_v2/utils.py

@@ -1,84 +0,0 @@
-import numpy as np
-from scipy.interpolate import interp1d
-
-def compute_path_from_wp(start_xp, start_yp, step = 0.1):
-    """
-    """
-    final_xp=[]
-    final_yp=[]
-    delta = step #[m]
-
-    for idx in range(len(start_xp)-1):
-        section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))
-
-        interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))
-
-        fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)
-        fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)
-
-        final_xp=np.append(final_xp,fx(interp_range))
-        final_yp=np.append(final_yp,fy(interp_range))
-
-    return np.vstack((final_xp,final_yp))
-
-def get_nn_idx(state,path):
-    """
-    """
-    dx = state[0]-path[0,:]
-    dy = state[1]-path[1,:]
-    dist = np.sqrt(dx**2 + dy**2)
-    nn_idx = np.argmin(dist)
-
-    try:
-        v = [path[0,nn_idx+1] - path[0,nn_idx],
-             path[1,nn_idx+1] - path[1,nn_idx]]
-        v /= np.linalg.norm(v)
-
-        d = [path[0,nn_idx] - state[0],
-             path[1,nn_idx] - state[1]]
-
-        if np.dot(d,v) > 0:
-            target_idx = nn_idx
-        else:
-            target_idx = nn_idx+1
-
-    except IndexError as e:
-        target_idx = nn_idx
-
-    return target_idx
-
-def road_curve(state,track):
-    """
-    """
-
-    POLY_RANK = 3
-
-    #given vehicle pos find lookahead waypoints
-    nn_idx=get_nn_idx(state,track)-1
-    LOOKAHED = POLY_RANK + 1
-    lk_wp=track[:,nn_idx:nn_idx+LOOKAHED]
-
-    #trasform lookahead waypoints to vehicle ref frame
-    dx = lk_wp[0,:] - state[0]
-    dy = lk_wp[1,:] - state[1]
-
-    wp_vehicle_frame = np.vstack(( dx * np.cos(-state[2]) - dy * np.sin(-state[2]),
-                                   dy * np.cos(-state[2]) + dx * np.sin(-state[2]) ))
-
-    #fit poly
-    return np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], POLY_RANK, rcond=None, full=False, w=None, cov=False)
-
-def f(x,coeff):
-    """
-    """
-    return round(coeff[0]*x**3 + coeff[1]*x**2 + coeff[2]*x**1 + coeff[3]*x**0,6)
-
-# def f(x,coeff):
-#     return  round(coeff[0]*x**5+coeff[1]*x**4+coeff[2]*x**3+coeff[3]*x**2+coeff[4]*x**1+coeff[5]*x**0,6)
-
-def df(x,coeff):
-    """
-    """
-    return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)
-# def df(x,coeff):
-#     return round(5*coeff[0]*x**4 + 4*coeff[1]*x**3 +3*coeff[2]*x**2 + 2*coeff[3]*x**1 + coeff[4]*x**0,6)

notebooks/MPC_cte_cvxpy.ipynb

@@ -36,17 +36,19 @@
     "\n",
     "These are the differential equations f(x,u) of the model:\n",
     "\n",
+    "$\\dot{x} = f(x,u)$\n",
+    "\n",
     "* $\\dot{x} = v\\cos{\\theta}$ \n",
     "* $\\dot{y} = v\\sin{\\theta}$\n",
     "* $\\dot{\\theta} = w$\n",
     "\n",
     "Discretize with forward Euler Integration for time step dt:\n",
     "\n",
-    "${x_{t+1}} = x_{t} + f(x,u)*dt$\n",
+    "${x_{t+1}} = x_{t} + f(x,u)dt$\n",
     "\n",
-    "* ${x_{t+1}} = x_{t} + v_t\\cos{\\theta}*dt$\n",
-    "* ${y_{t+1}} = y_{t} + v_t\\sin{\\theta}*dt$\n",
-    "* ${\\theta_{t+1}} = \\theta_{t} + w_t*dt$\n",
+    "* ${x_{t+1}} = x_{t} + v_t\\cos{\\theta}dt$\n",
+    "* ${y_{t+1}} = y_{t} + v_t\\sin{\\theta}dt$\n",
+    "* ${\\theta_{t+1}} = \\theta_{t} + w_t dt$\n",
     "\n",
     "----------------------\n",
     "\n",
@@ -116,13 +118,7 @@
     "\n",
     "$ B' = dtB|_{x=\\bar{x},u=\\bar{u}} $\n",
     "\n",
-    "$ C' = dt(f(\\bar{x},\\bar{u}) - A|_{x=\\bar{x},u=\\bar{u}}\\bar{x} - B|_{x=\\bar{x},u=\\bar{u}}\\bar{u}) $\n",
-    "\n",
-    "**Errors** are:\n",
-    "* $\\psi$ heading error = $\\psi = \\theta_{ref} - \\theta$\n",
-    "* $cte$ crosstrack error = lateral distance of the robot from the track w.r.t robot frame, cte is a function of the track and x\n",
-    "\n",
-    "described by:"
+    "$ C' = dt(f(\\bar{x},\\bar{u}) - A|_{x=\\bar{x},u=\\bar{u}}\\bar{x} - B|_{x=\\bar{x},u=\\bar{u}}\\bar{u}) $"
    ]
   },
   {

racecar.py

@@ -0,0 +1,52 @@
+import pybullet as p
+import pybullet_data
+
+import time
+import os
+
+p.connect(p.GUI)
+
+p.resetSimulation()
+p.setGravity(0, 0, -10)
+
+useRealTimeSim = 1
+
+#for video recording (works best on Mac and Linux, not well on Windows)
+#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4")
+p.setRealTimeSimulation(useRealTimeSim)  # either this
+p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"))
+
+car = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "racecar/racecar.urdf"))
+for i in range(p.getNumJoints(car)):
+  print(p.getJointInfo(car, i))
+
+inactive_wheels = [3, 5, 7]
+wheels = [2]
+
+for wheel in inactive_wheels:
+  p.setJointMotorControl2(car, wheel, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
+
+steering = [4, 6]
+
+targetVelocitySlider = p.addUserDebugParameter("wheelVelocity", -10, 10, 0)
+maxForceSlider = p.addUserDebugParameter("maxForce", 0, 10, 10)
+steeringSlider = p.addUserDebugParameter("steering", -0.5, 0.5, 0)
+while (True):
+  maxForce = p.readUserDebugParameter(maxForceSlider)
+  targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
+  steeringAngle = p.readUserDebugParameter(steeringSlider)
+
+  for wheel in wheels:
+    p.setJointMotorControl2(car,
+                            wheel,
+                            p.VELOCITY_CONTROL,
+                            targetVelocity=targetVelocity,
+                            force=maxForce)
+
+  for steer in steering:
+    p.setJointMotorControl2(car, steer, p.POSITION_CONTROL, targetPosition=steeringAngle)
+
+  if (useRealTimeSim == 0):
+    p.stepSimulation()
+    
+  time.sleep(0.01)