moved pybullet demo to latest MPC version, now runs ~10Hz

mpc_pybullet_demo/cvxpy_mpc.py

@@ -3,17 +3,17 @@ np.seterr(divide='ignore', invalid='ignore')
 
 from scipy.integrate import odeint
 from scipy.interpolate import interp1d
-import cvxpy as cp
+import cvxpy as opt
 
 from utils import *
 
 from mpc_config import Params
 P=Params()
 
-def get_linear_model(x_bar,u_bar):
+def get_linear_model_matrices(x_bar,u_bar):
     """
+    Computes the LTI approximated state space model x' = Ax + Bu + C
     """
-    L=0.3
     
     x = x_bar[0]
     y = x_bar[1]
@@ -23,104 +23,95 @@ def get_linear_model(x_bar,u_bar):
     a = u_bar[0]
     delta = u_bar[1]
     
+    ct = np.cos(theta)
+    st = np.sin(theta)
+    cd = np.cos(delta)
+    td = np.tan(delta)
+    
     A = np.zeros((P.N,P.N))
-    A[0,2]=np.cos(theta)
+    A[0,2] = ct
-    A[0,3]=-v*np.sin(theta)
+    A[0,3] = -v*st
-    A[1,2]=np.sin(theta)
+    A[1,2] = st
-    A[1,3]=v*np.cos(theta)
+    A[1,3] = v*ct
-    A[3,2]=v*np.tan(delta)/L
+    A[3,2] = v*td/P.L
     A_lin = np.eye(P.N)+P.dt*A
     
     B = np.zeros((P.N,P.M))
     B[2,0]=1
-    B[3,1]=v/(L*np.cos(delta)**2)
+    B[3,1]=v/(P.L*cd**2)
     B_lin=P.dt*B
     
-    f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).reshape(P.N,1)
+    f_xu=np.array([v*ct, v*st, a, v*td/P.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)))
+    C_lin = P.dt*(f_xu - np.dot(A, x_bar.reshape(P.N,1)) - np.dot(B, u_bar.reshape(P.M,1))).flatten()
     
-    return np.round(A_lin,4), np.round(B_lin,4), np.round(C_lin,4)
+    #return np.round(A_lin,6), np.round(B_lin,6), np.round(C_lin,6)
+    return A_lin, B_lin, C_lin
 
+class MPC():
     
-def optimize(state,u_bar,track,ref_vel=1.):
+    def __init__(self, N, M, Q, R):
-    '''
+        """
-    :param state:
+        """
-    :param u_bar:
+        self.state_len = N
-    :param track:
+        self.action_len = M
-    :returns:
+        self.state_cost = Q
-    '''
+        self.action_cost = R
         
-    MAX_SPEED = 1.5 #m/s
+    def optimize_linearized_model(self, A, B, C, initial_state, target, time_horizon=10, Q=None, R=None, verbose=False):
-    MAX_ACC = 1.0 #m/ss
+        """
-    MAX_D_ACC = 1.0 #m/sss
+        Optimisation problem defined for the linearised model,
-    MAX_STEER = np.radians(30) #rad
+        :param A: 
-    MAX_D_STEER = np.radians(30) #rad/s
+        :param B:
+        :param C: 
+        :param initial_state:
+        :param Q:
+        :param R:
+        :param target:
+        :param time_horizon:
+        :param verbose:
+        :return:
+        """
         
-    REF_VEL = ref_vel #m/s
+        assert len(initial_state) == self.state_len
         
-    # dynamics starting state
+        if (Q == None or R==None):
-    x_bar = np.zeros((P.N,P.T+1))
+            Q = self.state_cost
-    x_bar[:,0] = state
+            R = self.action_cost
         
-    #prediction for linearization of costrains
+        # Create variables
-    for t in range (1,P.T+1):
+        x = opt.Variable((self.state_len, time_horizon + 1), name='states')
-        xt=x_bar[:,t-1].reshape(P.N,1)
+        u = opt.Variable((self.action_len, time_horizon), name='actions')
-        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
+        # Loop through the entire time_horizon and append costs
-    cost = 0
+        cost_function = []
-    constr = []
-    x = cp.Variable((P.N, P.T+1))
-    u = cp.Variable((P.M, P.T))
 
-    # Cost Matrices
+        for t in range(time_horizon):
-    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
+            _cost = opt.quad_form(target[:, t + 1] - x[:, t + 1], Q) +\
-    x_ref, d_ref = get_ref_trajectory(x_bar[:,0] ,track, REF_VEL)
+                    opt.quad_form(u[:, t], R)
             
