plotting sim racecar

.old/cte/mpc_demo/cvxpy_mpc.py

@@ -0,0 +1,104 @@
+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

.old/cte/mpc_demo/mpc_config.py

@@ -0,0 +1,6 @@
+class Params():
+    def __init__(self):
+        self.N = 3 #number of state variables
+        self.M = 2 #number of control variables
+        self.T = 20 #Prediction Horizon
+        self.dt = 0.25 #discretization step

.old/cte/mpc_demo/mpc_demo_nosim.py

@@ -0,0 +1,207 @@
+#! /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=0.5
+SIM_START_H=0
+
+from mpc_config import Params
+P=Params()
+
+# 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]
+
+        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,3,4,6,10,13],
+                                         [0,0,2,4,3,3],1)
+
+        # Sim help vars
+        self.sim_time=0
+        self.x_history=[]
+        self.y_history=[]
+        self.h_history=[]
+        self.v_history=[]
+        self.w_history=[]
+        self.predicted=None
+
+        #Initialise plot
+        plt.style.use("ggplot")
+        self.fig = plt.figure()
+        plt.ion()
+        plt.show()
+
+    def predict_motion(self):
+        '''
+        '''
+        predicted=np.zeros(self.opt_u.shape)
+        x=self.state[0]
+        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
+            predicted[0,idx]=x
+            predicted[1,idx]=y
+
+        self.predicted = predicted
+
+    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.predict_motion()
+                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])*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+P.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")
+
+        if self.predicted is not None:
+            plt.plot(self.predicted[0,:], self.predicted[1,:], c='tab:green',
+                                                     marker=".",
+                                                     alpha=0.5,
+                                                     label="mpc opt 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.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.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:]*P.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()

.old/cte/mpc_demo/mpc_demo_pybullet.py

@@ -0,0 +1,108 @@
+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
+
+from mpc_config import Params
+P=Params()
+
+import sys
+import time
+
+import pybullet as p
+import time
+
+def get_state(robotId):
+    """
+    """
+    robPos, robOrn = p.getBasePositionAndOrientation(robotId)
+    linVel,angVel = p.getBaseVelocity(robotId)
+
+    return[robPos[0], robPos[1], p.getEulerFromQuaternion(robOrn)[2]]
+
+def set_ctrl(robotId,v,w):
+	"""
+	"""
+	L= 0.354
+	R= 0.076/2
+
+	rightWheelVelocity= (2*v+w*L)/(2*R)
+	leftWheelVelocity = (2*v-w*L)/(2*R)
+
+	p.setJointMotorControl2(robotId,0,p.VELOCITY_CONTROL,targetVelocity=leftWheelVelocity,force=1000)
+	p.setJointMotorControl2(robotId,1,p.VELOCITY_CONTROL,targetVelocity=rightWheelVelocity,force=1000)
+
+def plot(path,x_history,y_history):
+    """
+    """
+    plt.style.use("ggplot")
+    plt.figure()
+    plt.title("MPC Tracking Results")
+
+    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()
+
+def run_sim():
+    """
+    """
+    p.connect(p.GUI)
+
+    start_offset = [0,2,0]
+    start_orientation = p.getQuaternionFromEuler([0,0,0])
+    turtle = p.loadURDF("turtlebot.urdf",start_offset, start_orientation)
+    plane = p.loadURDF("plane.urdf")
+
+    p.setRealTimeSimulation(1)
+    p.setGravity(0,0,-10)
+
+    # MPC time step
+    P.dt = 0.25
+
+    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,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=[]
+
+    while (1):
+
+        state =  get_state(turtle)
+        x_history.append(state[0])
+        y_history.append(state[1])
+
+        #track path in bullet
+        p.addUserDebugLine([state[0],state[1],0],[state[0],state[1],0.5],[1,0,0])
+
+        if np.sqrt((state[0]-path[0,-1])**2+(state[1]-path[1,-1])**2)<1:
+            print("Success! Goal Reached")
+            set_ctrl(turtle,0,0)
+            plot(path,x_history,y_history)
+            p.disconnect()
+            return
+
+    	#optimization loop
+        start=time.time()
+        opt_u = optimize(state,opt_u,path)
+        elapsed=time.time()-start
+        print("CVXPY Optimization Time: {:.4f}s".format(elapsed))
+
+        set_ctrl(turtle,opt_u[0,1],opt_u[1,1])
+
+        if P.dt-elapsed>0:
+            time.sleep(P.dt-elapsed)
+
+if __name__ == '__main__':
+    run_sim()

.old/cte/mpc_demo/utils.py

@@ -0,0 +1,84 @@
+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)

