updated pybullet mpc demo to latest version

mpc_pybullet_demo/cvxpy_mpc.py

@@ -5,7 +5,7 @@ from scipy.integrate import odeint
 from scipy.interpolate import interp1d
 import cvxpy as cp
 
-from utils import road_curve, f, df
+from utils import *
 
 from mpc_config import Params
 P=Params()
@@ -50,16 +50,17 @@ def optimize(state,u_bar,track,ref_vel=1.):
     :returns:
     '''
 
-    MAX_SPEED = ref_vel*1.5
+    MAX_SPEED = 1.5 #m/s
-    MAX_STEER = np.pi/4
+    MAX_ACC = 1.0 #m/ss
-    MAX_ACC = 1.0
+    MAX_D_ACC = 1.0 #m/sss
+    MAX_STEER = np.radians(30) #rad
+    MAX_D_STEER = np.radians(30) #rad/s
 
-    # compute polynomial coefficients of the track
+    REF_VEL = ref_vel #m/s
-    K=road_curve(state,track)
+    
-
+    # dynamics starting state
-    # dynamics starting state w.r.t vehicle frame
+    x_bar = np.zeros((P.N,P.T+1))
-    x_bar=np.zeros((P.N,P.T+1))
+    x_bar[:,0] = state
-    x_bar[2,0]=state[2]
 
     #prediction for linearization of costrains
     for t in range (1,P.T+1):
@@ -74,30 +75,41 @@ def optimize(state,u_bar,track,ref_vel=1.):
     constr = []
     x = cp.Variable((P.N, P.T+1))
     u = cp.Variable((P.M, P.T))
+    
+    # Cost Matrices
+    Q = np.diag([20,20,10,0]) #state error cost
+    Qf = np.diag([10,10,10,10]) #state final error cost
+    R = np.diag([10,10])       #input cost
+    R_ = np.diag([10,10])      #input rate of change cost
 
+    #Get Reference_traj
+    x_ref, d_ref = get_ref_trajectory(x_bar[:,0] ,track, REF_VEL)
+
+    #Prediction Horizon
     for t in range(P.T):
 
-        cost += 20*cp.sum_squares(x[3,t]-np.clip(np.arctan(df(x_bar[0,t],K)),-np.pi,np.pi) ) # psi
+        # Tracking Error
-        cost += 40*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte
+        cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)
-        cost += 20*cp.sum_squares(ref_vel-x[2,t]) # desired v
+
+        # Actuation effort
+        cost += cp.quad_form(u[:,t], R)
 
         # Actuation rate of change
         if t < (P.T - 1):
-            cost += cp.quad_form(u[:, t + 1] - u[:, t], 10*np.eye(P.M))
+            cost += cp.quad_form(u[:,t+1] - u[:,t], R_)
-
+            constr+= [cp.abs(u[0, t + 1] - u[0, t])/P.dt <= MAX_D_ACC]    #max acc rate of change
-        # Actuation effort
+            constr += [cp.abs(u[1, t + 1] - u[1, t])/P.dt <= MAX_D_STEER] #max steer rate of change
-        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])
+        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 += [x[:,0] == x_bar[:,0]]           #starting condition
-    constr += [x[2, :] <= MAX_SPEED]
+    constr += [x[2,:] <= MAX_SPEED]           #max speed
-    constr += [x[2, :] >= 0.0]
+    constr += [x[2,:] >= 0.0]                 #min_speed (not really needed)
-    constr += [cp.abs(u[0, :]) <= MAX_ACC]
+    constr += [cp.abs(u[0,:]) <= MAX_ACC]     #max acc
-    constr += [cp.abs(u[1, :]) <= MAX_STEER]
+    constr += [cp.abs(u[1,:]) <= MAX_STEER]   #max steer
 
     # Solve
     prob = cp.Problem(cp.Minimize(cost), constr)

mpc_pybullet_demo/mpc_config.py

@@ -4,3 +4,4 @@ class Params():
         self.M = 2 #number of control variables
         self.T = 10 #Prediction Horizon
         self.dt = 0.25 #discretization step
+        self.path_tick = 0.05

mpc_pybullet_demo/mpc_demo_nosim.py

@@ -35,7 +35,7 @@ class MPC():
         # 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,12,13,13,6,1,0],
-                                         [0,0,2,4,3,3,-1,-2,-6,-2,-2],0.5)
+                                         [0,0,2,4,3,3,-1,-2,-6,-2,-2],P.path_tick)
 
         # Sim help vars
         self.sim_time=0

mpc_pybullet_demo/mpc_demo_pybullet.py

@@ -20,7 +20,7 @@ def get_state(robotId):
     robPos, robOrn = p.getBasePositionAndOrientation(robotId)
     linVel,angVel = p.getBaseVelocity(robotId)
 
-    return[robPos[0], robPos[1], linVel[0], p.getEulerFromQuaternion(robOrn)[2]]
+    return[robPos[0], robPos[1], np.sqrt(linVel[0]**2 + linVel[1]**2), p.getEulerFromQuaternion(robOrn)[2]]
 
 def set_ctrl(robotId,currVel,acceleration,steeringAngle):
 
@@ -105,7 +105,7 @@ def run_sim():
 
     # Interpolated Path to follow given waypoints
     path = compute_path_from_wp([0,3,4,6,10,11,12,6,1,0],
-                                [0,0,2,4,3,3,-1,-6,-2,-2],0.5)
+                                [0,0,2,4,3,3,-1,-6,-2,-2],P.path_tick)
 
     for x_,y_ in zip(path[0,:],path[1,:]):
         p.addUserDebugLine([x_,y_,0],[x_,y_,0.33],[0,0,1])

mpc_pybullet_demo/utils.py

@@ -1,36 +1,49 @@
 import numpy as np
 from scipy.interpolate import interp1d
 
+from mpc_config import Params
+P=Params()
+
 def compute_path_from_wp(start_xp, start_yp, step = 0.1):
     """
+    Computes a reference path given a set of waypoints
     """
+    
     final_xp=[]
     final_yp=[]
     delta = step #[m]
 
     for idx in range(len(start_xp)-1):
-        section_len = np.sqrt(np.sum(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))
+        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(1+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)
-
+        
         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))
 
-    return np.vstack((final_xp,final_yp))
 
 def get_nn_idx(state,path):
     """
+    Computes the index of the waypoint closest to vehicle
     """
+
     dx = state[0]-path[0,:]
     dy = state[1]-path[1,:]
-    dist = np.sqrt(dx**2 + dy**2)
+    dist = np.hypot(dx,dy)
     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]]
+             path[1,nn_idx+1] - path[1,nn_idx]]   
         v /= np.linalg.norm(v)
 
         d = [path[0,nn_idx] - state[0],
@@ -46,53 +59,42 @@ def get_nn_idx(state,path):
 
     return target_idx
 
-def road_curve(state,track):
+def get_ref_trajectory(state, path, target_v):
     """
     """
+    xref = np.zeros((P.N, P.T + 1))
+    dref = np.zeros((1, P.T + 1))
+    
+    #sp = np.ones((1,T +1))*target_v #speed profile
+    
+    ncourse = path.shape[1]
 
-    POLY_RANK = 3
+    ind = get_nn_idx(state, path)
 
-    #given vehicle pos find lookahead waypoints
+    xref[0, 0] = path[0,ind] #X
-    nn_idx=get_nn_idx(state,track)
+    xref[1, 0] = path[1,ind] #Y
-    LOOKAHED = POLY_RANK*2
+    xref[2, 0] = target_v    #sp[ind] #V
-    lk_wp=track[:,max(0,nn_idx-1):nn_idx+LOOKAHED]
+    xref[3, 0] = path[2,ind] #Theta
+    dref[0, 0] = 0.0         # steer operational point should be 0
+    
+    dl = P.path_tick # Waypoints spacing [m]
+    travel = 0.0
 
-    #trasform lookahead waypoints to vehicle ref frame
+    for i in range(P.T + 1):
-    dx = lk_wp[0,:] - state[0]
+        travel += abs(target_v) * P.dt #current V or target V?
-    dy = lk_wp[1,:] - state[1]
+        dind = int(round(travel / dl))
 
-    wp_vehicle_frame = np.vstack(( dx * np.cos(-state[3]) - dy * np.sin(-state[3]),
+        if (ind + dind) < ncourse:
-                                   dy * np.cos(-state[3]) + dx * np.sin(-state[3]) ))
+            xref[0, i] = path[0,ind + dind]
+            xref[1, i] = path[1,ind + dind]
+            xref[2, i] = target_v #sp[ind + dind]
+            xref[3, i] = path[2,ind + dind]
+            dref[0, i] = 0.0
+        else:
+            xref[0, i] = path[0,ncourse - 1]
+            xref[1, i] = path[1,ncourse - 1]
+            xref[2, i] = 0.0 #stop? #sp[ncourse - 1]
+            xref[3, i] = path[2,ncourse - 1]
+            dref[0, i] = 0.0
 
-    #fit poly
+    return xref, dref
-    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 f(x,coeff):
-    y=0
-    j=len(coeff)
-    for k in range(j):
-        y += coeff[k]*x**(j-k-1)
-    return round(y,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)
-
-def df(x,coeff):
-    y=0
-    j=len(coeff)
-    for k in range(j-1):
-        y += (j-k-1)*coeff[k]*x**(j-k-2)
-    return round(y,6)