-    #Prediction Horizon
+            _constraints = [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C,
-    for t in range(P.T):
+                            u[0, t] >= -P.MAX_ACC, u[0, t] <= P.MAX_ACC,
-
+                            u[1, t] >= -P.MAX_STEER, u[1, t] <= P.MAX_STEER]
-        # Tracking Error
+                            #opt.norm(target[:, t + 1] - x[:, t + 1], 1) <= 0.1]
-        cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)
-
-        # Actuation effort
-        cost += cp.quad_form(u[:,t], R)
             
             # Actuation rate of change
-        if t < (P.T - 1):
+            if t < (time_horizon - 1):
-            cost += cp.quad_form(u[:,t+1] - u[:,t], R_)
+                _cost += opt.quad_form(u[:,t + 1] - u[:,t], R * 1)
-            constr+= [cp.abs(u[0, t + 1] - u[0, t])/P.dt <= MAX_D_ACC]    #max acc rate of change
+                _constraints += [opt.abs(u[0, t + 1] - u[0, t])/P.dt <= P.MAX_D_ACC]
-            constr += [cp.abs(u[1, t + 1] - u[1, t])/P.dt <= MAX_D_STEER] #max steer rate of change
+                _constraints += [opt.abs(u[1, t + 1] - u[1, t])/P.dt <= P.MAX_D_STEER]
             
-        # 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.
+            if t == 0:
-    constr += [x[:,0] == x_bar[:,0]]           #starting condition
+                #_constraints += [opt.norm(target[:, time_horizon] - x[:, time_horizon], 1) <= 0.01,
-    constr += [x[2,:] <= MAX_SPEED]           #max speed
+                #                x[:, 0] == initial_state]
-    constr += [x[2,:] >= 0.0]                 #min_speed (not really needed)
+                _constraints += [x[:, 0] == initial_state]
-    constr += [cp.abs(u[0,:]) <= MAX_ACC]     #max acc
-    constr += [cp.abs(u[1,:]) <= MAX_STEER]   #max steer
             
-    # Solve
+            cost_function.append(opt.Problem(opt.Minimize(_cost), constraints=_constraints))
-    prob = cp.Problem(cp.Minimize(cost), constr)
-    prob.solve(solver=cp.OSQP, verbose=False)
 
-    if "optimal" not in prob.status:
+        # Add final cost
-        print("WARN: No optimal solution")
+        problem = sum(cost_function)
-        return u_bar
         
-    #retrieved optimized U and assign to u_bar to linearize in next step
+        # Minimize Problem
-    u_opt=np.vstack((np.array(u.value[0, :]).flatten(),
+        problem.solve(verbose=verbose, solver=opt.OSQP)
-                    (np.array(u.value[1, :]).flatten())))
+        return x, u
-
-    return u_opt

mpc_pybullet_demo/mpc_config.py

@@ -1,7 +1,15 @@
+import numpy as np
+
 class Params():
     def __init__(self):
         self.N = 4 #number of state variables
         self.M = 2 #number of control variables
         self.T = 10 #Prediction Horizon
-        self.dt = 0.25 #discretization step
+        self.dt = 0.2 #discretization step
         self.path_tick = 0.05
+        self.L = 0.3 #vehicle wheelbase
+        self.MAX_SPEED = 1.5 #m/s
+        self.MAX_ACC = 1.0 #m/ss
+        self.MAX_D_ACC = 1.0 #m/sss
+        self.MAX_STEER = np.radians(30) #rad
+        self.MAX_D_STEER = np.radians(30) #rad/s

mpc_pybullet_demo/mpc_demo_pybullet.py

@@ -3,7 +3,7 @@ import matplotlib.pyplot as plt
 from matplotlib import animation
 
 from utils import compute_path_from_wp
-from cvxpy_mpc import optimize
+from cvxpy_mpc import *
 
 from mpc_config import Params
 P=Params()
@@ -20,7 +20,10 @@ def get_state(robotId):
     robPos, robOrn = p.getBasePositionAndOrientation(robotId)
     linVel,angVel = p.getBaseVelocity(robotId)
 
-    return[robPos[0], robPos[1], np.sqrt(linVel[0]**2 + linVel[1]**2), p.getEulerFromQuaternion(robOrn)[2]]
+    return np.array([robPos[0],
+        robPos[1],
+        np.sqrt(linVel[0]**2 + linVel[1]**2),
+        p.getEulerFromQuaternion(robOrn)[2]])
 
 def set_ctrl(robotId,currVel,acceleration,steeringAngle):
 