mpc_demo/cvxpy_mpc.py

@@ -13,32 +13,36 @@ P=Params()
 def get_linear_model(x_bar,u_bar):
     """
     """
+    L=0.3
 
     x = x_bar[0]
     y = x_bar[1]
-    theta = x_bar[2]
+    v = x_bar[2]
+    theta = x_bar[3]
 
-    v = u_bar[0]
+    a = u_bar[0]
-    w = u_bar[1]
+    delta = u_bar[1]
 
     A = np.zeros((P.N,P.N))
-    A[0,2]=-v*np.sin(theta)
+    A[0,2]=np.cos(theta)
-    A[1,2]=v*np.cos(theta)
+    A[0,3]=-v*np.sin(theta)
+    A[1,2]=np.sin(theta)
+    A[1,3]=v*np.cos(theta)
+    A[3,2]=v*np.tan(delta)/L
     A_lin=np.eye(P.N)+P.dt*A
 
     B = np.zeros((P.N,P.M))
-    B[0,0]=np.cos(theta)
+    B[2,0]=1
-    B[1,0]=np.sin(theta)
+    B[3,1]=v/(L*np.cos(delta)**2)
-    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)
+    f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).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
+    return np.round(A_lin,4), np.round(B_lin,4), np.round(C_lin,4)
 
 
-def optimize(state,u_bar,track):
+def optimize(state,u_bar,track,ref_vel=1.):
     '''
     :param state:
     :param u_bar:
@@ -47,14 +51,15 @@ def optimize(state,u_bar,track):
     '''
 
     MAX_SPEED = 1.25
-    MIN_SPEED = 0.75
+    MAX_STEER = 1.57/2
-    MAX_STEER_SPEED = 1.57/2
+    MAX_ACC = 1.0
 
     # 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))
+    x_bar[2,0]=state[2]
 
     #prediction for linearization of costrains
     for t in range (1,P.T+1):
@@ -72,16 +77,16 @@ def optimize(state,u_bar,track):
 
     for t in range(P.T):
 
-        #cost += 30*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi
+        cost += 30*cp.sum_squares(x[3,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
+        cost += 10*cp.sum_squares(ref_vel-x[2,t]) # desired v
 
         # Actuation rate of change
         if t < (P.T - 1):
-            cost += cp.quad_form(u[:, t + 1] - u[:, t], 100*np.eye(P.M))
+            cost += cp.quad_form(u[:, t + 1] - u[:, t], 10*np.eye(P.M))
 
         # Actuation effort
-        cost += cp.quad_form( u[:, t],1*np.eye(P.M))
+        cost += cp.quad_form( u[:, t],10*np.eye(P.M))
 
         # Kinrmatics Constrains (Linearized model)
         A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])
@@ -89,9 +94,10 @@ def optimize(state,u_bar,track):
 
     # sums problem objectives and concatenates constraints.
     constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition
-    constr += [u[0, :] <= MAX_SPEED]
+    constr += [x[2, :] <= MAX_SPEED]
-    constr += [u[0, :] >= MIN_SPEED]
+    constr += [x[2, :] >= 0.0]
-    constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]
+    constr += [cp.abs(u[0, :]) <= MAX_ACC]
+    constr += [cp.abs(u[1, :]) <= MAX_STEER]
 
     # Solve
     prob = cp.Problem(cp.Minimize(cost), constr)

mpc_demo/mpc_config.py

@@ -1,6 +1,6 @@
 class Params():
     def __init__(self):
-        self.N = 3 #number of state variables
+        self.N = 4 #number of state variables
         self.M = 2 #number of control variables
-        self.T = 20 #Prediction Horizon
+        self.T = 10 #Prediction Horizon
         self.dt = 0.25 #discretization step

mpc_demo/mpc_demo_nosim.py

@@ -11,9 +11,11 @@ import sys
 import time
 
 # Robot Starting position
-SIM_START_X=0
+SIM_START_X=0.
 SIM_START_Y=0.5
-SIM_START_H=0
+SIM_START_V=0.
+SIM_START_H=0.
+L=0.3
 
 from mpc_config import Params
 P=Params()
