diff --git a/mpc_pybullet_demo/cvxpy_mpc.py b/mpc_pybullet_demo/cvxpy_mpc.py index a84019a..531adfb 100755 --- a/mpc_pybullet_demo/cvxpy_mpc.py +++ b/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_STEER = np.pi/4 - MAX_ACC = 1.0 + MAX_SPEED = 1.5 #m/s + MAX_ACC = 1.0 #m/ss + 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 - 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] + REF_VEL = ref_vel #m/s + + # dynamics starting state + x_bar = np.zeros((P.N,P.T+1)) + x_bar[:,0] = state #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 - cost += 40*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte - cost += 20*cp.sum_squares(ref_vel-x[2,t]) # desired v + # Tracking Error + 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): - cost += cp.quad_form(u[:, t + 1] - u[:, t], 10*np.eye(P.M)) - - # Actuation effort - cost += cp.quad_form( 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 + constr += [cp.abs(u[1, t + 1] - u[1, t])/P.dt <= MAX_D_STEER] #max steer rate of change # 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[2, :] <= MAX_SPEED] - constr += [x[2, :] >= 0.0] - constr += [cp.abs(u[0, :]) <= MAX_ACC] - constr += [cp.abs(u[1, :]) <= MAX_STEER] + constr += [x[:,0] == x_bar[:,0]] #starting condition + constr += [x[2,:] <= MAX_SPEED] #max speed + constr += [x[2,:] >= 0.0] #min_speed (not really needed) + constr += [cp.abs(u[0,:]) <= MAX_ACC] #max acc + constr += [cp.abs(u[1,:]) <= MAX_STEER] #max steer # Solve prob = cp.Problem(cp.Minimize(cost), constr) diff --git a/mpc_pybullet_demo/mpc_config.py b/mpc_pybullet_demo/mpc_config.py index a97a79c..d4cd526 100644 --- a/mpc_pybullet_demo/mpc_config.py +++ b/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 diff --git a/mpc_pybullet_demo/mpc_demo_nosim.py b/mpc_pybullet_demo/mpc_demo_nosim.py index 0b00533..f850358 100755 --- a/mpc_pybullet_demo/mpc_demo_nosim.py +++ b/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 diff --git a/mpc_pybullet_demo/mpc_demo_pybullet.py b/mpc_pybullet_demo/mpc_demo_pybullet.py index e54ae5f..c559c67 100644 --- a/mpc_pybullet_demo/mpc_demo_pybullet.py +++ b/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]) diff --git a/mpc_pybullet_demo/utils.py b/mpc_pybullet_demo/utils.py index 330b822..899f7aa 100755 --- a/mpc_pybullet_demo/utils.py +++ b/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))) - interp_range = np.linspace(0,1, int(1+section_len/delta)) + 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)) + + 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 - nn_idx=get_nn_idx(state,track) - LOOKAHED = POLY_RANK*2 - lk_wp=track[:,max(0,nn_idx-1):nn_idx+LOOKAHED] + xref[0, 0] = path[0,ind] #X + xref[1, 0] = path[1,ind] #Y + xref[2, 0] = target_v #sp[ind] #V + 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 - dx = lk_wp[0,:] - state[0] - dy = lk_wp[1,:] - state[1] + for i in range(P.T + 1): + travel += abs(target_v) * P.dt #current V or target V? + dind = int(round(travel / dl)) - wp_vehicle_frame = np.vstack(( dx * np.cos(-state[3]) - dy * np.sin(-state[3]), - dy * np.cos(-state[3]) + dx * np.sin(-state[3]) )) + if (ind + dind) < ncourse: + 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 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) + return xref, dref \ No newline at end of file