@@ -34,10 +37,17 @@ def set_ctrl(robotId,currVel,acceleration,steeringAngle):
     #print(targetVelocity)
 
     for wheel in wheels:
-    	p.setJointMotorControl2(robotId,wheel,p.VELOCITY_CONTROL,targetVelocity=targetVelocity/gearRatio,force=maxForce)
+    	p.setJointMotorControl2(robotId,
+                wheel,
+                p.VELOCITY_CONTROL,
+                targetVelocity=targetVelocity/gearRatio,
+                force=maxForce)
 
     for steer in steering:
-    	p.setJointMotorControl2(robotId,steer,p.POSITION_CONTROL,targetPosition=steeringAngle)
+    	p.setJointMotorControl2(robotId,
+                steer,
+                p.POSITION_CONTROL,
+                targetPosition=steeringAngle)
 
 def plot_results(path,x_history,y_history):
     """
@@ -56,7 +66,10 @@ def run_sim():
     """
     """
     p.connect(p.GUI)
-    p.resetDebugVisualizerCamera(cameraDistance=1.0, cameraYaw=-90, cameraPitch=-45, cameraTargetPosition=[-0.1,-0.0,0.65])
+    p.resetDebugVisualizerCamera(cameraDistance=1.0,
+            cameraYaw=-90,
+            cameraPitch=-45,
+            cameraTargetPosition=[-0.1,-0.0,0.65])
 
     p.resetSimulation()
 
@@ -71,7 +84,7 @@ def run_sim():
 
     car = p.loadURDF("racecar/f10_racecar/racecar_differential.urdf", [0,0.3,.3])
     for wheel in range(p.getNumJoints(car)):
-    	print("joint[",wheel,"]=", p.getJointInfo(car,wheel))
+    	#print("joint[",wheel,"]=", p.getJointInfo(car,wheel))
     	p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
     	p.getJointInfo(car,wheel)
 
@@ -99,10 +112,6 @@ def run_sim():
     c = p.createConstraint(car,3,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
     p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15,maxForce=10000)
 
-    opt_u =  np.zeros((P.M,P.T))
-    opt_u[0,:] = 1 #m/ss
-    opt_u[1,:] = np.radians(0) #rad/
-
     # 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],P.path_tick)
@@ -110,27 +119,34 @@ def run_sim():
     for x_,y_ in zip(path[0,:],path[1,:]):
         p.addUserDebugLine([x_,y_,0],[x_,y_,0.33],[0,0,1])
     
+    #starting guess
+    action = np.zeros(P.M)
+    action[0] = P.MAX_ACC/2 #a
+    action[1] = 0.0      #delta
+    
+    # Cost Matrices
+    Q = np.diag([20,20,10,20]) #state error cost
+    Qf = np.diag([30,30,30,30]) #state final error cost
+    R = np.diag([10,10])       #input cost
+    R_ = np.diag([10,10])      #input rate of change cost
+    
+    mpc = MPC(P.N,P.M,Q,R)
     x_history=[]
     y_history=[]
 
     time.sleep(0.5)
     input("Press Enter to continue...")
+    
     while (1):
 
         state = get_state(car)
         x_history.append(state[0])
         y_history.append(state[1])
 
-        #add 1 timestep delay to input
-        state[0]=state[0]+state[2]*np.cos(state[3])*P.dt
-        state[1]=state[1]+state[2]*np.sin(state[3])*P.dt
-        state[2]=state[2]+opt_u[0,0]*P.dt
-        state[3]=state[3]+opt_u[0,0]*np.tan(opt_u[1,0])/0.3*P.dt
-
         #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)<0.1:
+        if np.sqrt((state[0]-path[0,-1])**2+(state[1]-path[1,-1])**2)<0.2:
             print("Success! Goal Reached")
             set_ctrl(car,0,0,0)
             plot_results(path,x_history,y_history)
@@ -138,13 +154,38 @@ def run_sim():
             p.disconnect()
             return
 
+        #for MPC car ref frame is used
+        state[0:2] = 0.0
+        state[3] = 0.0
+
+        #add 1 timestep delay to input
+        state[0]=state[0]+state[2]*np.cos(state[3])*P.dt
+        state[1]=state[1]+state[2]*np.sin(state[3])*P.dt
+        state[2]=state[2]+action[0]*P.dt
+        state[3]=state[3]+action[0]*np.tan(action[1])/P.L*P.dt
+
+       
     	#optimization loop
         start=time.time()
-        opt_u = optimize(state,opt_u,path,ref_vel=1.0)
+        
+        # State Matrices
+        A,B,C = get_linear_model_matrices(state, action)
+    
+        #Get Reference_traj -> inputs are in worldframe
+        target, _ = get_ref_trajectory(get_state(car),
+                path, 1.0)
+    
+        x_mpc, u_mpc = mpc.optimize_linearized_model(A, B, C, state, target, time_horizon=P.T, verbose=False)
+    
+        #action = np.vstack((np.array(u_mpc.value[0,:]).flatten(),
+        #              (np.array(u_mpc.value[1,:]).flatten())))
+        
+        action[:] = [u_mpc.value[0,1],u_mpc.value[1,1]]
+        
         elapsed=time.time()-start
         print("CVXPY Optimization Time: {:.4f}s".format(elapsed))
 
-        set_ctrl(car,state[2],opt_u[0,1],opt_u[1,1])
+        set_ctrl(car,state[2],action[0],action[1])
 
         if P.dt-elapsed>0:
             time.sleep(P.dt-elapsed)

mpc_pybullet_demo/utils.py

@@ -21,8 +21,9 @@ def compute_path_from_wp(start_xp, start_yp, step = 0.1):
         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))
+        # watch out to duplicate points!
-        final_yp=np.append(final_yp,fy(interp_range))
+        final_xp=np.append(final_xp,fx(interp_range)[1:])
+        final_yp=np.append(final_yp,fy(interp_range)[1:])
     
     dx = np.append(0, np.diff(final_xp))
     dy = np.append(0, np.diff(final_yp))
@@ -59,8 +60,22 @@ def get_nn_idx(state,path):
 
     return target_idx
 
+def normalize_angle(angle):
+    """
+    Normalize an angle to [-pi, pi]
+    """
+    while angle > np.pi:
+        angle -= 2.0 * np.pi
+
+    while angle < -np.pi:
+        angle += 2.0 * np.pi
+
+    return angle
+
 def get_ref_trajectory(state, path, target_v):
     """