@@ -24,24 +26,25 @@ 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]
+        self.state =  [SIM_START_X, SIM_START_Y, SIM_START_V, SIM_START_H]
 
         self.opt_u =  np.zeros((P.M,P.T))
-        self.opt_u[0,:] = 1 #m/s
+        self.opt_u[0,:] = 0.5 #m/ss
         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],
+        self.path = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],
-                                         [0,0,2,4,3,3],1)
+                             [0,0,2,4,3,3,-2,-6,-2,-2],1)
 
         # Sim help vars
         self.sim_time=0
         self.x_history=[]
         self.y_history=[]
-        self.h_history=[]
         self.v_history=[]
-        self.w_history=[]
+        self.h_history=[]
+        self.a_history=[]
+        self.d_history=[]
         self.predicted=None
 
         #Initialise plot
@@ -56,11 +59,15 @@ class MPC():
         predicted=np.zeros(self.opt_u.shape)
         x=self.state[0]
         y=self.state[1]
-        th=self.state[2]
+        v=self.state[2]
-        for idx,v,w in zip(range(len(self.opt_u[0,:])),self.opt_u[0,:],self.opt_u[1,:]):
+        th=self.state[3]
+
+        for idx,a,delta 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
+            v = v+a*P.dt
+            th = th + v*np.tan(delta)/L*P.dt
+
             predicted[0,idx]=x
             predicted[1,idx]=y
 
@@ -73,7 +80,7 @@ class MPC():
         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:
+                if np.sqrt((self.state[0]-self.path[0,-1])**2+(self.state[1]-self.path[1,-1])**2)<0.5:
                     print("Success! Goal Reached")
                     return
 
@@ -89,7 +96,7 @@ class MPC():
                 self.predict_motion()
                 self.plot_sim()
 
-    def update_sim(self,lin_v,ang_v):
+    def update_sim(self,acc,steer):
         '''
         Updates state.
 
@@ -97,18 +104,20 @@ class MPC():
         :param ang_v: float
         '''
 
-        self.state[0] = self.state[0] +lin_v*np.cos(self.state[2])*P.dt
+        self.state[0] = self.state[0] +self.state[2]*np.cos(self.state[3])*P.dt
-        self.state[1] = self.state[1] +lin_v*np.sin(self.state[2])*P.dt
+        self.state[1] = self.state[1] +self.state[2]*np.sin(self.state[3])*P.dt
-        self.state[2] = self.state[2] +ang_v*P.dt
+        self.state[2] = self.state[2] +acc*P.dt
+        self.state[3] = self.state[3] + self.state[2]*np.tan(steer)/L*P.dt
 
     def plot_sim(self):
 
         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])
+        self.v_history.append(self.state[2])
-        self.v_history.append(self.opt_u[0,1])
+        self.h_history.append(self.state[3])
-        self.w_history.append(self.opt_u[1,1])
+        self.a_history.append(self.opt_u[0,1])
+        self.d_history.append(self.opt_u[1,1])
 
         plt.clf()
 
@@ -156,16 +165,16 @@ class MPC():
 
         plt.subplot(grid[0, 2])
         #plt.title("Linear Velocity {} m/s".format(self.v_history[-1]))
-        plt.plot(self.v_history,c='tab:orange')
+        plt.plot(self.a_history,c='tab:orange')
         locs, _ = plt.xticks()
         plt.xticks(locs[1:], locs[1:]*P.dt)
-        plt.ylabel('v(t) [m/s]')
+        plt.ylabel('a(t) [m/ss]')
         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.plot(np.degrees(self.d_history),c='tab:orange')
-        plt.ylabel('w(t) [deg/s]')
+        plt.ylabel('w(t) [deg]')
         locs, _ = plt.xticks()
         plt.xticks(locs[1:], locs[1:]*P.dt)
         plt.xlabel('t [s]')
@@ -177,9 +186,9 @@ class MPC():
 
 
 def plot_car(x, y, yaw):
-    LENGTH = 1.0  # [m]
+    LENGTH = 0.3  # [m]
-    WIDTH = 0.5  # [m]
+    WIDTH = 0.1  # [m]
-    OFFSET = LENGTH/2  # [m]
+    OFFSET = LENGTH  # [m]
 
     outline = np.array([[-OFFSET, (LENGTH - OFFSET), (LENGTH - OFFSET), -OFFSET, -OFFSET],
                         [WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]])