+    For each step in the time horizon
+    modified reference in robot frame
     """
     xref = np.zeros((P.N, P.T+1))
     dref = np.zeros((1, P.T+1))
@@ -70,31 +85,40 @@ def get_ref_trajectory(state, path, target_v):
     ncourse = path.shape[1]
 
     ind = get_nn_idx(state, path)
+    dx=path[0,ind] - state[0]
+    dy=path[1,ind] - state[1]
     
-    xref[0, 0] = path[0,ind] #X
+    
-    xref[1, 0] = path[1,ind] #Y
+    xref[0, 0] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) #X
-    xref[2, 0] = target_v    #sp[ind] #V
+    xref[1, 0] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) #Y
-    xref[3, 0] = path[2,ind] #Theta
+    xref[2, 0] = target_v                                        #V
+    xref[3, 0] = normalize_angle(path[2,ind]- state[3])          #Theta
     dref[0, 0] = 0.0                                             # steer operational point should be 0
     
-    dl = P.path_tick # Waypoints spacing [m]
+    dl = 0.05 # Waypoints spacing [m]
     travel = 0.0
     
-    for i in range(P.T + 1):
+    for i in range(1, P.T+1):
         travel += abs(target_v) * P.dt #current V or target V?
         dind = int(round(travel / dl))
         
         if (ind + dind) < ncourse:
-            xref[0, i] = path[0,ind + dind]
+            dx=path[0,ind + dind] - state[0]
-            xref[1, i] = path[1,ind + dind]
+            dy=path[1,ind + dind] - state[1]
+            
+            xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])
+            xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])
             xref[2, i] = target_v #sp[ind + dind]
-            xref[3, i] = path[2,ind + dind]
+            xref[3, i] = normalize_angle(path[2,ind + dind] - state[3])
             dref[0, i] = 0.0
         else:
-            xref[0, i] = path[0,ncourse - 1]
+            dx=path[0,ncourse - 1] - state[0]
-            xref[1, i] = path[1,ncourse - 1]
+            dy=path[1,ncourse - 1] - state[1]
+            
+            xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])
+            xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])
             xref[2, i] = 0.0 #stop? #sp[ncourse - 1]
-            xref[3, i] = path[2,ncourse - 1]
+            xref[3, i] = normalize_angle(path[2,ncourse - 1] - state[3])
             dref[0, i] = 0.0
 
     return xref, dref